diff --git a/feedingwebapp/public/robot_state_imgs/table.svg b/feedingwebapp/public/robot_state_imgs/table.svg new file mode 100644 index 00000000..c9818f40 --- /dev/null +++ b/feedingwebapp/public/robot_state_imgs/table.svg @@ -0,0 +1,13 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/feedingwebapp/src/Pages/Constants.js b/feedingwebapp/src/Pages/Constants.js index 0bf1cd4f..88cc6bac 100644 --- a/feedingwebapp/src/Pages/Constants.js +++ b/feedingwebapp/src/Pages/Constants.js @@ -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. diff --git a/feedingwebapp/src/Pages/GlobalState.jsx b/feedingwebapp/src/Pages/GlobalState.jsx index 45308a71..c530b38d 100644 --- a/feedingwebapp/src/Pages/GlobalState.jsx +++ b/feedingwebapp/src/Pages/GlobalState.jsx @@ -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 diff --git a/feedingwebapp/src/Pages/Settings/BiteTransfer.jsx b/feedingwebapp/src/Pages/Settings/BiteTransfer.jsx index e0c3d249..bcfba22e 100644 --- a/feedingwebapp/src/Pages/Settings/BiteTransfer.jsx +++ b/feedingwebapp/src/Pages/Settings/BiteTransfer.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 ( <> { Distance To Mouth (cm) { size: 'large' }} /> + {speedParameterIdsAndDescriptions.map(([id, description], index) => ( + <> + + 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' + }} + /> + + ))} { }, [ 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()} diff --git a/feedingwebapp/src/Pages/Settings/Main.jsx b/feedingwebapp/src/Pages/Settings/Main.jsx index 87cbbc94..a524163c 100644 --- a/feedingwebapp/src/Pages/Settings/Main.jsx +++ b/feedingwebapp/src/Pages/Settings/Main.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) } ] diff --git a/feedingwebapp/src/Pages/Settings/PlanningScene.jsx b/feedingwebapp/src/Pages/Settings/PlanningScene.jsx new file mode 100644 index 00000000..5e458e7a --- /dev/null +++ b/feedingwebapp/src/Pages/Settings/PlanningScene.jsx @@ -0,0 +1,131 @@ +// React imports +import React, { useCallback, useEffect, useMemo, useRef, useState } from 'react' +import ButtonGroup from 'react-bootstrap/ButtonGroup' +import Dropdown from 'react-bootstrap/Dropdown' +import DropdownButton from 'react-bootstrap/DropdownButton' +import { View } from 'react-native' + +// Local imports +import { PLANNING_SCENE_PARAM, PLANNING_SCENE_GET_PARAMETERS_SERVICE_NAME, PLANNING_SCENE_GET_PARAMETERS_SERVICE_TYPE } from '../Constants' +import { useGlobalState, SETTINGS_STATE } from '../GlobalState' +import { useROS, createROSService, createROSServiceRequest, getValueFromParameter } from '../../ros/ros_helpers' +import SettingsPageParent from './SettingsPageParent' + +/** + * The PlanningScene component allows users to change the planning scene that a particular + * settings namespace is configured with + */ +const PlanningScene = () => { + // Get relevant global state variables + const setSettingsState = useGlobalState((state) => state.setSettingsState) + + // Rendering variables + let textFontSize = '3.5vh' + + // Configure the parameters for SettingsPageParent + const paramNames = useMemo(() => [PLANNING_SCENE_PARAM], []) + const [currentParams, setCurrentParams] = useState([null]) + + /** + * Connect to ROS, if not already connected. Put this in useRef to avoid + * re-connecting upon re-renders. + */ + const ros = useRef(useROS().ros) + + // Get the options for the planning scene + let getParametersService = useRef( + createROSService(ros.current, PLANNING_SCENE_GET_PARAMETERS_SERVICE_NAME, PLANNING_SCENE_GET_PARAMETERS_SERVICE_TYPE) + ) + const [planningSceneNamespaces, setPlanningSceneNamespaces] = useState([]) + const getPlanningSceneNamespaces = useCallback(() => { + let service = getParametersService.current + let request = createROSServiceRequest({ + names: ['namespaces'] + }) + console.log('PlanningScene: Requesting planning scene namespaces', service, request) + service.callService(request, (response) => { + console.log('PlanningScene: Received planning scene namespaces', request, response) + if (response.values.length > 0 && response.values[0].type === 9) { + setPlanningSceneNamespaces(getValueFromParameter(response.values[0])) + } else { + console.error('PlanningScene: Error getting planning scene namespaces') + } + }) + }, [getParametersService, setPlanningSceneNamespaces]) + useEffect(() => { + getPlanningSceneNamespaces() + }, [getPlanningSceneNamespaces]) + + // Render the settings for the planning scene + const renderPlanningSceneSettings = useCallback(() => { + if (currentParams.some((param) => param === null)) { + return ( + <> + +
Loading...
+ + + ) + } else { + return ( + +
Select Planning Scene:
+ { + if (planningSceneNamespaces.length === 0) { + getPlanningSceneNamespaces() + } + }} + > + {planningSceneNamespaces.map((namespace) => ( + setCurrentParams([namespace])} active={namespace === currentParams[0]}> + {namespace} + + ))} + + + ) + } + }, [currentParams, planningSceneNamespaces, getPlanningSceneNamespaces, setCurrentParams, textFontSize]) + + return ( + setSettingsState(SETTINGS_STATE.MAIN)} + modalShow={false} + modalOnHide={null} + modalChildren={null} + paramNames={paramNames} + localParamValues={currentParams} + setLocalParamValues={setCurrentParams} + > + {renderPlanningSceneSettings()} + + ) +} + +export default PlanningScene diff --git a/feedingwebapp/src/Pages/Settings/Settings.jsx b/feedingwebapp/src/Pages/Settings/Settings.jsx index 3bdec420..dcf30b1b 100644 --- a/feedingwebapp/src/Pages/Settings/Settings.jsx +++ b/feedingwebapp/src/Pages/Settings/Settings.jsx @@ -22,6 +22,7 @@ import { STAGING_PARAM_POSITION, STOW_PARAM_JOINTS } from '../Constants' +import PlanningScene from './PlanningScene' /** * The Settings components displays the appropriate settings page based on the @@ -42,7 +43,7 @@ const Settings = (props) => { switch (settingsState) { case SETTINGS_STATE.MAIN: return
- case SETTINGS_STATE.DISTANCE_TO_MOUTH: + case SETTINGS_STATE.BITE_TRANSFER: return case SETTINGS_STATE.ABOVE_PLATE: return ( @@ -138,6 +139,8 @@ const Settings = (props) => { webrtcURL={props.webrtcURL} /> ) + case SETTINGS_STATE.PLANNING_SCENE: + return default: console.log('Invalid settings state', settingsState) return