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