Extend Settings to allow customizing planning scene and bite transfer speed (#146)

* Allow users to adjust speed in the Bite Transfer settings

* Allow users to customize planning scene from settings

* Fixes from testing

Author: amalnanavati

feedingwebapp/public/robot_state_imgs/table.svg

@@ -29,6 +29,7 @@ MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingFromMouth] = '/robot_state_imgs/move_t
 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 }
+export const TABLE_ICON = '/robot_state_imgs/table.svg'
 
 // The names of the ROS topic(s)
 export const CAMERA_FEED_TOPIC = '/local/camera/color/image_raw/compressed'
@@ -122,14 +123,21 @@ export const GET_PARAMETERS_SERVICE_NAME = 'ada_feeding_action_servers/get_param
 export const GET_PARAMETERS_SERVICE_TYPE = 'rcl_interfaces/srv/GetParameters'
 export const SET_PARAMETERS_SERVICE_NAME = 'ada_feeding_action_servers/set_parameters_atomically'
 export const SET_PARAMETERS_SERVICE_TYPE = 'rcl_interfaces/srv/SetParametersAtomically'
+export const PLANNING_SCENE_GET_PARAMETERS_SERVICE_NAME = 'ada_planning_scene/get_parameters'
+export const PLANNING_SCENE_GET_PARAMETERS_SERVICE_TYPE = 'rcl_interfaces/srv/GetParameters'
 
 // 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 MOVE_TO_MOUTH_SPEED_PARAM = 'MoveToMouth.tree_kwargs.max_linear_speed'
+export const MOVE_TO_MOUTH_SPEED_NEAR_MOUTH_PARAM = 'MoveToMouth.tree_kwargs.linear_speed_near_mouth'
+export const MOVE_FROM_MOUTH_SPEED_PARAM = 'MoveFromMouth.tree_kwargs.max_linear_speed_to_staging_configuration'
+export const MOVE_FROM_MOUTH_SPEED_NEAR_MOUTH_PARAM = 'MoveFromMouth.tree_kwargs.linear_speed_near_mouth'
+export const PLANNING_SCENE_PARAM = 'planning_scene_namespace_to_use'
 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
+// TODO: Eventually, we should break AcquireFood into two actions 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.

feedingwebapp/src/Pages/Constants.js

@@ -76,8 +76,8 @@ export { NON_MOVING_STATES }
 /**
  * SETTINGS_STATE controls which settings page to display.
  *  - MAIN: The main page, with options to navigate to the other pages.
- *  - DISTANCE_TO_MOUTH: Allow the user to customize how close the robot gets
- *    to their mouth.
+ *  - BITE_TRANSFER: Allow the user to customize how close the robot gets
+ *    to their mouth and the speed of approach/departure.
  *  - ABOVE_PLATE: Allow the user to customize how high the fixed above plate
  *    arm configuration.
  *  - RESTING_CONFIGURATION: Allow the user to customize the fixed resting
@@ -89,11 +89,12 @@ export { NON_MOVING_STATES }
  */
 export const SETTINGS_STATE = {
   MAIN: 'MAIN',
-  DISTANCE_TO_MOUTH: 'DISTANCE_TO_MOUTH',
+  BITE_TRANSFER: 'BITE_TRANSFER',
   ABOVE_PLATE: 'ABOVE_PLATE',
   RESTING_CONFIGURATION: 'RESTING_CONFIGURATION',
   STAGING_CONFIGURATION: 'STAGING_CONFIGURATION',
-  STOW_CONFIGURATION: 'STOW_CONFIGURATION'
+  STOW_CONFIGURATION: 'STOW_CONFIGURATION',
+  PLANNING_SCENE: 'PLANNING_SCENE'
 }
 
 // The name of the default parameter namespace

feedingwebapp/src/Pages/GlobalState.jsx

@@ -7,7 +7,14 @@ import Button from 'react-bootstrap/Button'
 import { View } from 'react-native'
 
 // Local imports
-import { getRobotMotionText, DISTANCE_TO_MOUTH_PARAM } from '../Constants'
+import {
+  getRobotMotionText,
+  DISTANCE_TO_MOUTH_PARAM,
+  MOVE_TO_MOUTH_SPEED_PARAM,
+  MOVE_TO_MOUTH_SPEED_NEAR_MOUTH_PARAM,
+  MOVE_FROM_MOUTH_SPEED_PARAM,
+  MOVE_FROM_MOUTH_SPEED_NEAR_MOUTH_PARAM
+} from '../Constants'
 import { useGlobalState, MEAL_STATE, SETTINGS_STATE } from '../GlobalState'
 import RobotMotion from '../Home/MealStates/RobotMotion'
 import DetectingFaceSubcomponent from '../Home/MealStates/DetectingFaceSubcomponent'
@@ -28,8 +35,17 @@ const BiteTransfer = (props) => {
 
   // Create relevant local state variables
   // Configure the parameters for SettingsPageParent
-  const paramNames = useMemo(() => [DISTANCE_TO_MOUTH_PARAM], [])
-  const [currentDistanceToMouth, setCurrentDistanceToMouth] = useState([null])
+  const paramNames = useMemo(
+    () => [
+      DISTANCE_TO_MOUTH_PARAM,
+      MOVE_TO_MOUTH_SPEED_PARAM,
+      MOVE_TO_MOUTH_SPEED_NEAR_MOUTH_PARAM,
+      MOVE_FROM_MOUTH_SPEED_PARAM,
+      MOVE_FROM_MOUTH_SPEED_NEAR_MOUTH_PARAM
+    ],
+    []
+  )
+  const [currentParams, setCurrentParams] = useState([null, null, null, null, null])
   const [localCurrAndNextMealState, setLocalCurrAndNextMealState] = useState(
     globalMealState === MEAL_STATE.U_BiteDone || globalMealState === MEAL_STATE.R_DetectingFace || settingsPageAtMouth
       ? [MEAL_STATE.R_MovingFromMouth, null]
@@ -53,6 +69,8 @@ const BiteTransfer = (props) => {
   // Get min and max distance to mouth
   const minDistanceToMouth = 1 // cm
   const maxDistanceToMouth = 10 // cm
+  const minLinearSpeed = 2 // cm/s
+  const maxLinearSpeed = 15 // cm/s
 
   // When we set local meal state, also update bite transfer page at face
   const setLocalCurrMealStateWrapper = useCallback(
@@ -166,16 +184,57 @@ const BiteTransfer = (props) => {
       if (value > maxDistanceToMouth) {
         value = maxDistanceToMouth
       }
-      let fullDistanceToMouth = [value / 100.0, currentDistanceToMouth[0][1], currentDistanceToMouth[0][2]]
-      setCurrentDistanceToMouth([fullDistanceToMouth])
+      let distance_m = value / 100.0
+      setCurrentParams((currentParams) => [
+        [distance_m, currentParams[0][1], currentParams[0][2]],
+        currentParams[1],
+        currentParams[2],
+        currentParams[3],
+        currentParams[4]
+      ])
+    },
+    [minDistanceToMouth, maxDistanceToMouth]
+  )
+
+  // Callback for when the user changes the speed to the mouth
+  const onSpeedChange = useCallback(
+    (_ev, data, index) => {
+      let value = data.value !== null ? data.value : parseFloat(data.displayValue)
+      if (value < minLinearSpeed) {
+        value = minLinearSpeed
+      }
+      if (value > maxLinearSpeed) {
+        value = maxLinearSpeed
+      }
+      let speed_mps = value / 100.0
+      setCurrentParams((currentParams) => [
+        currentParams[0],
+        index === 1 ? speed_mps : currentParams[1],
+        index === 2 ? speed_mps : currentParams[2],
+        index === 3 ? speed_mps : currentParams[3],
+        index === 4 ? speed_mps : currentParams[4]
+      ])
     },
-    [currentDistanceToMouth, minDistanceToMouth, maxDistanceToMouth]
+    [minLinearSpeed, maxLinearSpeed]
   )
 
   // Callback to render the main contents of the page
   const distanceToMouthId = useId()
+  const moveToMouthSpeedId = useId()
+  const moveToMouthSpeedNearMouthId = useId()
+  const moveFromMouthSpeedId = useId()
+  const moveFromMouthSpeedNearMouthId = useId()
+  const speedParameterIdsAndDescriptions = useMemo(
+    () => [
+      [moveToMouthSpeedId, 'Approach Speed (cm/s)'],
+      [moveToMouthSpeedNearMouthId, 'Approach Speed Near Mouth (cm/s)'],
+      [moveFromMouthSpeedId, 'Retreat Speed (cm/s)'],
+      [moveFromMouthSpeedNearMouthId, 'Retreat Speed Near Mouth (cm/s)']
+    ],
+    [moveToMouthSpeedId, moveToMouthSpeedNearMouthId, moveFromMouthSpeedId, moveFromMouthSpeedNearMouthId]
+  )
   const renderBiteTransferSettings = useCallback(() => {
-    if (currentDistanceToMouth[0] === null) {
+    if (currentParams.some((param) => param === null)) {
       return (
         <>
           <View
@@ -225,7 +284,7 @@ const BiteTransfer = (props) => {
               Distance To Mouth (cm)
             </Label>
             <SpinButton
-              value={currentDistanceToMouth[0][0] * 100}
+              value={currentParams[0][0] * 100}
               id={distanceToMouthId}
               step={0.5}
               onChange={onDistanceToMouthChange}
@@ -241,6 +300,38 @@ const BiteTransfer = (props) => {
                 size: 'large'
               }}
             />
+            {speedParameterIdsAndDescriptions.map(([id, description], index) => (
+              <>
+                <Label
+                  htmlFor={id}
+                  style={{
+                    fontSize: textFontSize,
+                    width: '90%',
+                    color: 'black',
+                    textAlign: 'center'
+                  }}
+                >
+                  {description}
+                </Label>
+                <SpinButton
+                  value={currentParams[index + 1] * 100}
+                  id={id}
+                  step={1.0}
+                  onChange={(_ev, data) => onSpeedChange(_ev, data, index + 1)}
+                  appearance='filled-lighter'
+                  style={{
+                    fontSize: textFontSize,
+                    width: '90%',
+                    color: 'black'
+                  }}
+                  incrementButton={{
+                    'aria-label': 'Increase value by 1.0',
+                    'aria-roledescription': 'spinner',
+                    size: 'large'
+                  }}
+                />
+              </>
+            ))}
           </View>
           <View
             style={{
@@ -321,9 +412,11 @@ const BiteTransfer = (props) => {
   }, [
     dimension,
     textFontSize,
-    currentDistanceToMouth,
+    currentParams,
     onDistanceToMouthChange,
+    onSpeedChange,
     distanceToMouthId,
+    speedParameterIdsAndDescriptions,
     moveToMouthButtonClicked,
     moveAwayFromMouthButtonClicked
   ])
@@ -368,8 +461,8 @@ const BiteTransfer = (props) => {
       modalOnHide={() => setLocalCurrMealStateWrapper(null)}
       modalChildren={renderModalBody()}
       paramNames={paramNames}
-      localParamValues={currentDistanceToMouth}
-      setLocalParamValues={setCurrentDistanceToMouth}
+      localParamValues={currentParams}
+      setLocalParamValues={setCurrentParams}
     >
       {renderBiteTransferSettings()}
     </SettingsPageParent>

feedingwebapp/src/Pages/Settings/BiteTransfer.jsx

@@ -20,7 +20,8 @@ import {
   MOVING_STATE_ICON_DICT,
   REGULAR_CONTAINER_ID,
   SET_PARAMETERS_SERVICE_NAME,
-  SET_PARAMETERS_SERVICE_TYPE
+  SET_PARAMETERS_SERVICE_TYPE,
+  TABLE_ICON
 } from '../Constants'
 
 /**
@@ -163,9 +164,9 @@ const Main = () => {
   // Configure the different options in the settings menu
   let settingsConfig = [
     {
-      title: 'Distance to Mouth',
+      title: 'Motion to/from Mouth',
       icon: moveToMouthConfigurationImage,
-      onClick: () => onClickSettingsPage(SETTINGS_STATE.DISTANCE_TO_MOUTH)
+      onClick: () => onClickSettingsPage(SETTINGS_STATE.BITE_TRANSFER)
     },
     {
       title: 'Above Plate',
@@ -186,6 +187,11 @@ const Main = () => {
       title: 'Stow Position',
       icon: moveToStowConfigurationImage,
       onClick: () => onClickSettingsPage(SETTINGS_STATE.STOW_CONFIGURATION)
+    },
+    {
+      title: 'Planning Scene',
+      icon: TABLE_ICON,
+      onClick: () => onClickSettingsPage(SETTINGS_STATE.PLANNING_SCENE)
     }
   ]