Implement Web App Teleop & Arm Configuration Customization (#133)

* Added a HoldButton to the app

* [WIP] began teleop subcomponent

* [WIP] MVP done and tested in sim

* Works on lovelace

* Arranged teleop within the modal

* Only allow teleop in non-moving states (WIP)

* Allow customization of teleop speed

* Fixes from in-person testing of speed

* Unified setting of non-moving states, fixed prevMealState bug, removed PlateLocator

* [WIP] adding teleop to settings

* [WIP] Visual layout of AbovePlate done

* [WIP] Code is in-place, need to debug

* [WIP] basic functionality works, need to test it

* [WIP] adjust parent view

* Mostly working, few lingering bugs with action cancellation, then test in-person

* Sim testing done, need to do real testing

* Fixes through in-person testing

* Updated min and default speeds for teleop

* Got teleop working directly with controllers

* Adjust default joint teleop speed

* Adjust video feed formatting

Author: amalnanavati

feedingwebapp/src/Pages/Constants.js

@@ -30,21 +30,6 @@ MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingToMouth] = '/robot_state_imgs/move_to_
 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)
-NON_MOVING_STATES.add(MEAL_STATE.U_BiteSelection)
-NON_MOVING_STATES.add(MEAL_STATE.U_BiteAcquisitionCheck)
-NON_MOVING_STATES.add(MEAL_STATE.U_BiteDone)
-NON_MOVING_STATES.add(MEAL_STATE.U_PostMeal)
-export { NON_MOVING_STATES }
-
 // The names of the ROS topic(s)
 export const CAMERA_FEED_TOPIC = '/local/camera/color/image_raw/compressed'
 export const FACE_DETECTION_TOPIC = '/face_detection'
@@ -53,6 +38,10 @@ export const FACE_DETECTION_IMG_TOPIC = '/face_detection_img/compressed'
 export const FOOD_ON_FORK_DETECTION_TOPIC = '/food_on_fork_detection'
 export const FOOD_ON_FORK_DETECTION_TOPIC_MSG = 'ada_feeding_msgs/FoodOnForkDetection'
 export const ROBOT_COMPRESSED_IMG_TOPICS = [CAMERA_FEED_TOPIC, FACE_DETECTION_IMG_TOPIC]
+export const SERVO_CARTESIAN_TOPIC = '/web_app/servo_node/delta_twist_cmds'
+export const SERVO_CARTESIAN_TOPIC_MSG = 'geometry_msgs/msg/TwistStamped'
+export const SERVO_JOINT_TOPIC = '/web_app/servo_node/delta_joint_cmds'
+export const SERVO_JOINT_TOPIC_MSG = 'control_msgs/msg/JointJog'
 
 // States from which, if they fail, it is NOT okay for the user to retry the
 // same action.
@@ -98,6 +87,10 @@ ROS_ACTIONS_NAMES[MEAL_STATE.R_StowingArm] = {
   messageType: 'ada_feeding_msgs/action/MoveTo'
 }
 export { ROS_ACTIONS_NAMES }
+export const START_CARTESIAN_CONTROLLER_ACTION_NAME = 'ActivateCartesianController'
+export const START_CARTESIAN_CONTROLLER_ACTION_TYPE = 'ada_feeding_msgs/action/Trigger'
+export const START_JOINT_CONTROLLER_ACTION_NAME = 'ActivateJointController'
+export const START_JOINT_CONTROLLER_ACTION_TYPE = 'ada_feeding_msgs/action/Trigger'
 
 /**
  * For states that call ROS services, this dictionary contains
@@ -121,13 +114,35 @@ 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_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'
 
 // The names of parameters users can change in the settings menu
 export const DISTANCE_TO_MOUTH_PARAM = 'MoveToMouth.tree_kwargs.plan_distance_from_mouth'
+export const ABOVE_PLATE_PARAM_JOINTS = 'MoveAbovePlate.tree_kwargs.joint_positions'
+export const STAGING_PARAM_JOINTS = 'MoveToStagingConfiguration.tree_kwargs.goal_configuration'
+export const STAGING_PARAM_POSITION = 'MoveFromMouth.tree_kwargs.staging_configuration_position'
+export const STAGING_PARAM_ORIENTATION = 'MoveFromMouth.tree_kwargs.staging_configuration_quat_xyzw'
+// TODO: Eventually, we should break AcquireFood into two actionss to avoid these
+// two different resting parameters.
+export const RESTING_PARAM_JOINTS_1 = 'AcquireFood.tree_kwargs.resting_joint_positions'
+// TODO: We may need to remove the orientation constraint from the below action.
+export const RESTING_PARAM_JOINTS_2 = 'MoveToRestingPosition.tree_kwargs.goal_configuration'
+
+// Robot link names
+export const ROBOT_BASE_LINK = 'j2n6s200_link_base'
+export const ROBOT_JOINTS = [
+  'j2n6s200_joint_1',
+  'j2n6s200_joint_2',
+  'j2n6s200_joint_3',
+  'j2n6s200_joint_4',
+  'j2n6s200_joint_5',
+  'j2n6s200_joint_6'
+]
 
 /**
  * The meaning of the status that motion actions return in their results.
@@ -157,3 +172,31 @@ export const ROS_ACTION_STATUS_CANCEL_GOAL = '2'
 export const ROS_ACTION_STATUS_SUCCEED = '3'
 export const ROS_ACTION_STATUS_ABORT = '4'
 export const ROS_ACTION_STATUS_CANCELED = '5'
+
+/**
+ * A function that provides the text associated with robot motion for each meal state.
+ */
+export function getRobotMotionText(mealState) {
+  switch (mealState) {
+    case MEAL_STATE.R_MovingAbovePlate:
+      return 'Waiting to move above the plate...'
+    case MEAL_STATE.R_BiteAcquisition:
+      return 'Waiting to acquire the food...'
+    case MEAL_STATE.R_MovingToRestingPosition:
+      return 'Waiting to move to the resting position...'
+    case MEAL_STATE.R_MovingToStagingConfiguration:
+      return 'Waiting to move in front of you...'
+    case MEAL_STATE.R_MovingToMouth:
+      return 'Waiting to move to your mouth...'
+    case MEAL_STATE.R_MovingFromMouth:
+      return 'Waiting to move from your mouth...'
+    case MEAL_STATE.R_StowingArm:
+      return 'Waiting to stow the arm...'
+    default:
+      return 'Unknown meal state' + mealState.toString()
+  }
+}
+
+// Container IDs for multiple ToastContainers
+export const REGULAR_CONTAINER_ID = 'RegularContainerID'
+export const MODAL_CONTAINER_ID = 'ModalContainerID'

feedingwebapp/src/Pages/GlobalState.jsx

@@ -26,8 +26,6 @@ export const APP_PAGE = {
  *   - U_PreMeal: Waiting for the user to click "Start Feeding."
  *   - R_MovingAbovePlate: Waiting for the robot to move above the plate.
  *   - U_BiteSelection: Waiting for the user to select the food item they want.
- *   - U_PlateLocator: Allows the user to teleoperate the robot to center the
- *     the plate.
  *   - R_BiteAcquisition: Waiting for the robot to execute one bite acquisition
  *     attempt.
  *   - R_MovingToRestingPosition: Waiting for the robot to move to resting
@@ -51,7 +49,6 @@ export const MEAL_STATE = {
   U_PreMeal: 'U_PreMeal',
   R_MovingAbovePlate: 'R_MovingAbovePlate',
   U_BiteSelection: 'U_BiteSelection',
-  U_PlateLocator: 'U_PlateLocator',
   R_BiteAcquisition: 'R_BiteAcquisition',
   R_MovingToRestingPosition: 'R_MovingToRestingPosition',
   U_BiteAcquisitionCheck: 'U_BiteAcquisitionCheck',
@@ -64,6 +61,18 @@ export const MEAL_STATE = {
   U_PostMeal: 'U_PostMeal'
 }
 
+/**
+ * A set containing the states where the robot does not move.
+ */
+let NON_MOVING_STATES = new Set()
+NON_MOVING_STATES.add(MEAL_STATE.U_PreMeal)
+NON_MOVING_STATES.add(MEAL_STATE.U_BiteSelection)
+NON_MOVING_STATES.add(MEAL_STATE.U_BiteAcquisitionCheck)
+NON_MOVING_STATES.add(MEAL_STATE.R_DetectingFace)
+NON_MOVING_STATES.add(MEAL_STATE.U_BiteDone)
+NON_MOVING_STATES.add(MEAL_STATE.U_PostMeal)
+export { NON_MOVING_STATES }
+
 /**
  * SETTINGS_STATE controls which settings page to display.
  *  - MAIN: The main page, with options to navigate to the other pages.
@@ -72,7 +81,8 @@ export const MEAL_STATE = {
  */
 export const SETTINGS_STATE = {
   MAIN: 'MAIN',
-  BITE_TRANSFER: 'BITE_TRANSFER'
+  BITE_TRANSFER: 'BITE_TRANSFER',
+  ABOVE_PLATE: 'ABOVE_PLATE'
 }
 
 // The name of the default parameter namespace
@@ -92,6 +102,9 @@ export const useGlobalState = create(
       mealState: MEAL_STATE.U_PreMeal,
       // The app's previous meal state
       prevMealState: null,
+      // Whether the app is currently in a non-moving state (i.e., the robot will
+      // not move unless the user initiates it)
+      inNonMovingState: true,
       // The timestamp when the robot transitioned to its current meal state
       mealStateTransitionTime: Date.now(),
       // The currently displayed settings page
@@ -106,10 +119,14 @@ export const useGlobalState = create(
       moveToMouthActionGoal: null,
       // Last RobotMotion action response
       lastMotionActionResponse: null,
-      // Whether or not the currently-executing robot motion was paused by the user
+      // Whether or not the currently-executing robot motion was paused by the user.
+      // NOTE: `paused` may no longer need to be in global state now that we have
+      // the `inNonMovingState` flag.
       paused: false,
-      // Flag to indicate robot motion trough teleoperation interface
-      teleopIsMoving: false,
+      // Store the user;s current settings for teleop speeds
+      teleopLinearSpeed: 0.1, // m/s
+      teleopAngularSpeed: 0.15, // rad/s
+      teleopJointSpeed: 0.2, // rad/s
       // Flag to indicate whether to auto-continue after face detection
       faceDetectionAutoContinue: true,
       // Flag to indicate whether to auto-continue in bite done after food-on-fork detection
@@ -145,17 +162,37 @@ export const useGlobalState = create(
         })),
       setMealState: (mealState, mostRecentBiteDoneResponse = null) =>
         set(() => {
+          let prevMealState = get().mealState
+          console.log('Setting meal state to', mealState, 'from', prevMealState)
           let retval = {
             mealState: mealState,
-            prevMealState: get().mealState,
             mealStateTransitionTime: Date.now(),
             biteTransferPageAtFace: 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)
+          if (prevMealState !== mealState) {
+            retval.prevMealState = prevMealState
+          }
           if (mostRecentBiteDoneResponse) {
             retval.mostRecentBiteDoneResponse = mostRecentBiteDoneResponse
           }
+          if (NON_MOVING_STATES.has(mealState)) {
+            retval.inNonMovingState = true
+            console.log('Setting inNonMovingState to true through setMealState')
+          } else {
+            retval.inNonMovingState = false
+            console.log('Setting inNonMovingState to false through setMealState')
+          }
           return retval
         }),
+      setInNonMovingState: (inNonMovingState) =>
+        set(() => {
+          console.log('Setting inNonMovingState to', inNonMovingState, 'through setInNonMovingState')
+          return {
+            inNonMovingState: inNonMovingState
+          }
+        }),
       setSettingsState: (settingsState) =>
         set(() => ({
           settingsState: settingsState
@@ -177,12 +214,34 @@ export const useGlobalState = create(
           moveToMouthActionGoal: moveToMouthActionGoal
         })),
       setPaused: (paused) =>
+        set(() => {
+          let retval = { paused: paused }
+          if (paused) {
+            // If the robot is paused, we should store this as a non-moving state
+            retval.inNonMovingState = true
+          } else {
+            // If the robot is unpaused, we should check if the meal state is moving
+            if (!NON_MOVING_STATES.has(get().mealState)) {
+              retval.inNonMovingState = false
+              console.log('Setting inNonMovingState to false through setPaused')
+            } else {
+              retval.inNonMovingState = true
+              console.log('Setting inNonMovingState to true through setPaused')
+            }
+          }
+          return retval
+        }),
+      setTeleopLinearSpeed: (teleopLinearSpeed) =>
+        set(() => ({
+          teleopLinearSpeed: teleopLinearSpeed
+        })),
+      setTeleopAngularSpeed: (teleopAngularSpeed) =>
         set(() => ({
-          paused: paused
+          teleopAngularSpeed: teleopAngularSpeed
         })),
-      setTeleopIsMoving: (teleopIsMoving) =>
+      setTeleopJointSpeed: (teleopJointSpeed) =>
         set(() => ({
-          teleopIsMoving: teleopIsMoving
+          teleopJointSpeed: teleopJointSpeed
         })),
       setFaceDetectionAutoContinue: (faceDetectionAutoContinue) =>
         set(() => ({

feedingwebapp/src/Pages/Header/Header.jsx

@@ -11,9 +11,9 @@ import 'react-toastify/dist/ReactToastify.css'
 // ROS imports
 import { useROS } from '../../ros/ros_helpers'
 // Local imports
-import { ROS_CHECK_INTERVAL_MS, NON_MOVING_STATES } from '../Constants'
-import { useGlobalState, APP_PAGE, MEAL_STATE } from '../GlobalState'
-import LiveVideoModal from './LiveVideoModal'
+import { REGULAR_CONTAINER_ID, ROS_CHECK_INTERVAL_MS } from '../Constants'
+import { useGlobalState, APP_PAGE, NON_MOVING_STATES } from '../GlobalState'
+import InfoModal from './InfoModal'
 
 /**
  * The Header component consists of the navigation bar (which has buttons Home,
@@ -23,10 +23,10 @@ import LiveVideoModal from './LiveVideoModal'
  */
 const Header = (props) => {
   // Create a local state variable to toggle on/off the video
-  // TODO: Since this local state variable is in the header, the LiveVideoModal
+  // TODO: Since this local state variable is in the header, the InfoModal
   // continues showing even if the state changes. Is this desirable? Perhaps
   // it should close if the state changes?
-  const [videoShow, setVideoShow] = useState(false)
+  const [infoModalShow, setInfoModalShow] = useState(false)
   // useROS gives us access to functions to configure and interact with ROS.
   let { ros } = useROS()
   const [isConnected, setIsConncected] = useState(ros.isConnected)
@@ -47,9 +47,8 @@ const Header = (props) => {
 
   // Get the relevant global state variables
   const mealState = useGlobalState((state) => state.mealState)
+  const inNonMovingState = useGlobalState((state) => state.inNonMovingState)
   const setAppPage = useGlobalState((state) => state.setAppPage)
-  const paused = useGlobalState((state) => state.paused)
-  const teleopIsMoving = useGlobalState((state) => state.teleopIsMoving)
 
   /**
    * When the Home button in the header is clicked, return to the Home page.
@@ -64,12 +63,18 @@ const Header = (props) => {
    * or terminate the meal because modifying settings.
    */
   const settingsClicked = useCallback(() => {
-    if (NON_MOVING_STATES.has(mealState)) {
+    // Both checks are necessary because some states that are nominally non-moving
+    // could be in an auto-continue state, and some states that are nominally moving
+    // could be paused.
+    if (inNonMovingState && NON_MOVING_STATES.has(mealState)) {
       setAppPage(APP_PAGE.Settings)
     } else {
-      toast('Wait for robot motion to complete before accessing Settings.')
+      toast.info('Wait for robot motion to complete before accessing Settings.', {
+        containerId: REGULAR_CONTAINER_ID,
+        toastId: 'noSettings'
+      })
     }
-  }, [mealState, setAppPage])
+  }, [inNonMovingState, mealState, setAppPage])
 
   // Render the component. The NavBar will stay fixed even as we vertically scroll.
   return (
@@ -78,7 +83,7 @@ const Header = (props) => {
        * The ToastContainer is an alert that pops up on the top of the screen
        * and has a timeout.
        */}
-      <ToastContainer style={{ fontSize: textFontSize }} />
+      <ToastContainer style={{ fontSize: textFontSize, zIndex: 9999 }} containerId={REGULAR_CONTAINER_ID} enableMultiContainer={true} />
       {/**
        * The NavBar has two elements, Home and Settings, on the left side and three
        * elements, Lock, Robot Connection Icon and VideoVideo, on the right side.
@@ -115,7 +120,7 @@ const Header = (props) => {
               Settings
             </Nav.Link>
           </Nav>
-          {NON_MOVING_STATES.has(mealState) || paused || (mealState === MEAL_STATE.U_PlateLocator && teleopIsMoving === false) ? (
+          {inNonMovingState ? (
             <Nav>
               <Nav.Link
                 className='text-dark rounded mx-1 btn-lg btn-huge p-2'
@@ -150,20 +155,20 @@ const Header = (props) => {
           )}
           <Nav>
             <Nav.Link
-              onClick={() => setVideoShow(true)}
+              onClick={() => setInfoModalShow(true)}
               className='text-dark bg-info rounded mx-1 btn-lg btn-huge p-2'
               style={{ fontSize: textFontSize }}
             >
-              Video
+              Info
             </Nav.Link>
           </Nav>
         </Navbar>
       </Navbar>
       {/**
-       * The LiveVideoModal toggles on and off with the Video button and shows the
+       * The InfoModal toggles on and off with the Video button and shows the
        * robot's live camera feed.
        */}
-      <LiveVideoModal show={videoShow} onHide={() => setVideoShow(false)} webrtcURL={props.webrtcURL} />
+      <InfoModal show={infoModalShow} onHide={() => setInfoModalShow(false)} webrtcURL={props.webrtcURL} />
     </>
   )
 }