Sim testing done, need to do real testing

feedingwebapp/src/Pages/Constants.js

@@ -123,7 +123,15 @@ 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 = 'MoveAbovePlate.tree_kwargs.joint_positions'
+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'

feedingwebapp/src/Pages/Header/TeleopSubcomponent.jsx

@@ -1,5 +1,6 @@
 // React Imports
 import React, { useCallback, useEffect, useMemo, useRef, useState } from 'react'
+import Button from 'react-bootstrap/Button'
 import { useId, Label, SpinButton } from '@fluentui/react-components'
 import { useMediaQuery } from 'react-responsive'
 import PropTypes from 'prop-types'
@@ -33,6 +34,7 @@ const TeleopSubcomponent = (props) => {
   const CARTESIAN_ANGULAR_MODE = 'Rotate'
   const JOINT_MODE = 'Joints'
   const [teleopMode, setTeleopMode] = useState(CARTESIAN_LINEAR_MODE)
+  const [refreshCount, setRefreshCount] = useState(0)
   const speedSpinButtonID = useId()
 
   // Cartesian teleop speeds
@@ -137,7 +139,7 @@ const TeleopSubcomponent = (props) => {
    * unmounted.
    */
   useEffect(() => {
-    console.log()
+    console.log('Starting servo', refreshCount)
     callROSAction(startServoAction, createROSMessage({}))
     let stopServoSuccessCallback = props.stopServoSuccessCallback
     return () => {
@@ -149,7 +151,7 @@ const TeleopSubcomponent = (props) => {
         stopServoSuccessCallback.current()
       })
     }
-  }, [startServoAction, stopServoAction, props.stopServoSuccessCallback])
+  }, [refreshCount, startServoAction, stopServoAction, props.stopServoSuccessCallback])
 
   /**
    * Callback function to publish constant cartesian cartesian velocity commands.
@@ -557,6 +559,20 @@ const TeleopSubcomponent = (props) => {
             width: '100%'
           }}
         >
+          <Button
+            variant='secondary'
+            className='mx-2 mb-2 btn-huge'
+            size='lg'
+            style={{
+              fontSize: textFontSize,
+              paddingTop: 0,
+              paddingBottom: 0,
+              margin: '0 !important'
+            }}
+            onClick={() => setRefreshCount(refreshCount + 1)}
+          >
+            &#10227;
+          </Button>
           <Label
             htmlFor={speedSpinButtonID}
             style={{

feedingwebapp/src/Pages/Settings/AbovePlate.jsx

@@ -8,7 +8,7 @@ import { View } from 'react-native'
 // Local imports
 import { useROS, createROSService, createROSServiceRequest } from '../../ros/ros_helpers'
 import {
-  ABOVE_PLATE_PARAM,
+  ABOVE_PLATE_PARAM_JOINTS,
   CAMERA_FEED_TOPIC,
   getRobotMotionText,
   GET_JOINT_STATE_SERVICE_NAME,
@@ -35,7 +35,7 @@ const AbovePlate = (props) => {
 
   // Create relevant local state variables
   // Configure the parameters for SettingsPageParent
-  const paramNames = useMemo(() => [ABOVE_PLATE_PARAM], [])
+  const paramNames = useMemo(() => [ABOVE_PLATE_PARAM_JOINTS], [])
   const [currentAbovePlateParam, setCurrentAbovePlateParam] = useState([null])
   const [localCurrAndNextMealState, setLocalCurrAndNextMealState] = useState(
     globalMealState === MEAL_STATE.U_BiteDone || biteTransferPageAtFace
@@ -44,7 +44,6 @@ const AbovePlate = (props) => {
   )
   const actionInput = useMemo(() => ({}), [])
   const [doneButtonIsClicked, setDoneButtonIsClicked] = useState(false)
-  const [robotIsAbovePlate, setRobotIsAbovePlate] = useState(true)
   const [zoomLevel, setZoomLevel] = useState(1.0)
   const [mountTeleopSubcomponent, setMountTeleopSubcomponent] = useState(false)
   const stopServoSuccessCallback = useRef(() => {})
@@ -61,13 +60,12 @@ const AbovePlate = (props) => {
   // Update other state variables that are related to the local meal state
   const setLocalCurrMealStateWrapper = useCallback(
     (newLocalCurrMealState, newLocalNextMealState = null) => {
-      if (newLocalCurrMealState === null) {
-        setMountTeleopSubcomponent(true)
-      }
       console.log('setLocalCurrMealStateWrapper evaluated')
       let oldLocalCurrMealState = localCurrAndNextMealState[0]
-      // If the oldlocalCurrMealState was R_MovingToMouth, then the robot is at the mouth
-      setRobotIsAbovePlate(oldLocalCurrMealState === MEAL_STATE.R_MovingAbovePlate)
+      // Only mount the teleop subcomponent if the robot finished moving above the plate
+      if (newLocalCurrMealState === null && oldLocalCurrMealState === MEAL_STATE.R_MovingAbovePlate) {
+        setMountTeleopSubcomponent(true)
+      }
       // Start in a moving state, not a paused state
       setPaused(false)
       if (newLocalCurrMealState === null && doneButtonIsClicked) {
@@ -79,15 +77,7 @@ const AbovePlate = (props) => {
         setLocalCurrAndNextMealState([newLocalCurrMealState, newLocalNextMealState])
       }
     },
-    [
-      localCurrAndNextMealState,
-      setLocalCurrAndNextMealState,
-      setMountTeleopSubcomponent,
-      setRobotIsAbovePlate,
-      doneButtonIsClicked,
-      setPaused,
-      setSettingsState
-    ]
+    [localCurrAndNextMealState, setLocalCurrAndNextMealState, setMountTeleopSubcomponent, doneButtonIsClicked, setPaused, setSettingsState]
   )
 
   // Get the function that sets the local curr meal state and next meal state variables.
@@ -98,6 +88,7 @@ const AbovePlate = (props) => {
       let retval = () => {
         console.log('getSetLocalCurrMealStateWrapper retval evaluated')
         setLocalCurrMealStateWrapper(newLocalCurrMealState, newLocalNextMealState)
+        stopServoSuccessCallback.current = () => {}
       }
       // If the teleop subcomponent was mounted, unmount it and let the stopServo
       // success callback handle the rest. However, sometimes the success message
@@ -110,7 +101,7 @@ const AbovePlate = (props) => {
       }
       return retval
     },
-    [mountTeleopSubcomponent, setLocalCurrMealStateWrapper, setMountTeleopSubcomponent]
+    [mountTeleopSubcomponent, setLocalCurrMealStateWrapper, setMountTeleopSubcomponent, stopServoSuccessCallback]
   )
 
   // Store the props for the RobotMotion call.
@@ -152,35 +143,24 @@ const AbovePlate = (props) => {
   // Get the current joint states and store them as the above plate param
   const storeJointStatesAsLocalParam = useCallback(() => {
     console.log('storeJointStatesAsLocalParam called')
-    if (robotIsAbovePlate) {
-      let service = getJointStateService.current
-      let request = createROSServiceRequest({
-        joint_names: ROBOT_JOINTS
-      })
-      service.callService(request, (response) => {
-        console.log('Got joint state response', response)
-        setCurrentAbovePlateParam([response.joint_state.position])
-      })
-    }
-  }, [getJointStateService, robotIsAbovePlate, setCurrentAbovePlateParam])
-
-  // Callback to move the robot to the staging configuration
-  const moveToStagingButtonClicked = useCallback(() => {
-    setDoneButtonIsClicked(false)
-    stopServoSuccessCallback.current = getSetLocalCurrMealStateWrapper(MEAL_STATE.R_MovingToStagingConfiguration)
-  }, [getSetLocalCurrMealStateWrapper, setDoneButtonIsClicked, stopServoSuccessCallback])
+    let service = getJointStateService.current
+    let request = createROSServiceRequest({
+      joint_names: ROBOT_JOINTS
+    })
+    service.callService(request, (response) => {
+      console.log('Got joint state response', response)
+      setCurrentAbovePlateParam([response.joint_state.position])
+    })
+  }, [getJointStateService, setCurrentAbovePlateParam])
 
-  // Callback to move the robot to the resting configuration
-  const moveToRestingButtonClicked = useCallback(() => {
-    setDoneButtonIsClicked(false)
-    stopServoSuccessCallback.current = getSetLocalCurrMealStateWrapper(MEAL_STATE.R_MovingToRestingPosition)
-  }, [getSetLocalCurrMealStateWrapper, setDoneButtonIsClicked, stopServoSuccessCallback])
-
-  // Callback to move the robot above the plate
-  const moveAbovePlateButtonClicked = useCallback(() => {
-    setDoneButtonIsClicked(false)
-    stopServoSuccessCallback.current = getSetLocalCurrMealStateWrapper(MEAL_STATE.R_MovingAbovePlate)
-  }, [getSetLocalCurrMealStateWrapper, setDoneButtonIsClicked, stopServoSuccessCallback])
+  // Callback to move the robot to another configuration
+  const moveToButtonClicked = useCallback(
+    (nextMealState) => {
+      setDoneButtonIsClicked(false)
+      stopServoSuccessCallback.current = getSetLocalCurrMealStateWrapper(nextMealState)
+    },
+    [getSetLocalCurrMealStateWrapper, setDoneButtonIsClicked, stopServoSuccessCallback]
+  )
 
   // Callback to return to the main settings page
   const doneButtonClicked = useCallback(() => {
@@ -196,19 +176,19 @@ const AbovePlate = (props) => {
         break
       case MEAL_STATE.U_PreMeal:
       case MEAL_STATE.U_BiteSelection:
-        localNextMealState = MEAL_STATE.R_MovingAbovePlate
+        localCurrMealState = MEAL_STATE.R_MovingAbovePlate
         break
       case MEAL_STATE.U_BiteAcquisitionCheck:
-        localNextMealState = MEAL_STATE.R_MovingToRestingPosition
+        localCurrMealState = MEAL_STATE.R_MovingToRestingPosition
         break
       case MEAL_STATE.R_DetectingFace:
-        localNextMealState = MEAL_STATE.R_MovingToStagingConfiguration
+        localCurrMealState = MEAL_STATE.R_MovingToStagingConfiguration
         break
       case MEAL_STATE.U_PostMeal:
-        localNextMealState = MEAL_STATE.R_StowingArm
+        localCurrMealState = MEAL_STATE.R_StowingArm
         break
       default:
-        localNextMealState = MEAL_STATE.R_MovingAbovePlate
+        localCurrMealState = MEAL_STATE.R_MovingAbovePlate
         break
     }
     stopServoSuccessCallback.current = getSetLocalCurrMealStateWrapper(localCurrMealState, localNextMealState)
@@ -242,13 +222,15 @@ const AbovePlate = (props) => {
               </View>
             </>
           ) : (
-            <></>
+            <p style={{ textAlign: 'center', fontSize: (textFontSize * 0.75).toString() + sizeSuffix, margin: 0 }} className='txt-huge'>
+              To tune the &ldquo;Above Plate&rdquo; configuration, first &ldquo;Move Above Plate.&rdquo;
+            </p>
           )}
         </View>
         <View style={{ flex: 1, alignItems: 'center', justifyContent: 'center', width: '100%', height: '100%' }}></View>
         <View
           style={{
-            flex: 4,
+            flex: 3,
             flexDirection: otherDimension,
             alignItems: 'center',
             justifyContent: 'center',
@@ -267,7 +249,7 @@ const AbovePlate = (props) => {
                 height: '90%',
                 color: 'black'
               }}
-              onClick={moveToStagingButtonClicked}
+              onClick={() => moveToButtonClicked(MEAL_STATE.R_MovingToStagingConfiguration)}
             >
               Move To Staging
             </Button>
@@ -283,7 +265,7 @@ const AbovePlate = (props) => {
                 height: '90%',
                 color: 'black'
               }}
-              onClick={moveToRestingButtonClicked}
+              onClick={() => moveToButtonClicked(MEAL_STATE.R_MovingToRestingPosition)}
             >
               Move To Resting
             </Button>
@@ -299,7 +281,7 @@ const AbovePlate = (props) => {
                 height: '90%',
                 color: 'black'
               }}
-              onClick={moveAbovePlateButtonClicked}
+              onClick={() => moveToButtonClicked(MEAL_STATE.R_MovingAbovePlate)}
             >
               Move Above Plate
             </Button>
@@ -315,9 +297,7 @@ const AbovePlate = (props) => {
     zoomLevel,
     props.webrtcURL,
     mountTeleopSubcomponent,
-    moveToStagingButtonClicked,
-    moveToRestingButtonClicked,
-    moveAbovePlateButtonClicked,
+    moveToButtonClicked,
     stopServoSuccessCallback,
     storeJointStatesAsLocalParam
   ])
@@ -366,7 +346,7 @@ const AbovePlate = (props) => {
       paramNames={paramNames}
       localParamValues={currentAbovePlateParam}
       setLocalParamValues={setCurrentAbovePlateParam}
-      resetToPresetSuccessCallback={moveAbovePlateButtonClicked}
+      resetToPresetSuccessCallback={() => moveToButtonClicked(MEAL_STATE.R_MovingAbovePlate)}
     >
       {renderAbovePlateSettings()}
     </SettingsPageParent>

feedingwebapp/src/Pages/Settings/SettingsPageParent.jsx

@@ -1,5 +1,5 @@
 // React imports
-import React, { useCallback, useEffect, useRef, useState } from 'react'
+import React, { useCallback, useEffect, useRef } from 'react'
 import { useMediaQuery } from 'react-responsive'
 import Button from 'react-bootstrap/Button'
 import Dropdown from 'react-bootstrap/Dropdown'
@@ -29,7 +29,7 @@ const SettingsPageParent = (props) => {
   const settingsPresets = useGlobalState((state) => state.settingsPresets)
 
   // Get relevant local state variables
-  const [isResettingToPreset, setIsResettingToPreset] = useState(false)
+  const isResettingToPreset = useRef(false)
 
   // Flag to check if the current orientation is portrait
   const isPortrait = useMediaQuery({ query: '(orientation: portrait)' })
@@ -126,10 +126,10 @@ const SettingsPageParent = (props) => {
     service.callService(currentRequest, (response) => {
       console.log('For request', currentRequest, 'received SetParameter response', response)
       // If this is wrapping up a reset to preset, call the callback
-      if (isResettingToPreset) {
+      if (isResettingToPreset.current) {
         let resetToPresetSuccessCallback = props.resetToPresetSuccessCallback
         resetToPresetSuccessCallback()
-        setIsResettingToPreset(false)
+        isResettingToPreset.current = false
       }
     })
   }, [
@@ -143,22 +143,23 @@ const SettingsPageParent = (props) => {
 
   /**
    * Every time the local parameter values change, update the global parameter.
+   * The above frequency holds true because setGlobalParameter only changes when
+   * props.localParamValues changes -- everything else should be constant.
    */
   useEffect(() => {
-    console.log('Local parameter values changed', props.localParamValues)
     setGlobalParameter()
-  }, [props.localParamValues, setGlobalParameter])
+  }, [setGlobalParameter])
 
   /**
    * A callback for when the user asks to reset parameters to a preset.
    */
   const resetToPreset = useCallback(
     (preset) => {
       console.log('Resetting parameters to preset', preset)
-      setIsResettingToPreset(true)
+      isResettingToPreset.current = true
       setLocalParametersToGlobalValues(preset)
     },
-    [setLocalParametersToGlobalValues, setIsResettingToPreset]
+    [setLocalParametersToGlobalValues, isResettingToPreset]
   )
 
   return (