Split MoveToMouth Into two Actions (#94)

* [WIP] Mostly implemented, a few lingering bugs

* Fixed bugs, tested in sim

* Fixed back/resume calls

* Lowered dummy FaceDetection time

* Added MoveFromMouthToStagingConfiguration

* Add the option to auto-continue or not from face detection

* Reduce distance for valid mouths to 1.25m

* Clear Octomap on Retry

* Show detected food once the SegmentFood action has been called

Author: amalnanavati

feeding_web_app_ros2_test/feeding_web_app_ros2_test/FaceDetection.py

@@ -14,7 +14,7 @@
 class FaceDetectionNode(Node):
     def __init__(
         self,
-        face_detection_interval=150,
+        face_detection_interval=90,
         num_images_with_face=60,
         open_mouth_interval=90,
         num_images_with_open_mouth=30,

feeding_web_app_ros2_test/feeding_web_app_ros2_test/MoveFromMouthToStagingConfiguration.py

@@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+from ada_feeding_msgs.action import MoveTo
+from feeding_web_app_ros2_test.MoveToDummy import MoveToDummy
+import rclpy
+from rclpy.executors import MultiThreadedExecutor
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    move_from_mouth_to_staging_configuration = MoveToDummy(
+        "MoveFromMouthToStagingConfiguration", MoveTo
+    )
+
+    # Use a MultiThreadedExecutor to enable processing goals concurrently
+    executor = MultiThreadedExecutor()
+
+    rclpy.spin(move_from_mouth_to_staging_configuration, executor=executor)
+
+
+if __name__ == "__main__":
+    main()

feeding_web_app_ros2_test/feeding_web_app_ros2_test/MoveToMouth.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python3
-from ada_feeding_msgs.action import MoveTo
+from ada_feeding_msgs.action import MoveToMouth
 from feeding_web_app_ros2_test.MoveToDummy import MoveToDummy
 import rclpy
 from rclpy.executors import MultiThreadedExecutor
@@ -8,7 +8,7 @@
 def main(args=None):
     rclpy.init(args=args)
 
-    move_to_mouth = MoveToDummy("MoveToMouth", MoveTo)
+    move_to_mouth = MoveToDummy("MoveToMouth", MoveToMouth)
 
     # Use a MultiThreadedExecutor to enable processing goals concurrently
     executor = MultiThreadedExecutor()

feeding_web_app_ros2_test/feeding_web_app_ros2_test/MoveToStagingConfiguration.py

@@ -0,0 +1,20 @@
+#!/usr/bin/env python3
+from ada_feeding_msgs.action import MoveTo
+from feeding_web_app_ros2_test.MoveToDummy import MoveToDummy
+import rclpy
+from rclpy.executors import MultiThreadedExecutor
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    move_to_staging_configuration = MoveToDummy("MoveToStagingConfiguration", MoveTo)
+
+    # Use a MultiThreadedExecutor to enable processing goals concurrently
+    executor = MultiThreadedExecutor()
+
+    rclpy.spin(move_to_staging_configuration, executor=executor)
+
+
+if __name__ == "__main__":
+    main()

feeding_web_app_ros2_test/launch/feeding_web_app_dummy_nodes_launch.xml

@@ -46,8 +46,12 @@
     <node pkg="feeding_web_app_ros2_test" exec="AcquireFood" name="AcquireFood"/>
     <!-- Motion: The MoveToRestingPosition action -->
     <node pkg="feeding_web_app_ros2_test" exec="MoveToRestingPosition" name="MoveToRestingPosition"/>
+    <!-- Motion: The MoveToStagingConfiguration action -->
+    <node pkg="feeding_web_app_ros2_test" exec="MoveToStagingConfiguration" name="MoveToStagingConfiguration"/>
     <!-- Motion: The MoveToMouth action -->
     <node pkg="feeding_web_app_ros2_test" exec="MoveToMouth" name="MoveToMouth"/>
+    <!-- Motion: The MoveFromMouthToStagingConfiguration action -->
+    <node pkg="feeding_web_app_ros2_test" exec="MoveFromMouthToStagingConfiguration" name="MoveFromMouthToStagingConfiguration"/>
     <!-- Motion: The MoveFromMouthToAbovePlate action -->
     <node pkg="feeding_web_app_ros2_test" exec="MoveFromMouthToAbovePlate" name="MoveFromMouthToAbovePlate"/>
     <!-- Motion: The MoveFromMouthToRestingPosition action -->

feeding_web_app_ros2_test/setup.py

@@ -38,7 +38,9 @@
             "FaceDetection = feeding_web_app_ros2_test.FaceDetection:main",
             "MoveAbovePlate = feeding_web_app_ros2_test.MoveAbovePlate:main",
             "MoveToRestingPosition = feeding_web_app_ros2_test.MoveToRestingPosition:main",
+            "MoveToStagingConfiguration = feeding_web_app_ros2_test.MoveToStagingConfiguration:main",
             "MoveToMouth = feeding_web_app_ros2_test.MoveToMouth:main",
+            "MoveFromMouthToStagingConfiguration = feeding_web_app_ros2_test.MoveFromMouthToStagingConfiguration:main",
             "MoveFromMouthToAbovePlate = feeding_web_app_ros2_test.MoveFromMouthToAbovePlate:main",
             "MoveFromMouthToRestingPosition = feeding_web_app_ros2_test.MoveFromMouthToRestingPosition:main",
             "MoveToStowLocation = feeding_web_app_ros2_test.MoveToStowLocation:main",

feedingwebapp/public/robot_state_imgs/move_to_staging_configuration.svg

@@ -26,12 +26,18 @@ MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingAbovePlate] = '/robot_state_imgs/move_
 MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingFromMouthToAbovePlate] = '/robot_state_imgs/move_above_plate_position.svg'
 MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingToRestingPosition] = '/robot_state_imgs/move_to_resting_position.svg'
 MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingFromMouthToRestingPosition] = '/robot_state_imgs/move_to_resting_position.svg'
+MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingToStagingConfiguration] = '/robot_state_imgs/move_to_staging_configuration.svg'
+MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingFromMouthToStagingConfiguration] = '/robot_state_imgs/move_to_staging_configuration.svg'
 MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingToMouth] = '/robot_state_imgs/move_to_mouth_position.svg'
 MOVING_STATE_ICON_DICT[MEAL_STATE.R_StowingArm] = '/robot_state_imgs/stowing_arm_position.svg'
 export { MOVING_STATE_ICON_DICT }
 
 /**
  * A set containing the states where the robot does not move.
+ *
+ * NOTE: Although in R_DetectingFace the robot does not technically move,
+ * the app might transition out of that state into a robot motion state without
+ * user intervention, so it is not included in this set.
  */
 let NON_MOVING_STATES = new Set()
 NON_MOVING_STATES.add(MEAL_STATE.U_PreMeal)
@@ -47,6 +53,12 @@ export const FACE_DETECTION_TOPIC = '/face_detection'
 export const FACE_DETECTION_TOPIC_MSG = 'ada_feeding_msgs/FaceDetection'
 export const FACE_DETECTION_IMG_TOPIC = '/face_detection_img'
 
+// States from which, if they fail, it is NOT okay for the user to retry the
+// same action.
+let NON_RETRYABLE_STATES = new Set()
+NON_RETRYABLE_STATES.add(MEAL_STATE.R_BiteAcquisition)
+export { NON_RETRYABLE_STATES }
+
 /**
  * For states that call ROS actions, this dictionary contains
  * the action name and the message type
@@ -76,16 +88,37 @@ ROS_ACTIONS_NAMES[MEAL_STATE.R_MovingFromMouthToRestingPosition] = {
   actionName: 'MoveFromMouthToRestingPosition',
   messageType: 'ada_feeding_msgs/action/MoveTo'
 }
+ROS_ACTIONS_NAMES[MEAL_STATE.R_MovingToStagingConfiguration] = {
+  actionName: 'MoveToStagingConfiguration',
+  messageType: 'ada_feeding_msgs/action/MoveTo'
+}
+ROS_ACTIONS_NAMES[MEAL_STATE.R_MovingFromMouthToStagingConfiguration] = {
+  actionName: 'MoveFromMouthToStagingConfiguration',
+  messageType: 'ada_feeding_msgs/action/MoveTo'
+}
 ROS_ACTIONS_NAMES[MEAL_STATE.R_MovingToMouth] = {
   actionName: 'MoveToMouth',
-  messageType: 'ada_feeding_msgs/action/MoveTo'
+  messageType: 'ada_feeding_msgs/action/MoveToMouth'
 }
 ROS_ACTIONS_NAMES[MEAL_STATE.R_StowingArm] = {
   actionName: 'MoveToStowLocation',
   messageType: 'ada_feeding_msgs/action/MoveTo'
 }
 export { ROS_ACTIONS_NAMES }
 
+/**
+ * For states that call ROS services, this dictionary contains
+ * the service name and the message type
+ */
+let ROS_SERVICE_NAMES = {}
+ROS_SERVICE_NAMES[MEAL_STATE.R_DetectingFace] = {
+  serviceName: 'toggle_face_detection',
+  messageType: 'std_srvs/srv/SetBool'
+}
+export { ROS_SERVICE_NAMES }
+export const CLEAR_OCTOMAP_SERVICE_NAME = 'clear_octomap'
+export const CLEAR_OCTOMAP_SERVICE_TYPE = 'std_srvs/srv/Empty'
+
 /**
  * The meaning of the status that motion actions return in their results.
  * These should match the action definition(s).

feedingwebapp/src/Pages/Constants.js

@@ -34,8 +34,15 @@ export const APP_PAGE = {
  *     position.
  *   - U_BiteAcquisitionCheck: Waiting for the user to specify whether the
  *     bite acquisition was succesful or not.
+ *   - R_MovingToStagingConfiguration: Waiting for the robot to move to the
+ *     staging configuration.
+ *   - R_DetectingFace: Waiting for the robot to detect a face.
  *   - R_MovingToMouth: Waiting for the robot to finish moving to the user's
  *     mouth.
+ *   - R_MovingFromMouthToStagingConfiguration: Waiting for the robot to move
+ *     from the user's mouth to the staging configuration. This is a separate
+ *     action from R_MovingToStagingConfiguration to allow us to customize the
+ *     departure from the mouth (e.g., a slower speed).
  *   - R_MovingFromMouthToAbovePlate: Waiting for the robot to move from the
  *     user's mouth to above the plate. This is a separate action from
  *     R_MovingAbovePlate to allow us to customize the departure from the mouth
@@ -57,7 +64,10 @@ export const MEAL_STATE = {
   R_BiteAcquisition: 'R_BiteAcquisition',
   R_MovingToRestingPosition: 'R_MovingToRestingPosition',
   U_BiteAcquisitionCheck: 'U_BiteAcquisitionCheck',
+  R_MovingToStagingConfiguration: 'R_MovingToStagingConfiguration',
+  R_DetectingFace: 'R_DetectingFace',
   R_MovingToMouth: 'R_MovingToMouth',
+  R_MovingFromMouthToStagingConfiguration: 'R_MovingFromMouthToStagingConfiguration',
   R_MovingFromMouthToAbovePlate: 'R_MovingFromMouthToAbovePlate',
   R_MovingFromMouthToRestingPosition: 'R_MovingFromMouthToRestingPosition',
   U_BiteDone: 'U_BiteDone',
@@ -100,12 +110,19 @@ export const useGlobalState = create(
       mealStateTransitionTime: Date.now(),
       // The current app page
       appPage: APP_PAGE.Home,
-      // The most recent food item that the user selected in "bite selection"
-      desiredFoodItem: null,
+      // The goal for the bite acquisition action, including the most recent
+      // food item that the user selected in "bite selection"
+      biteAcquisitionActionGoal: null,
+      // The goal for the move to mouth action, including the most recent
+      // message received from the face detection node where a
+      // face was detected and within the distance bounds of the camera.
+      moveToMouthActionGoal: null,
       // Whether or not the currently-executing robot motion was paused by the user
       paused: false,
       // Flag to indicate robot motion trough teleoperation interface
       teleopIsMoving: false,
+      // Flag to indicate whether to auto-continue after face detection
+      faceDetectionAutoContinue: false,
       // Settings values
       stagingPosition: SETTINGS.stagingPosition[0],
       biteInitiation: SETTINGS.biteInitiation[0],
@@ -121,9 +138,13 @@ export const useGlobalState = create(
         set(() => ({
           appPage: appPage
         })),
-      setDesiredFoodItem: (desiredFoodItem) =>
+      setBiteAcquisitionActionGoal: (biteAcquisitionActionGoal) =>
         set(() => ({
-          desiredFoodItem: desiredFoodItem
+          biteAcquisitionActionGoal: biteAcquisitionActionGoal
+        })),
+      setMoveToMouthActionGoal: (moveToMouthActionGoal) =>
+        set(() => ({
+          moveToMouthActionGoal: moveToMouthActionGoal
         })),
       setPaused: (paused) =>
         set(() => ({
@@ -133,6 +154,10 @@ export const useGlobalState = create(
         set(() => ({
           teleopIsMoving: teleopIsMoving
         })),
+      setFaceDetectionAutoContinue: (faceDetectionAutoContinue) =>
+        set(() => ({
+          faceDetectionAutoContinue: faceDetectionAutoContinue
+        })),
       setStagingPosition: (stagingPosition) =>
         set(() => ({
           stagingPosition: stagingPosition

feedingwebapp/src/Pages/GlobalState.jsx

@@ -8,7 +8,7 @@ import { useMediaQuery } from 'react-responsive'
 // Component
 import PropTypes from 'prop-types'
 // Toast generates a temporary pop-up with a timeout.
-import { ToastContainer, toast } from 'react-toastify'
+import { ToastContainer /* , toast */ } from 'react-toastify'
 import 'react-toastify/dist/ReactToastify.css'
 // ROS imports
 import { useROS } from '../../ros/ros_helpers'
@@ -65,13 +65,13 @@ const Header = (props) => {
    * started, take the user to the settings menu. Else, ask them to complete
    * or terminate the meal because modifying settings.
    */
-  const settingsClicked = useCallback(() => {
-    if (mealState === MEAL_STATE.U_PreMeal || mealState === MEAL_STATE.U_PostMeal) {
-      setAppPage(APP_PAGE.Settings)
-    } else {
-      toast('Please complete or terminate the feeding process to access Settings.')
-    }
-  }, [mealState, setAppPage])
+  // const settingsClicked = useCallback(() => {
+  //   if (mealState === MEAL_STATE.U_PreMeal || mealState === MEAL_STATE.U_PostMeal) {
+  //     setAppPage(APP_PAGE.Settings)
+  //   } else {
+  //     toast('Please complete or terminate the feeding process to access Settings.')
+  //   }
+  // }, [mealState, setAppPage])
 
   // Render the component. The NavBar will stay fixed even as we vertically scroll.
   return (
@@ -109,13 +109,14 @@ const Header = (props) => {
             >
               Home
             </Nav.Link>
-            <Nav.Link
+            {/* TODO: Reinstate the settings menu when we implement settings! */}
+            {/* <Nav.Link
               onClick={settingsClicked}
               className='text-dark bg-info rounded mx-1 btn-lg btn-huge p-2'
               style={{ fontSize: textFontSize }}
             >
               Settings
-            </Nav.Link>
+            </Nav.Link> */}
           </Nav>
           {NON_MOVING_STATES.has(mealState) || paused || (mealState === MEAL_STATE.U_PlateLocator && teleopIsMoving === false) ? (
             <Nav>

feedingwebapp/src/Pages/Header/Header.jsx

@@ -9,6 +9,7 @@ import { useGlobalState, MEAL_STATE } from '../GlobalState'
 import BiteAcquisitionCheck from './MealStates/BiteAcquisitionCheck'
 import BiteDone from './MealStates/BiteDone'
 import BiteSelection from './MealStates/BiteSelection'
+import DetectingFace from './MealStates/DetectingFace'
 import PlateLocator from './MealStates/PlateLocator'
 import PostMeal from './MealStates/PostMeal'
 import PreMeal from './MealStates/PreMeal'
@@ -28,6 +29,8 @@ function Home(props) {
   // Get the relevant values from global state
   const mealState = useGlobalState((state) => state.mealState)
   const mealStateTransitionTime = useGlobalState((state) => state.mealStateTransitionTime)
+  const setBiteAcquisitionActionGoal = useGlobalState((state) => state.setBiteAcquisitionActionGoal)
+  const setMoveToMouthActionGoal = useGlobalState((state) => state.setMoveToMouthActionGoal)
   const setMealState = useGlobalState((state) => state.setMealState)
   const setPaused = useGlobalState((state) => state.setPaused)
 
@@ -40,23 +43,27 @@ function Home(props) {
   useEffect(() => {
     if (Date.now() - mealStateTransitionTime >= TIME_TO_RESET_MS) {
       console.log('Reverting to PreMeal due to too much elapsed time in one state.')
+      setBiteAcquisitionActionGoal(null)
+      setMoveToMouthActionGoal(null)
       setMealState(MEAL_STATE.U_PreMeal)
       setPaused(false)
     }
-  }, [mealStateTransitionTime, setMealState, setPaused])
+  }, [mealStateTransitionTime, setMealState, setPaused, setMoveToMouthActionGoal, setBiteAcquisitionActionGoal])
 
   // Get the relevant global variables
-  const desiredFoodItem = useGlobalState((state) => state.desiredFoodItem)
+  const biteAcquisitionActionGoal = useGlobalState((state) => state.biteAcquisitionActionGoal)
+  const moveToMouthActionGoal = useGlobalState((state) => state.moveToMouthActionGoal)
 
   /**
    * All action inputs are constant. Note that we must be cautious if making
    * them non-constant, because the robot will re-execute an action every time
    * the action input changes (even on re-renders).
    */
   const moveAbovePlateActionInput = useMemo(() => ({}), [])
-  const biteAcquisitionActionInput = useMemo(() => desiredFoodItem, [desiredFoodItem])
+  const biteAcquisitionActionInput = useMemo(() => biteAcquisitionActionGoal, [biteAcquisitionActionGoal])
   const moveToRestingPositionActionInput = useMemo(() => ({}), [])
-  const moveToMouthActionInput = useMemo(() => ({}), [])
+  const moveToStagingConfigurationActionInput = useMemo(() => ({}), [])
+  const moveToMouthActionInput = useMemo(() => moveToMouthActionGoal, [moveToMouthActionGoal])
   const moveToStowPositionActionInput = useMemo(() => ({}), [])
 
   /**
@@ -127,6 +134,27 @@ function Home(props) {
       case MEAL_STATE.U_BiteAcquisitionCheck: {
         return <BiteAcquisitionCheck debug={props.debug} />
       }
+      case MEAL_STATE.R_MovingToStagingConfiguration: {
+        /**
+         * We recreate currentMealState due to a race condition where sometimes
+         * the app is performing a re-rendering and *then* the state is updated.
+         */
+        let currentMealState = MEAL_STATE.R_MovingToStagingConfiguration
+        let nextMealState = MEAL_STATE.R_DetectingFace
+        let waitingText = 'Waiting to move in front of you...'
+        return (
+          <RobotMotion
+            debug={props.debug}
+            mealState={currentMealState}
+            nextMealState={nextMealState}
+            actionInput={moveToStagingConfigurationActionInput}
+            waitingText={waitingText}
+          />
+        )
+      }
+      case MEAL_STATE.R_DetectingFace: {
+        return <DetectingFace debug={props.debug} webVideoServerURL={props.webVideoServerURL} />
+      }
       case MEAL_STATE.R_MovingToMouth: {
         /**
          * We recreate currentMealState due to a race condition where sometimes
@@ -145,6 +173,24 @@ function Home(props) {
           />
         )
       }
+      case MEAL_STATE.R_MovingFromMouthToStagingConfiguration: {
+        /**
+         * We recreate currentMealState due to a race condition where sometimes
+         * the app is performing a re-rendering and *then* the state is updated.
+         */
+        let currentMealState = MEAL_STATE.R_MovingFromMouthToStagingConfiguration
+        let nextMealState = MEAL_STATE.R_DetectingFace
+        let waitingText = 'Waiting to move from your mouth to in front of you...'
+        return (
+          <RobotMotion
+            debug={props.debug}
+            mealState={currentMealState}
+            nextMealState={nextMealState}
+            actionInput={moveToStagingConfigurationActionInput}
+            waitingText={waitingText}
+          />
+        )
+      }
       case MEAL_STATE.R_MovingFromMouthToAbovePlate: {
         /**
          * We recreate currentMealState due to a race condition where sometimes
@@ -213,7 +259,8 @@ function Home(props) {
     moveAbovePlateActionInput,
     moveToMouthActionInput,
     moveToRestingPositionActionInput,
-    moveToStowPositionActionInput
+    moveToStowPositionActionInput,
+    moveToStagingConfigurationActionInput
   ])
 
   // Render the component