Added MoveFromMouthToStagingConfiguration

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/launch/feeding_web_app_dummy_nodes_launch.xml

@@ -50,6 +50,8 @@
     <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

@@ -40,6 +40,7 @@
             "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/src/Pages/Constants.js

@@ -27,6 +27,7 @@ MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingFromMouthToAbovePlate] = '/robot_state
 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 }
@@ -91,6 +92,10 @@ 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/MoveToMouth'

feedingwebapp/src/Pages/GlobalState.jsx

@@ -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
@@ -60,6 +67,7 @@ export const MEAL_STATE = {
   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',

feedingwebapp/src/Pages/Home/Home.jsx

@@ -173,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

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

@@ -234,7 +234,7 @@ const RobotMotion = (props) => {
     if (mealState === MEAL_STATE.R_MovingToStagingConfiguration) {
       backMealState.current = MEAL_STATE.R_MovingToRestingPosition
     } else if (mealState === MEAL_STATE.R_MovingToMouth) {
-      backMealState.current = MEAL_STATE.R_MovingToStagingConfiguration
+      backMealState.current = MEAL_STATE.R_MovingFromMouthToStagingConfiguration
     } else {
       backMealState.current = MEAL_STATE.R_MovingAbovePlate
     }