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 }) => (
{},
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