Added staging configuration customization

feedingwebapp/src/Pages/Constants.js

@@ -118,8 +118,8 @@ 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'
@@ -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',

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
@@ -144,7 +145,7 @@ export const useGlobalState = create(
       // 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,
+      settingsPageAtFace: 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
+            settingsPageAtFace: 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) =>
+      setSettingsPageAtFace: (settingsPageAtFace) =>
         set(() => ({
-          biteTransferPageAtFace: biteTransferPageAtFace
+          settingsPageAtFace: settingsPageAtFace
         })),
       setBiteSelectionZoom: (biteSelectionZoom) =>
         set(() => ({

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
    *

feedingwebapp/src/Pages/Home/MealStates/DetectingFaceSubcomponent.jsx

@@ -5,18 +5,25 @@ 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
  * configuration. It displays the output of face detection, and automatically
  * 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 +58,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 +79,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 +103,7 @@ const DetectingFaceSubcomponent = (props) => {
           height: '100%'
         }}
       >
-        <VideoFeed topic={FACE_DETECTION_IMG_TOPIC} webrtcURL={props.webrtcURL} />
+        <VideoFeed topic={FACE_DETECTION_IMG_TOPIC} webrtcURL={props.webrtcURL} toggleFaceDetection={true} />
       </View>
     </>
   )

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
           </Button>
@@ -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