diff --git a/feedingwebapp/src/Pages/Constants.js b/feedingwebapp/src/Pages/Constants.js index a847c94..0d07b80 100644 --- a/feedingwebapp/src/Pages/Constants.js +++ b/feedingwebapp/src/Pages/Constants.js @@ -87,10 +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' +export const ACTIVATE_CONTROLLER_ACTION_NAME = 'ActivateController' +export const ACTIVATE_CONTROLLER_ACTION_TYPE = 'ada_feeding_msgs/action/ActivateController' +export const CARTESIAN_CONTROLLER_NAME = 'jaco_arm_cartesian_controller' +export const JOINT_CONTROLLER_NAME = 'jaco_arm_servo_controller' export const RECOMPUTE_WORKSPACE_WALLS_ACTION_NAME = 'recompute_workspace_walls' export const RECOMPUTE_WORKSPACE_WALLS_ACTION_TYPE = 'ada_feeding_msgs/action/Trigger' @@ -139,6 +139,11 @@ export const RESTING_PARAM_JOINTS_1 = 'AcquireFood.tree_kwargs.resting_joint_pos // TODO: We may need to remove the orientation constraint from the below action. export const RESTING_PARAM_JOINTS_2 = 'MoveToRestingPosition.tree_kwargs.goal_configuration' +// Parameters for modifying the force threshold +export const FORCE_THRESHOLD_PARAM = 'wrench_threshold.fMag' +export const DEFAULT_FORCE_THRESHOLD = 1.0 // N +export const INCREASED_FORCE_THRESHOLD = 75.0 // N + // Robot link names export const ROBOT_BASE_LINK = 'j2n6s200_link_base' export const ROBOT_END_EFFECTOR = 'forkTip' diff --git a/feedingwebapp/src/Pages/Header/InfoModal.jsx b/feedingwebapp/src/Pages/Header/InfoModal.jsx index 377d842..ea494a6 100644 --- a/feedingwebapp/src/Pages/Header/InfoModal.jsx +++ b/feedingwebapp/src/Pages/Header/InfoModal.jsx @@ -94,7 +94,7 @@ function InfoModal(props) { ) : mode === TELEOP_MODE ? ( - + ) : mode === SYSTEM_STATUS_MODE ? (
System Status
) : ( diff --git a/feedingwebapp/src/Pages/Header/TeleopSubcomponent.jsx b/feedingwebapp/src/Pages/Header/TeleopSubcomponent.jsx index 132f9ef..2a6b22c 100644 --- a/feedingwebapp/src/Pages/Header/TeleopSubcomponent.jsx +++ b/feedingwebapp/src/Pages/Header/TeleopSubcomponent.jsx @@ -7,19 +7,33 @@ import PropTypes from 'prop-types' import { View } from 'react-native' // Local Imports -import { useROS, createROSTopic, createROSMessage, createROSActionClient, callROSAction, destroyActionClient } from '../../ros/ros_helpers' +import { + useROS, + createROSTopic, + createROSMessage, + createROSActionClient, + callROSAction, + destroyActionClient, + createROSService, + createROSServiceRequest, + getParameterFromValue +} from '../../ros/ros_helpers' import '../Home/Home.css' import { + CARTESIAN_CONTROLLER_NAME, + JOINT_CONTROLLER_NAME, ROBOT_BASE_LINK, ROBOT_JOINTS, SERVO_CARTESIAN_TOPIC, SERVO_CARTESIAN_TOPIC_MSG, SERVO_JOINT_TOPIC, SERVO_JOINT_TOPIC_MSG, - START_CARTESIAN_CONTROLLER_ACTION_NAME, - START_CARTESIAN_CONTROLLER_ACTION_TYPE, - START_JOINT_CONTROLLER_ACTION_NAME, - START_JOINT_CONTROLLER_ACTION_TYPE + ACTIVATE_CONTROLLER_ACTION_NAME, + ACTIVATE_CONTROLLER_ACTION_TYPE, + SET_PARAMETERS_SERVICE_NAME, + INCREASED_FORCE_THRESHOLD, + DEFAULT_FORCE_THRESHOLD, + FORCE_THRESHOLD_PARAM } from '../Constants' import { useGlobalState } from '../GlobalState' import HoldButton from '../../buttons/HoldButton' @@ -122,11 +136,17 @@ const TeleopSubcomponent = (props) => { /** * Create the ROS Action Client to start the teleop controllers. */ - let startCartesianControllerAction = useMemo(() => { - return createROSActionClient(ros.current, START_CARTESIAN_CONTROLLER_ACTION_NAME, START_CARTESIAN_CONTROLLER_ACTION_TYPE) - }, []) - let startJointControllerAction = useMemo(() => { - return createROSActionClient(ros.current, START_JOINT_CONTROLLER_ACTION_NAME, START_JOINT_CONTROLLER_ACTION_TYPE) + let activateControllerActionGoal = useMemo( + () => + createROSMessage({ + controller_to_activate: teleopMode === JOINT_MODE ? JOINT_CONTROLLER_NAME : CARTESIAN_CONTROLLER_NAME, + re_tare: true + }), + [teleopMode] + ) + + let activateControllerAction = useMemo(() => { + return createROSActionClient(ros.current, ACTIVATE_CONTROLLER_ACTION_NAME, ACTIVATE_CONTROLLER_ACTION_TYPE) }, []) /** @@ -144,9 +164,9 @@ const TeleopSubcomponent = (props) => { */ useEffect(() => { console.log('Starting controller', refreshCount) - let action = teleopMode === JOINT_MODE ? startJointControllerAction : startCartesianControllerAction - callROSAction(action, createROSMessage({}), null, null) - }, [refreshCount, startCartesianControllerAction, startJointControllerAction, teleopMode]) + let action = activateControllerAction + callROSAction(action, activateControllerActionGoal, null, null) + }, [refreshCount, activateControllerAction, activateControllerActionGoal]) /** * When the component is unmounted, stop servo. @@ -155,11 +175,10 @@ const TeleopSubcomponent = (props) => { let unmountCallback = props.unmountCallback return () => { console.log('Unmounting teleop subcomponent.') - destroyActionClient(startCartesianControllerAction) - destroyActionClient(startJointControllerAction) + destroyActionClient(activateControllerAction) unmountCallback.current() } - }, [startCartesianControllerAction, startJointControllerAction, props.unmountCallback]) + }, [activateControllerAction, props.unmountCallback]) /** * Callback function to publish constant cartesian cartesian velocity commands. @@ -534,6 +553,38 @@ const TeleopSubcomponent = (props) => { }, [teleopMode, setTeleopMode, textFontSize, sizeSuffix] ) + + /** + * Service and callback to increase the force threshold. Additionally, before + * unmounting, the force threshold should be reset. + */ + const [forceThresholdIsIncreased, setForceThresholdIsIncreased] = useState(false) + let changeForceThresholdService = useMemo(() => { + let activeController = teleopMode === JOINT_MODE ? JOINT_CONTROLLER_NAME : CARTESIAN_CONTROLLER_NAME + return createROSService(ros.current, activeController + '/set_parameters_atomically', SET_PARAMETERS_SERVICE_NAME) + }, [ros, teleopMode]) + const setForceThreshold = useCallback( + (threshold) => { + let service = changeForceThresholdService + let request = createROSServiceRequest({ + parameters: [{ name: FORCE_THRESHOLD_PARAM, value: getParameterFromValue(threshold, 3) }] + }) + console.log('Calling setForceThreshold with request', request, 'for service', service.name) + service.callService(request, (response) => { + console.log('For setForceThreshold request', request, 'received response', response) + setForceThresholdIsIncreased(threshold > DEFAULT_FORCE_THRESHOLD) + }) + }, + [changeForceThresholdService, setForceThresholdIsIncreased] + ) + useEffect(() => { + return () => { + if (props.allowIncreasingForceThreshold) { + setForceThreshold(DEFAULT_FORCE_THRESHOLD) + } + } + }, [props.allowIncreasingForceThreshold, setForceThreshold]) + // Render the component return ( { { {/* Allow users to tune to speed of the current teleop mode */} { }} /> + {/* If the props specify, show a button to increase the force threshold. */} + {props.allowIncreasingForceThreshold ? ( + + {forceThresholdIsIncreased ? ( + + ) : ( + + )} + + ) : ( + <> + )} {/* Render the controls for the mode */} {} }, - teleopButtonOnReleaseCallback: () => {} + teleopButtonOnReleaseCallback: () => {}, + allowIncreasingForceThreshold: false } export default TeleopSubcomponent diff --git a/feedingwebapp/src/ros/ros_helpers.js b/feedingwebapp/src/ros/ros_helpers.js index 4b2fd4f..037bc2b 100644 --- a/feedingwebapp/src/ros/ros_helpers.js +++ b/feedingwebapp/src/ros/ros_helpers.js @@ -226,12 +226,12 @@ export function getParameterFromValue(value, typeOverride = null) { if (typeOverride === 1 || typeof value === 'boolean') { parameter.bool_value = value parameter.type = 1 - } else if (typeOverride === 2 || Number.isInteger(value)) { - parameter.integer_value = value - parameter.type = 2 } else if (typeOverride === 3 || typeof value === 'number') { parameter.double_value = value parameter.type = 3 + } else if (typeOverride === 2 || Number.isInteger(value)) { + parameter.integer_value = value + parameter.type = 2 } else if (typeOverride === 4 || typeof value === 'string') { parameter.string_value = value parameter.type = 4 @@ -241,12 +241,12 @@ export function getParameterFromValue(value, typeOverride = null) { } else if (typeOverride === 6 || (Array.isArray(value) && value.length > 0 && typeof value[0] === 'boolean')) { parameter.bool_array_value = value parameter.type = 6 - } else if (typeOverride === 7 || (Array.isArray(value) && value.length > 0 && Number.isInteger(value[0]))) { - parameter.integer_array_value = value - parameter.type = 7 } else if (typeOverride === 8 || (Array.isArray(value) && value.length > 0 && typeof value[0] === 'number')) { parameter.double_array_value = value parameter.type = 8 + } else if (typeOverride === 7 || (Array.isArray(value) && value.length > 0 && Number.isInteger(value[0]))) { + parameter.integer_array_value = value + parameter.type = 7 } else if (typeOverride === 9 || (Array.isArray(value) && value.length > 0 && typeof value[0] === 'string')) { parameter.string_array_value = value parameter.type = 9