Added MoveFromMouth Actions (#80)

* Added MoveFromMouth actions

* Added MoveFromMouth dummy actions

* Update setup.py

Author: amalnanavati

feeding_web_app_ros2_test/feeding_web_app_ros2_test/MoveFromMouthToAbovePlate.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_above_plate = MoveToDummy("MoveFromMouthToAbovePlate", MoveTo)
+
+    # Use a MultiThreadedExecutor to enable processing goals concurrently
+    executor = MultiThreadedExecutor()
+
+    rclpy.spin(move_above_plate, executor=executor)
+
+
+if __name__ == "__main__":
+    main()

feeding_web_app_ros2_test/feeding_web_app_ros2_test/MoveFromMouthToRestingPosition.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_resting_position = MoveToDummy("MoveFromMouthToRestingPosition", MoveTo)
+
+    # Use a MultiThreadedExecutor to enable processing goals concurrently
+    executor = MultiThreadedExecutor()
+
+    rclpy.spin(move_to_resting_position, executor=executor)
+
+
+if __name__ == "__main__":
+    main()

feeding_web_app_ros2_test/launch/feeding_web_app_dummy_nodes_launch.xml

@@ -48,6 +48,10 @@
     <node pkg="feeding_web_app_ros2_test" exec="MoveToRestingPosition" name="MoveToRestingPosition"/>
     <!-- Motion: The MoveToMouth action -->
     <node pkg="feeding_web_app_ros2_test" exec="MoveToMouth" name="MoveToMouth"/>
+    <!-- Motion: The MoveFromMouthToAbovePlate action -->
+    <node pkg="feeding_web_app_ros2_test" exec="MoveFromMouthToAbovePlate" name="MoveFromMouthToAbovePlate"/>
+    <!-- Motion: The MoveFromMouthToRestingPosition action -->
+    <node pkg="feeding_web_app_ros2_test" exec="MoveFromMouthToRestingPosition" name="MoveFromMouthToRestingPosition"/>
     <!-- Motion: The MoveToStowLocation action -->
     <node pkg="feeding_web_app_ros2_test" exec="MoveToStowLocation" name="MoveToStowLocation"/>
   </group>

feeding_web_app_ros2_test/setup.py

@@ -39,6 +39,8 @@
             "MoveAbovePlate = feeding_web_app_ros2_test.MoveAbovePlate:main",
             "MoveToRestingPosition = feeding_web_app_ros2_test.MoveToRestingPosition:main",
             "MoveToMouth = feeding_web_app_ros2_test.MoveToMouth: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",
             "SegmentFromPoint = feeding_web_app_ros2_test.SegmentFromPoint:main",
             # Scripts for the "TestROS" component

feedingwebapp/src/Pages/Constants.js

@@ -23,7 +23,9 @@ export const TIME_TO_RESET_MS = 3600000 // 1 hour in milliseconds
  */
 let MOVING_STATE_ICON_DICT = {}
 MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingAbovePlate] = '/robot_state_imgs/move_above_plate_position.svg'
+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_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 }
@@ -54,6 +56,10 @@ ROS_ACTIONS_NAMES[MEAL_STATE.R_MovingAbovePlate] = {
   actionName: 'MoveAbovePlate',
   messageType: 'ada_feeding_msgs/action/MoveTo'
 }
+ROS_ACTIONS_NAMES[MEAL_STATE.R_MovingFromMouthToAbovePlate] = {
+  actionName: 'MoveFromMouthToAbovePlate',
+  messageType: 'ada_feeding_msgs/action/MoveTo'
+}
 ROS_ACTIONS_NAMES[MEAL_STATE.U_BiteSelection] = {
   actionName: 'SegmentFromPoint',
   messageType: 'ada_feeding_msgs/action/SegmentFromPoint'
@@ -66,6 +72,10 @@ ROS_ACTIONS_NAMES[MEAL_STATE.R_MovingToRestingPosition] = {
   actionName: 'MoveToRestingPosition',
   messageType: 'ada_feeding_msgs/action/MoveTo'
 }
+ROS_ACTIONS_NAMES[MEAL_STATE.R_MovingFromMouthToRestingPosition] = {
+  actionName: 'MoveFromMouthToRestingPosition',
+  messageType: 'ada_feeding_msgs/action/MoveTo'
+}
 ROS_ACTIONS_NAMES[MEAL_STATE.R_MovingToMouth] = {
   actionName: 'MoveToMouth',
   messageType: 'ada_feeding_msgs/action/MoveTo'

feedingwebapp/src/Pages/GlobalState.jsx

@@ -36,6 +36,14 @@ export const APP_PAGE = {
  *     bite acquisition was succesful or not.
  *   - R_MovingToMouth: Waiting for the robot to finish moving to the user's
  *     mouth.
+ *   - 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
+ *     (e.g., a slower speed).
+ *   - R_MovingFromMouthToRestingPosition: Waiting for the robot to move from
+ *     the user's mouth to resting position. This is a separate action from
+ *     R_MovingToRestingPosition to allow us to customize the departure from
+ *     the mouth (e.g., a slower speed).
  *   - U_BiteDone: Waiting for the user to indicate that they are done eating
  *     the bite.
  *   - R_StowingArm: Waiting for the robot to stow the arm.
@@ -50,6 +58,8 @@ export const MEAL_STATE = {
   R_MovingToRestingPosition: 'R_MovingToRestingPosition',
   U_BiteAcquisitionCheck: 'U_BiteAcquisitionCheck',
   R_MovingToMouth: 'R_MovingToMouth',
+  R_MovingFromMouthToAbovePlate: 'R_MovingFromMouthToAbovePlate',
+  R_MovingFromMouthToRestingPosition: 'R_MovingFromMouthToRestingPosition',
   U_BiteDone: 'U_BiteDone',
   R_StowingArm: 'R_StowingArm',
   U_PostMeal: 'U_PostMeal'

feedingwebapp/src/Pages/Home/Home.jsx

@@ -145,6 +145,38 @@ function Home(props) {
           />
         )
       }
+      case MEAL_STATE.R_MovingFromMouthToAbovePlate: {
+        /**
+         * 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_MovingFromMouthToAbovePlate
+        let nextMealState = MEAL_STATE.U_BiteSelection
+        let waitingText = 'Waiting to move above the plate...'
+        return (
+          <RobotMotion
+            debug={props.debug}
+            mealState={currentMealState}
+            nextMealState={nextMealState}
+            actionInput={moveAbovePlateActionInput}
+            waitingText={waitingText}
+          />
+        )
+      }
+      case MEAL_STATE.R_MovingFromMouthToRestingPosition: {
+        let currentMealState = MEAL_STATE.R_MovingFromMouthToRestingPosition
+        let nextMealState = MEAL_STATE.U_BiteAcquisitionCheck
+        let waitingText = 'Waiting to move to the resting position...'
+        return (
+          <RobotMotion
+            debug={props.debug}
+            mealState={currentMealState}
+            nextMealState={nextMealState}
+            actionInput={moveToRestingPositionActionInput}
+            waitingText={waitingText}
+          />
+        )
+      }
       case MEAL_STATE.U_BiteDone: {
         return <BiteDone debug={props.debug} />
       }

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

@@ -18,9 +18,9 @@ const BiteDone = () => {
   // Get the relevant global variables
   const setMealState = useGlobalState((state) => state.setMealState)
   // Get icon image for move above plate
-  let moveAbovePlateImage = MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingAbovePlate]
+  let moveAbovePlateImage = MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingFromMouthToAbovePlate]
   // Get icon image for move to resting position
-  let moveToRestingPositionImage = MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingToRestingPosition]
+  let moveToRestingPositionImage = MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingFromMouthToRestingPosition]
   // Flag to check if the current orientation is portrait
   const isPortrait = useMediaQuery({ query: '(orientation: portrait)' })
   // Indicator of how to arrange screen elements based on orientation
@@ -36,14 +36,14 @@ const BiteDone = () => {
    * Callback function for when the user wants to move above plate.
    */
   const moveAbovePlate = useCallback(() => {
-    setMealState(MEAL_STATE.R_MovingAbovePlate)
+    setMealState(MEAL_STATE.R_MovingFromMouthToAbovePlate)
   }, [setMealState])
 
   /**
    * Callback function for when the user wants to move to resting position.
    */
   const moveToRestingPosition = useCallback(() => {
-    setMealState(MEAL_STATE.R_MovingToRestingPosition)
+    setMealState(MEAL_STATE.R_MovingFromMouthToRestingPosition)
   }, [setMealState])
 
   /**