diff --git a/feeding_web_app_ros2_test/data/above_plate_5_depth.png b/feeding_web_app_ros2_test/data/above_plate_5_depth.png new file mode 100644 index 00000000..1f4d8b31 Binary files /dev/null and b/feeding_web_app_ros2_test/data/above_plate_5_depth.png differ diff --git a/feeding_web_app_ros2_test/data/above_plate_5_rgb.jpg b/feeding_web_app_ros2_test/data/above_plate_5_rgb.jpg new file mode 100644 index 00000000..592701a4 Binary files /dev/null and b/feeding_web_app_ros2_test/data/above_plate_5_rgb.jpg differ diff --git a/feedingwebapp/package.json b/feedingwebapp/package.json index 40abaa86..5c2cb4b3 100644 --- a/feedingwebapp/package.json +++ b/feedingwebapp/package.json @@ -47,7 +47,7 @@ "test": "react-scripts test", "eject": "react-scripts eject", "lint:fix": "eslint \"{,!(node_modules)/**/}*.{js,jsx}\" --fix", - "prettier:fix": "prettier \"{,!(node_modules)/**/}*.{js,jsx,css}\" --write --config ./.prettierrc", + "prettier:fix": "prettier \"{,!(node_modules)/**/}*.{js,jsx,css}\" --write --config ./.prettierrc --ignore-path ./.gitignore", "format": "npm run prettier:fix && npm run lint:fix" }, "eslintConfig": { diff --git a/feedingwebapp/src/Pages/Constants.js b/feedingwebapp/src/Pages/Constants.js index 79f6fd7b..e4c56cde 100644 --- a/feedingwebapp/src/Pages/Constants.js +++ b/feedingwebapp/src/Pages/Constants.js @@ -118,12 +118,12 @@ export const CLEAR_OCTOMAP_SERVICE_NAME = 'clear_octomap' export const CLEAR_OCTOMAP_SERVICE_TYPE = 'std_srvs/srv/Empty' export const ACQUISITION_REPORT_SERVICE_NAME = 'ada_feeding_action_select/action_report' export const ACQUISITION_REPORT_SERVICE_TYPE = 'ada_feeding_msgs/srv/AcquisitionReport' -export const GET_JOINT_STATE_SERVICE_NAME = 'get_joint_state' -export const GET_JOINT_STATE_SERVICE_TYPE = 'ada_feeding_msgs/srv/GetJointState' +export const GET_ROBOT_STATE_SERVICE_NAME = 'get_robot_state' +export const GET_ROBOT_STATE_SERVICE_TYPE = 'ada_feeding_msgs/srv/GetRobotState' export const GET_PARAMETERS_SERVICE_NAME = 'ada_feeding_action_servers/get_parameters' export const GET_PARAMETERS_SERVICE_TYPE = 'rcl_interfaces/srv/GetParameters' -export const SET_PARAMETERS_SERVICE_NAME = 'ada_feeding_action_servers/set_parameters' -export const SET_PARAMETERS_SERVICE_TYPE = 'rcl_interfaces/srv/SetParameters' +export const SET_PARAMETERS_SERVICE_NAME = 'ada_feeding_action_servers/set_parameters_atomically' +export const SET_PARAMETERS_SERVICE_TYPE = 'rcl_interfaces/srv/SetParametersAtomically' // The names of parameters users can change in the settings menu export const DISTANCE_TO_MOUTH_PARAM = 'MoveToMouth.tree_kwargs.plan_distance_from_mouth' @@ -139,6 +139,7 @@ export const RESTING_PARAM_JOINTS_2 = 'MoveToRestingPosition.tree_kwargs.goal_co // Robot link names export const ROBOT_BASE_LINK = 'j2n6s200_link_base' +export const ROBOT_END_EFFECTOR = 'forkTip' export const ROBOT_JOINTS = [ 'j2n6s200_joint_1', 'j2n6s200_joint_2', diff --git a/feedingwebapp/src/Pages/GlobalState.jsx b/feedingwebapp/src/Pages/GlobalState.jsx index 3958a126..53baf507 100644 --- a/feedingwebapp/src/Pages/GlobalState.jsx +++ b/feedingwebapp/src/Pages/GlobalState.jsx @@ -83,7 +83,8 @@ export const SETTINGS_STATE = { MAIN: 'MAIN', BITE_TRANSFER: 'BITE_TRANSFER', ABOVE_PLATE: 'ABOVE_PLATE', - RESTING_CONFIGURATION: 'RESTING_CONFIGURATION' + RESTING_CONFIGURATION: 'RESTING_CONFIGURATION', + STAGING_CONFIGURATION: 'STAGING_CONFIGURATION' } // The name of the default parameter namespace @@ -140,11 +141,11 @@ export const useGlobalState = create( biteAcquisitionCheckAutoContinueSecs: 3.0, biteAcquisitionCheckAutoContinueProbThreshLower: 0.25, biteAcquisitionCheckAutoContinueProbThreshUpper: 0.75, - // Whether the settings bite transfer page is currently at the user's face + // Whether any of the settings pages is currently at the user's mouth // or not. This is in the off-chance that the mealState is not at the user's - // face, the settings page is, and the user refreshes -- the page should - // call MoveFromMouthToStaging instead of just MoveToStaging. - biteTransferPageAtFace: false, + // mouth, the settings page is, and the user refreshes -- the page should + // call MoveFromMouth instead of just MoveToStaging. + settingsPageAtMouth: false, // The button the user most recently clicked on the BiteDone page. In practice, // this is the state we transition to after R_MovingFromMouth. In practice, // it is either R_MovingAbovePlate, R_MovingToRestingPosition, or R_DetectingFace. @@ -168,7 +169,7 @@ export const useGlobalState = create( let retval = { mealState: mealState, mealStateTransitionTime: Date.now(), - biteTransferPageAtFace: false // Reset this flag when the meal state changes + settingsPageAtMouth: false // Reset this flag when the meal state changes } // Only update the previous state if it is not a self-transition (to // account for cases where a MoveTo action result message is reveived twice) @@ -211,9 +212,12 @@ export const useGlobalState = create( lastMotionActionResponse: lastMotionActionResponse })), setMoveToMouthActionGoal: (moveToMouthActionGoal) => - set(() => ({ - moveToMouthActionGoal: moveToMouthActionGoal - })), + set(() => { + console.log('setMoveToMouthActionGoal called with', moveToMouthActionGoal) + return { + moveToMouthActionGoal: moveToMouthActionGoal + } + }), setPaused: (paused) => set(() => { let retval = { paused: paused } @@ -276,9 +280,9 @@ export const useGlobalState = create( set(() => ({ biteAcquisitionCheckAutoContinueProbThreshUpper: biteAcquisitionCheckAutoContinueProbThreshUpper })), - setBiteTransferPageAtFace: (biteTransferPageAtFace) => + setSettingsPageAtMouth: (settingsPageAtMouth) => set(() => ({ - biteTransferPageAtFace: biteTransferPageAtFace + settingsPageAtMouth: settingsPageAtMouth })), setBiteSelectionZoom: (biteSelectionZoom) => set(() => ({ diff --git a/feedingwebapp/src/Pages/Home/MealStates/BiteSelection.jsx b/feedingwebapp/src/Pages/Home/MealStates/BiteSelection.jsx index 28ee027f..c9514305 100644 --- a/feedingwebapp/src/Pages/Home/MealStates/BiteSelection.jsx +++ b/feedingwebapp/src/Pages/Home/MealStates/BiteSelection.jsx @@ -243,7 +243,7 @@ const BiteSelection = (props) => { // Create a service request let request = createROSServiceRequest({ data: false }) // Call the service - service.callService(request, (response) => console.log('Got toggle face detection service response', response)) + service.callService(request, (response) => console.log('Got toggle table detection service response', response)) // Destroy the action client destroyActionClient(action) } diff --git a/feedingwebapp/src/Pages/Home/MealStates/DetectingFace.jsx b/feedingwebapp/src/Pages/Home/MealStates/DetectingFace.jsx index 2ee3d905..01267df3 100644 --- a/feedingwebapp/src/Pages/Home/MealStates/DetectingFace.jsx +++ b/feedingwebapp/src/Pages/Home/MealStates/DetectingFace.jsx @@ -26,7 +26,6 @@ const DetectingFace = (props) => { const prevMealState = useGlobalState((state) => state.prevMealState) const setInNonMovingState = useGlobalState((state) => state.setInNonMovingState) const setMealState = useGlobalState((state) => state.setMealState) - const setMoveToMouthActionGoal = useGlobalState((state) => state.setMoveToMouthActionGoal) const faceDetectionAutoContinue = useGlobalState((state) => state.faceDetectionAutoContinue) const setFaceDetectionAutoContinue = useGlobalState((state) => state.setFaceDetectionAutoContinue) // Get icon image for move to mouth @@ -105,20 +104,13 @@ const DetectingFace = (props) => { /** * Callback for when a face is detected within the correct range. */ - const faceDetectedCallback = useCallback( - (message) => { - console.log('Face detected callback') - setMouthDetected(true) - setMoveToMouthActionGoal({ - face_detection: message - }) - // If auto-continue is enabled, move to the mouth position - if (autoContinueIsEnabled()) { - moveToMouthCallback() - } - }, - [autoContinueIsEnabled, moveToMouthCallback, setMoveToMouthActionGoal] - ) + const faceDetectedCallback = useCallback(() => { + setMouthDetected(true) + // If auto-continue is enabled, move to the mouth position + if (autoContinueIsEnabled()) { + moveToMouthCallback() + } + }, [autoContinueIsEnabled, moveToMouthCallback]) /** Get the full page view * diff --git a/feedingwebapp/src/Pages/Home/MealStates/DetectingFaceSubcomponent.jsx b/feedingwebapp/src/Pages/Home/MealStates/DetectingFaceSubcomponent.jsx index fe832277..8af6052b 100644 --- a/feedingwebapp/src/Pages/Home/MealStates/DetectingFaceSubcomponent.jsx +++ b/feedingwebapp/src/Pages/Home/MealStates/DetectingFaceSubcomponent.jsx @@ -5,11 +5,11 @@ import { useMediaQuery } from 'react-responsive' import { View } from 'react-native' // Local Imports -import { useROS, createROSService, createROSServiceRequest, subscribeToROSTopic, unsubscribeFromROSTopic } from '../../../ros/ros_helpers' +import { useROS, subscribeToROSTopic, unsubscribeFromROSTopic } from '../../../ros/ros_helpers' import '../Home.css' -import { MEAL_STATE } from '../../GlobalState' -import { FACE_DETECTION_IMG_TOPIC, FACE_DETECTION_TOPIC, FACE_DETECTION_TOPIC_MSG, ROS_SERVICE_NAMES } from '../../Constants' +import { FACE_DETECTION_IMG_TOPIC, FACE_DETECTION_TOPIC, FACE_DETECTION_TOPIC_MSG } from '../../Constants' import VideoFeed from '../VideoFeed' +import { useGlobalState } from '../../GlobalState' /** * The DetectingFace component appears after the robot has moved to the staging @@ -17,6 +17,9 @@ import VideoFeed from '../VideoFeed' * moves on to `R_MovingToMouth` when a face is detected. */ const DetectingFaceSubcomponent = (props) => { + // Get the relevant global variables + const setMoveToMouthActionGoal = useGlobalState((state) => state.setMoveToMouthActionGoal) + // Keep track of whether a mouth has been detected or not const [mouthDetected, setMouthDetected] = useState(false) // Flag to check if the current orientation is portrait @@ -51,11 +54,14 @@ const DetectingFaceSubcomponent = (props) => { 0.5 if (distance > min_face_distance && distance < max_face_distance) { setMouthDetected(true) + setMoveToMouthActionGoal({ + face_detection: message + }) faceDetectedCallback() } } }, - [props.faceDetectedCallback, setMouthDetected] + [props.faceDetectedCallback, setMouthDetected, setMoveToMouthActionGoal] ) useEffect(() => { let topic = subscribeToROSTopic(ros.current, FACE_DETECTION_TOPIC, FACE_DETECTION_TOPIC_MSG, faceDetectionCallback) @@ -69,38 +75,6 @@ const DetectingFaceSubcomponent = (props) => { } }, [faceDetectionCallback]) - /** - * Create the ROS Service. This is created in local state to avoid re-creating - * it upon every re-render. - */ - let { serviceName, messageType } = ROS_SERVICE_NAMES[MEAL_STATE.R_DetectingFace] - let toggleFaceDetectionService = useRef(createROSService(ros.current, serviceName, messageType)) - - /** - * Toggles face detection on the first time this component is rendered, but - * not upon additional re-renders. See here for more details on how `useEffect` - * achieves this goal: https://stackoverflow.com/a/69264685 - */ - useEffect(() => { - // Create a service request - let request = createROSServiceRequest({ data: true }) - // Call the service - let service = toggleFaceDetectionService.current - service.callService(request, (response) => console.log('Got toggle face detection service response', response)) - - /** - * In practice, because the values passed in in the second argument of - * useEffect will not change on re-renders, this return statement will - * only be called when the component unmounts. - */ - return () => { - // Create a service request - let request = createROSServiceRequest({ data: false }) - // Call the service - service.callService(request, (response) => console.log('Got toggle face detection service response', response)) - } - }, [toggleFaceDetectionService]) - // Render the component return ( <> @@ -125,7 +99,7 @@ const DetectingFaceSubcomponent = (props) => { height: '100%' }} > - + ) diff --git a/feedingwebapp/src/Pages/Home/VideoFeed.jsx b/feedingwebapp/src/Pages/Home/VideoFeed.jsx index b7809ddf..1a87cba9 100644 --- a/feedingwebapp/src/Pages/Home/VideoFeed.jsx +++ b/feedingwebapp/src/Pages/Home/VideoFeed.jsx @@ -7,9 +7,11 @@ import PropTypes from 'prop-types' import { View } from 'react-native' // Local Imports -import { CAMERA_FEED_TOPIC, REALSENSE_WIDTH, REALSENSE_HEIGHT } from '../Constants' +import { CAMERA_FEED_TOPIC, REALSENSE_WIDTH, REALSENSE_HEIGHT, ROS_SERVICE_NAMES } from '../Constants' import { useWindowSize } from '../../helpers' import { WebRTCConnection } from '../../webrtc/webrtc_helpers' +import { createROSService, createROSServiceRequest, useROS } from '../../ros/ros_helpers' +import { MEAL_STATE } from '../GlobalState' /** * Takes in an imageWidth and imageHeight, and returns a width and height that @@ -94,12 +96,31 @@ const VideoFeed = (props) => { let textFontSize = isPortrait ? 2.5 : 3.0 let sizeSuffix = 'vh' + /** + * Connect to ROS, if not already connected. Put this in useRef to avoid + * re-connecting upon re-renders. + */ + const ros = useRef(useROS().ros) + + /** + * Create the ROS Service Clients to toggle face detection. + */ + let { serviceName, messageType } = ROS_SERVICE_NAMES[MEAL_STATE.R_DetectingFace] + let toggleFaceDetectionService = useRef(createROSService(ros.current, serviceName, messageType)) + /** * Create the peer connection */ useEffect(() => { + // Toggle on face detection if specified + let service = toggleFaceDetectionService.current + if (props.toggleFaceDetection) { + let request = createROSServiceRequest({ data: true }) + service.callService(request, (response) => console.log('VideoFeed got toggle face detection on service response', response)) + } + // Create the peer connection - console.log('Creating peer connection', props.webrtcURL, refreshCount) + console.log('Creating peer connection', props.webrtcURL, refreshCount, props.externalRefreshCount) const webRTCConnection = new WebRTCConnection({ url: props.webrtcURL + '/subscribe', topic: props.topic, @@ -115,56 +136,75 @@ const VideoFeed = (props) => { }) return () => { + // Close the peer connection webRTCConnection.close() + + // Toggle off face detection if specified + if (props.toggleFaceDetection) { + let request = createROSServiceRequest({ data: false }) + service.callService(request, (response) => console.log('VideoFeed got toggle face detection off service response', response)) + } } - }, [props.topic, props.webrtcURL, refreshCount, videoRef]) + }, [ + props.externalRefreshCount, + props.toggleFaceDetection, + props.topic, + props.webrtcURL, + refreshCount, + toggleFaceDetectionService, + videoRef + ]) // Callback to resize the image based on the parent width and height - const resizeImage = useCallback(() => { - console.log('Resizing image', parentRef.current) - if (!parentRef.current) { - return - } - // Get the width and height of the parent DOM element - let parentWidth = parentRef.current.clientWidth - let parentHeight = parentRef.current.clientHeight + const resizeImage = useCallback( + (delay_ms = 10) => { + if (!parentRef.current) { + return + } + // Get the width and height of the parent DOM element + let parentWidth = parentRef.current.clientWidth + let parentHeight = parentRef.current.clientHeight - // Calculate the width and height of the video feed - let { - width: childWidth, - height: childHeight, - scaleFactor: childScaleFactor - } = scaleWidthHeightToWindow( - parentWidth, - parentHeight, - REALSENSE_WIDTH, - REALSENSE_HEIGHT, - props.marginTop, - props.marginBottom, - props.marginLeft, - props.marginRight - ) + // Calculate the width and height of the video feed + let { + width: childWidth, + height: childHeight, + scaleFactor: childScaleFactor + } = scaleWidthHeightToWindow( + parentWidth, + parentHeight, + REALSENSE_WIDTH, + REALSENSE_HEIGHT, + props.marginTop, + props.marginBottom, + props.marginLeft, + props.marginRight + ) - // Set the width and height of the video feed - setImgWidth(childWidth * props.zoom) - setImgHeight(childHeight * props.zoom) - setScaleFactor(childScaleFactor * props.zoom) - }, [parentRef, props.marginTop, props.marginBottom, props.marginLeft, props.marginRight, props.zoom]) + // Set the width and height of the video feed + setImgWidth(childWidth * props.zoom) + setImgHeight(childHeight * props.zoom) + setScaleFactor(childScaleFactor * props.zoom) - /** When the resize event is triggered, the elements have not yet been laid out, - * and hence the parent width/height might not be accurate yet based on the - * specified flex layout. Hence, we wait until the next event cycle to resize - * the video feed. - */ - const resizeImageNextEventCycle = useCallback(() => { - setTimeout(resizeImage, 0) - }, [resizeImage]) - useWindowSize(resizeImageNextEventCycle) + // If the width or height is zero, schedule another resize event in the next + // event cycle. This is because initially the elements have not been laid out, + // and it might take a few event cycles to do so. + if (childWidth === 0.0 || childHeight === 0.0) { + setTimeout(resizeImage, delay_ms) + } + }, + [parentRef, props.marginTop, props.marginBottom, props.marginLeft, props.marginRight, props.zoom] + ) + + // Resize the element when the window is resized + useWindowSize(resizeImage) - // When the component is first mounted, resize the image + // When the component is first mounted and when the reload button is clicked, + // resize the image useEffect(() => { - resizeImageNextEventCycle() - }, [resizeImageNextEventCycle]) + console.log('Resizing image', refreshCount, props.externalRefreshCount) + resizeImage() + }, [props.externalRefreshCount, refreshCount, resizeImage]) // The callback for when the image is clicked. const imageClicked = useCallback( @@ -329,7 +369,7 @@ const VideoFeed = (props) => { fontSize: textFontSize.toString() + sizeSuffix, color: 'black' }} - onClick={() => setRefreshCount(refreshCount + 1)} + onClick={() => setRefreshCount((x) => x + 1)} > Reload Video @@ -344,8 +384,13 @@ VideoFeed.propTypes = { marginBottom: PropTypes.number, marginLeft: PropTypes.number, marginRight: PropTypes.number, + // A number that changes when some external entity wants this component to refresh. + externalRefreshCount: PropTypes.number, // The topic of the video feed topic: PropTypes.string.isRequired, + // Whether this component should toggle face detection on when it is mounted and + // the reload button is clicked, and toggle it off when it is unmounted + toggleFaceDetection: PropTypes.bool, /** * An optional callback function for when the user clicks on the video feed. * This function should take in two parameters, `x` and `y`, which are the @@ -368,7 +413,9 @@ VideoFeed.defaultProps = { marginBottom: 0, marginLeft: 0, marginRight: 0, + externalRefreshCount: 0, topic: CAMERA_FEED_TOPIC, + toggleFaceDetection: false, zoom: 1.0, zoomMin: 1.0, zoomMax: 2.0 diff --git a/feedingwebapp/src/Pages/Settings/BiteTransfer.jsx b/feedingwebapp/src/Pages/Settings/BiteTransfer.jsx index 861fdcbc..e0c3d249 100644 --- a/feedingwebapp/src/Pages/Settings/BiteTransfer.jsx +++ b/feedingwebapp/src/Pages/Settings/BiteTransfer.jsx @@ -22,19 +22,25 @@ const BiteTransfer = (props) => { const setSettingsState = useGlobalState((state) => state.setSettingsState) const globalMealState = useGlobalState((state) => state.mealState) const setPaused = useGlobalState((state) => state.setPaused) - const biteTransferPageAtFace = useGlobalState((state) => state.biteTransferPageAtFace) - const setBiteTransferPageAtFace = useGlobalState((state) => state.setBiteTransferPageAtFace) + const settingsPageAtMouth = useGlobalState((state) => state.settingsPageAtMouth) + const setSettingsPageAtMouth = useGlobalState((state) => state.setSettingsPageAtMouth) + const moveToMouthActionGoal = useGlobalState((state) => state.moveToMouthActionGoal) // Create relevant local state variables // Configure the parameters for SettingsPageParent const paramNames = useMemo(() => [DISTANCE_TO_MOUTH_PARAM], []) const [currentDistanceToMouth, setCurrentDistanceToMouth] = useState([null]) const [localCurrAndNextMealState, setLocalCurrAndNextMealState] = useState( - globalMealState === MEAL_STATE.U_BiteDone || globalMealState === MEAL_STATE.R_DetectingFace || biteTransferPageAtFace + globalMealState === MEAL_STATE.U_BiteDone || globalMealState === MEAL_STATE.R_DetectingFace || settingsPageAtMouth ? [MEAL_STATE.R_MovingFromMouth, null] : [MEAL_STATE.R_MovingToStagingConfiguration, null] ) - const actionInput = useMemo(() => ({}), []) + const actionInput = useMemo(() => { + if (localCurrAndNextMealState[0] === MEAL_STATE.R_MovingToMouth) { + return moveToMouthActionGoal + } + return {} + }, [localCurrAndNextMealState, moveToMouthActionGoal]) const doneButtonIsClicked = useRef(false) // Flag to check if the current orientation is portrait @@ -53,8 +59,8 @@ const BiteTransfer = (props) => { (newLocalCurrMealState, newLocalNextMealState = null) => { let oldLocalCurrMealState = localCurrAndNextMealState[0] // If the oldlocalCurrMealState was R_MovingToMouth, then the robot is at the mouth - setBiteTransferPageAtFace( - newLocalCurrMealState === null && (biteTransferPageAtFace || oldLocalCurrMealState === MEAL_STATE.R_MovingToMouth) + setSettingsPageAtMouth( + newLocalCurrMealState === null && (settingsPageAtMouth || oldLocalCurrMealState === MEAL_STATE.R_MovingToMouth) ) // Start in a moving state, not a paused state setPaused(false) @@ -69,10 +75,10 @@ const BiteTransfer = (props) => { } }, [ - biteTransferPageAtFace, + settingsPageAtMouth, localCurrAndNextMealState, setLocalCurrAndNextMealState, - setBiteTransferPageAtFace, + setSettingsPageAtMouth, doneButtonIsClicked, setPaused, setSettingsState @@ -97,10 +103,10 @@ const BiteTransfer = (props) => { useEffect(() => { doneButtonIsClicked.current = false // Since we start by moving to staging, this should be initialized to false - setBiteTransferPageAtFace(false) + setSettingsPageAtMouth(false) // Start in a moving state, not a paused state setPaused(false) - }, [setBiteTransferPageAtFace, setPaused, doneButtonIsClicked]) + }, [setSettingsPageAtMouth, setPaused, doneButtonIsClicked]) // Callback to move the robot to the mouth const moveToMouthButtonClicked = useCallback(() => { @@ -123,7 +129,7 @@ const BiteTransfer = (props) => { // To get to Settings, the globalMealState must be one of the NON_MOVING_STATES switch (globalMealState) { case MEAL_STATE.U_BiteDone: - if (biteTransferPageAtFace) { + if (settingsPageAtMouth) { localCurrMealState = null localNextMealState = null } else { @@ -148,7 +154,7 @@ const BiteTransfer = (props) => { break } setLocalCurrMealStateWrapper(localCurrMealState, localNextMealState) - }, [biteTransferPageAtFace, globalMealState, setLocalCurrMealStateWrapper, doneButtonIsClicked]) + }, [settingsPageAtMouth, globalMealState, setLocalCurrMealStateWrapper, doneButtonIsClicked]) // Callback for when the user changes the distance to mouth const onDistanceToMouthChange = useCallback( diff --git a/feedingwebapp/src/Pages/Settings/CustomizeConfiguration.jsx b/feedingwebapp/src/Pages/Settings/CustomizeConfiguration.jsx index 514e2567..34fe4415 100644 --- a/feedingwebapp/src/Pages/Settings/CustomizeConfiguration.jsx +++ b/feedingwebapp/src/Pages/Settings/CustomizeConfiguration.jsx @@ -10,8 +10,10 @@ import { useROS, createROSService, createROSServiceRequest } from '../../ros/ros import { CAMERA_FEED_TOPIC, getRobotMotionText, - GET_JOINT_STATE_SERVICE_NAME, - GET_JOINT_STATE_SERVICE_TYPE, + GET_ROBOT_STATE_SERVICE_NAME, + GET_ROBOT_STATE_SERVICE_TYPE, + ROBOT_BASE_LINK, + ROBOT_END_EFFECTOR, ROBOT_JOINTS } from '../Constants' import { useGlobalState, MEAL_STATE, SETTINGS_STATE } from '../GlobalState' @@ -21,6 +23,38 @@ import TeleopSubcomponent from '../Header/TeleopSubcomponent' import SettingsPageParent from './SettingsPageParent' import VideoFeed from '../Home/VideoFeed' +/** + * This function extracts the joint positions from the robot state service's response + * and returns it. + */ +export function getJointPositionsFromRobotStateResponse(response) { + return response.joint_state.position +} + +/** + * The function extracts the end effector position and quaternion in the robot's + * base link frame, from the robot state service's response. + */ +export function getEndEffectorPositionFromRobotStateResponse(response) { + if (response.poses.length === 0) { + return [] + } + let pose = response.poses[0].pose + return [pose.position.x, pose.position.y, pose.position.z] +} + +/** + * This function extracts the end effector orientation in the robot's base link frame, + * from the robot state service's response. + */ +export function getEndEffectorOrientationFromRobotStateResponse(response) { + if (response.poses.length === 0) { + return [] + } + let pose = response.poses[0].pose + return [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] +} + /** * The CustomizeConfiguration component allows users to configure the one of the * fixed configurations the robot uses. In its current form, the node can take in @@ -36,19 +70,27 @@ const CustomizeConfiguration = (props) => { const setSettingsState = useGlobalState((state) => state.setSettingsState) const globalMealState = useGlobalState((state) => state.mealState) const setPaused = useGlobalState((state) => state.setPaused) - const biteTransferPageAtFace = useGlobalState((state) => state.biteTransferPageAtFace) + const settingsPageAtMouth = useGlobalState((state) => state.settingsPageAtMouth) + const setSettingsPageAtMouth = useGlobalState((state) => state.setSettingsPageAtMouth) + const moveToMouthActionGoal = useGlobalState((state) => state.moveToMouthActionGoal) // Create relevant local state variables // Configure the parameters for SettingsPageParent const [currentConfigurationParams, setCurrentConfigurationParams] = useState(props.paramNames.map(() => null)) const [localCurrAndNextMealState, setLocalCurrAndNextMealState] = useState( - globalMealState === MEAL_STATE.U_BiteDone || biteTransferPageAtFace + globalMealState === MEAL_STATE.U_BiteDone || settingsPageAtMouth ? [MEAL_STATE.R_MovingFromMouth, props.startingMealState] : [props.startingMealState, null] ) - const actionInput = useMemo(() => ({}), []) + const actionInput = useMemo(() => { + if (localCurrAndNextMealState[0] === MEAL_STATE.R_MovingToMouth) { + return moveToMouthActionGoal + } + return {} + }, [localCurrAndNextMealState, moveToMouthActionGoal]) const doneButtonIsClicked = useRef(false) const [zoomLevel, setZoomLevel] = useState(1.0) + const [videoFeedRefreshCount, setVideoFeedrefreshCount] = useState(0) const [mountTeleopSubcomponent, setMountTeleopSubcomponent] = useState(false) const unmountTeleopSubcomponentCallback = useRef(() => {}) @@ -61,14 +103,38 @@ const CustomizeConfiguration = (props) => { let textFontSize = 3.5 let sizeSuffix = isPortrait ? 'vh' : 'vw' + /** + * Connect to ROS, if not already connected. Put this in useRef to avoid + * re-connecting upon re-renders. + */ + const ros = useRef(useROS().ros) + + /** + * Create the ROS Service Clients to get joint states + */ + let getRobotStateService = useRef(createROSService(ros.current, GET_ROBOT_STATE_SERVICE_NAME, GET_ROBOT_STATE_SERVICE_TYPE)) + // Update other state variables that are related to the local meal state const setLocalCurrMealStateWrapper = useCallback( (newLocalCurrMealState, newLocalNextMealState = null) => { - console.log('setLocalCurrMealStateWrapper evaluated') + console.log('setLocalCurrMealStateWrapper evaluated', newLocalCurrMealState, newLocalNextMealState) let oldLocalCurrMealState = localCurrAndNextMealState[0] - // Only mount the teleop subcomponent if the robot finished the prereq motion for this page - if (newLocalCurrMealState === null && oldLocalCurrMealState === props.startingMealState) { - setMountTeleopSubcomponent(true) + + if (newLocalCurrMealState === null) { + // If toggling face detection is enabled, refresh the video feed. This is + // because other elements/actions might have toggled face detection off. + if (props.toggleFaceDetection) { + setVideoFeedrefreshCount((x) => x + 1) + } + + // Only mount the teleop subcomponent if the robot finished the prereq motion for this page + // Treat MoveFromMouth and MoveToStaging as the same. + if ( + oldLocalCurrMealState === props.startingMealState || + (props.startingMealState === MEAL_STATE.R_MovingToStagingConfiguration && oldLocalCurrMealState === MEAL_STATE.R_MovingFromMouth) + ) { + setMountTeleopSubcomponent(true) + } } // Start in a moving state, not a paused state setPaused(false) @@ -80,15 +146,23 @@ const CustomizeConfiguration = (props) => { } else { setLocalCurrAndNextMealState([newLocalCurrMealState, newLocalNextMealState]) } + // If the oldlocalCurrMealState was R_MovingToMouth, then the robot is at the mouth + setSettingsPageAtMouth( + newLocalCurrMealState === null && (settingsPageAtMouth || oldLocalCurrMealState === MEAL_STATE.R_MovingToMouth) + ) }, [ doneButtonIsClicked, localCurrAndNextMealState, props.startingMealState, + props.toggleFaceDetection, setLocalCurrAndNextMealState, setMountTeleopSubcomponent, setPaused, - setSettingsState + settingsPageAtMouth, + setSettingsPageAtMouth, + setSettingsState, + setVideoFeedrefreshCount ] ) @@ -132,17 +206,6 @@ const CustomizeConfiguration = (props) => { } }, [localCurrAndNextMealState, setLocalCurrMealStateWrapper, actionInput]) - /** - * Connect to ROS, if not already connected. Put this in useRef to avoid - * re-connecting upon re-renders. - */ - const ros = useRef(useROS().ros) - - /** - * Create the ROS Service Clients to get/set parameters. - */ - let getJointStateService = useRef(createROSService(ros.current, GET_JOINT_STATE_SERVICE_NAME, GET_JOINT_STATE_SERVICE_TYPE)) - // Reset state the first time the page is rendered useEffect(() => { doneButtonIsClicked.current = false @@ -152,24 +215,43 @@ const CustomizeConfiguration = (props) => { // Get the current joint states and store them as the above plate param const storeJointStatesAsLocalParam = useCallback(() => { - console.log('storeJointStatesAsLocalParam called') - let service = getJointStateService.current - let request = createROSServiceRequest({ + let service = getRobotStateService.current + let request_object = { joint_names: ROBOT_JOINTS - }) + } + if (props.getEndEffectorPose) { + request_object.child_frames = [ROBOT_END_EFFECTOR] + request_object.parent_frames = [ROBOT_BASE_LINK] + } + let request = createROSServiceRequest(request_object) + console.log('storeJointStatesAsLocalParam called with request', request) service.callService(request, (response) => { console.log('Got joint state response', response) - setCurrentConfigurationParams(props.paramNames.map(() => response.joint_state.position)) + setCurrentConfigurationParams(props.paramNames.map((_, i) => props.getParamValues[i](response))) }) - }, [getJointStateService, props.paramNames, setCurrentConfigurationParams]) + }, [getRobotStateService, props.getEndEffectorPose, props.getParamValues, props.paramNames, setCurrentConfigurationParams]) - // Callback to move the robot to another configuration + // Callback to move the robot to another configuration. If the robot is at the user's face, + // first moves back from their mouth. const moveToButtonClicked = useCallback( (nextMealState) => { doneButtonIsClicked.current = false - unmountTeleopSubcomponentCallback.current = getSetLocalCurrMealStateWrapper(nextMealState) + let nextNextMealState = null + // If we are at the user's mouth, prepend MoveFromMouth to the motion. + if (settingsPageAtMouth) { + // MoveIt often fails to execute small trajectories, which are often planned + // when doing MovetoStaging immediately following MoveFromMouth. Thus, we + // leave the robot in the MoveFromMouth configuration. It is not technically + // the actual staging configuration, but it is the best we can do given + // the MoveIt limitation. + if (nextMealState !== MEAL_STATE.R_MovingToStagingConfiguration) { + nextNextMealState = nextMealState + } + nextMealState = MEAL_STATE.R_MovingFromMouth + } + unmountTeleopSubcomponentCallback.current = getSetLocalCurrMealStateWrapper(nextMealState, nextNextMealState) }, - [getSetLocalCurrMealStateWrapper, doneButtonIsClicked, unmountTeleopSubcomponentCallback] + [getSetLocalCurrMealStateWrapper, doneButtonIsClicked, settingsPageAtMouth, unmountTeleopSubcomponentCallback] ) // Callback to return to the main settings page @@ -218,7 +300,15 @@ const CustomizeConfiguration = (props) => { }} > - + @@ -251,14 +341,14 @@ const CustomizeConfiguration = (props) => { {props.otherButtonConfigs.map(({ name, mealState }) => ( - {/* - props.doneCallback()}> - Go To Menu Without Saving - - */} {}, modalChildren: <>, - resetToPresetSuccessCallback: () => {} + resetToPresetSuccessCallback: { + current: () => {} + } } export default SettingsPageParent diff --git a/feedingwebapp/src/buttons/HoldButton.jsx b/feedingwebapp/src/buttons/HoldButton.jsx index 0d6eccb7..48349087 100644 --- a/feedingwebapp/src/buttons/HoldButton.jsx +++ b/feedingwebapp/src/buttons/HoldButton.jsx @@ -29,34 +29,50 @@ function HoldButton(props) { const intervalRef = useRef(null) // Callback to stop the interval - const stopInterval = useCallback(() => { - if (intervalRef.current) { - clearInterval(intervalRef.current) - intervalRef.current = null - let cleanupCallback = props.cleanupCallback - cleanupCallback() - } - }, [props.cleanupCallback]) + const stopInterval = useCallback( + (event = null) => { + // Prevent further processing of the event. This is because sometimes touches + // also trigger clicks: https://web.dev/articles/mobile-touchandmouse + if (event) { + event.preventDefault() + } + if (intervalRef.current) { + clearInterval(intervalRef.current) + intervalRef.current = null + let cleanupCallback = props.cleanupCallback + cleanupCallback() + } + }, + [props.cleanupCallback] + ) // Callback to start the rate_hz interval - const startInterval = useCallback(() => { - // Stop the interval if it exists - stopInterval() - // Start a new interval - intervalRef.current = setInterval(() => { - let holdCallback = props.holdCallback - holdCallback() - }, 1000.0 / props.rate_hz) - }, [ - props.rate_hz, - props.holdCallback, - stopInterval - // setCounter - ]) + const startInterval = useCallback( + (event = null) => { + // Prevent further processing of the event. This is because sometimes touches + // also trigger clicks: https://web.dev/articles/mobile-touchandmouse + if (event) { + event.preventDefault() + } + // Stop the interval if it exists + stopInterval() + // Start a new interval + intervalRef.current = setInterval(() => { + let holdCallback = props.holdCallback + holdCallback() + }, 1000.0 / props.rate_hz) + }, + [props.rate_hz, props.holdCallback, stopInterval] + ) // Callback for when the touch moves const onTouchMove = useCallback( - (event) => { + (event = null) => { + // Prevent further processing of the event. This is because sometimes touches + // also trigger clicks: https://web.dev/articles/mobile-touchandmouse + if (event) { + event.preventDefault() + } let { top, left, bottom, right } = buttonRef.current.getBoundingClientRect() if ( event.touches === null || @@ -91,7 +107,7 @@ function HoldButton(props) { onTouchEnd={stopInterval} onTouchCancel={stopInterval} onTouchMove={onTouchMove} - onContextMenu={(e) => e.preventDefault()} + onContextMenu={(event) => event.preventDefault()} > {props.children} diff --git a/feedingwebapp/src/robot/VideoStream.jsx b/feedingwebapp/src/robot/VideoStream.jsx index 928271e1..d9c736d0 100644 --- a/feedingwebapp/src/robot/VideoStream.jsx +++ b/feedingwebapp/src/robot/VideoStream.jsx @@ -56,13 +56,13 @@ function VideoStream(props) { */ const imageCallback = useCallback( (message) => { - // console.log('Got image message', message) + // console.log('Got image message for topic', props.topic) if (img.src) { URL.revokeObjectURL(img.src) } img.src = URL.createObjectURL(dataURItoBlob('data:image/jpg;base64,' + message.data)) }, - [img] + [img /*, props.topic*/] ) // Subscribe to the image topic