Skip to content

Commit

Permalink
Allow TeleopSubcomponent to re-tare and change F/T threshold (#140)
Browse files Browse the repository at this point in the history
* re-tare F/T sensor on teleop (un)mount and refresh

* Allow increasing F/T threshold from InfoModal

* Improve layout
  • Loading branch information
amalnanavati authored May 13, 2024
1 parent cde96aa commit 2dc2dfe
Show file tree
Hide file tree
Showing 4 changed files with 141 additions and 34 deletions.
13 changes: 9 additions & 4 deletions feedingwebapp/src/Pages/Constants.js
Original file line number Diff line number Diff line change
Expand Up @@ -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'

Expand Down Expand Up @@ -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'
Expand Down
6 changes: 3 additions & 3 deletions feedingwebapp/src/Pages/Header/InfoModal.jsx
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ function InfoModal(props) {
<ToastContainer style={{ fontSize: textFontSize }} containerId={MODAL_CONTAINER_ID} enableMultiContainer={true} />
<View
style={{
flex: 2,
flex: 3,
flexDirection: otherDirection,
justifyContent: 'center',
alignItems: 'center',
Expand All @@ -112,7 +112,7 @@ function InfoModal(props) {
</View>
<View
style={{
flex: 9,
flex: 18,
flexDirection: 'row',
justifyContent: 'center',
alignItems: 'center',
Expand All @@ -124,7 +124,7 @@ function InfoModal(props) {
{mode === VIDEO_MODE ? (
<VideoFeed topic={CAMERA_FEED_TOPIC} updateRateHz={10} webrtcURL={props.webrtcURL} />
) : mode === TELEOP_MODE ? (
<TeleopSubcomponent />
<TeleopSubcomponent allowIncreasingForceThreshold={true} />
) : mode === SYSTEM_STATUS_MODE ? (
<div>System Status</div>
) : (
Expand Down
144 changes: 123 additions & 21 deletions feedingwebapp/src/Pages/Header/TeleopSubcomponent.jsx
Original file line number Diff line number Diff line change
Expand Up @@ -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'
Expand Down Expand Up @@ -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)
}, [])

/**
Expand All @@ -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.
Expand All @@ -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.
Expand Down Expand Up @@ -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 (
<View
Expand Down Expand Up @@ -562,7 +613,7 @@ const TeleopSubcomponent = (props) => {
</View>
<View
style={{
flex: 9,
flex: 14,
justifyContent: 'center',
alignItems: 'center',
width: '100%',
Expand All @@ -572,7 +623,7 @@ const TeleopSubcomponent = (props) => {
{/* Allow users to tune to speed of the current teleop mode */}
<View
style={{
flex: 1,
flex: props.allowIncreasingForceThreshold ? 1 : 2,
flexDirection: 'row',
justifyContent: 'center',
alignItems: 'center',
Expand Down Expand Up @@ -628,10 +679,58 @@ const TeleopSubcomponent = (props) => {
}}
/>
</View>
{/* If the props specify, show a button to increase the force threshold. */}
{props.allowIncreasingForceThreshold ? (
<View
style={{
flex: 1,
flexDirection: 'row',
justifyContent: 'center',
alignItems: 'center',
width: '100%'
}}
>
{forceThresholdIsIncreased ? (
<Button
variant='warning'
className='mx-2 mb-2 btn-huge'
size='lg'
style={{
fontSize: (textFontSize * 1.0).toString() + sizeSuffix,
paddingTop: 0,
paddingBottom: 0,
margin: '0 !important',
width: '100%'
}}
onClick={() => setForceThreshold(DEFAULT_FORCE_THRESHOLD)}
>
&#x26E8; Restore Force Threshold
</Button>
) : (
<Button
variant='danger'
className='mx-2 mb-2 btn-huge'
size='lg'
style={{
fontSize: (textFontSize * 1.0).toString() + sizeSuffix,
paddingTop: 0,
paddingBottom: 0,
margin: '0 !important',
width: '100%'
}}
onClick={() => setForceThreshold(INCREASED_FORCE_THRESHOLD)}
>
&#9888; Increase Force Threshold
</Button>
)}
</View>
) : (
<></>
)}
{/* Render the controls for the mode */}
<View
style={{
flex: 5,
flex: 8,
flexDirection: 'column',
justifyContent: 'center',
alignItems: 'center',
Expand All @@ -652,10 +751,13 @@ TeleopSubcomponent.propTypes = {
// A reference to a function to be called if StopServo is succesfully run.
unmountCallback: PropTypes.object,
// A function to be called when one of the teleop buttons are released
teleopButtonOnReleaseCallback: PropTypes.func
teleopButtonOnReleaseCallback: PropTypes.func,
// Whether to allow the user to increase the force threshold
allowIncreasingForceThreshold: PropTypes.bool
}
TeleopSubcomponent.defaultProps = {
unmountCallback: { current: () => {} },
teleopButtonOnReleaseCallback: () => {}
teleopButtonOnReleaseCallback: () => {},
allowIncreasingForceThreshold: false
}
export default TeleopSubcomponent
12 changes: 6 additions & 6 deletions feedingwebapp/src/ros/ros_helpers.js
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down

0 comments on commit 2dc2dfe

Please sign in to comment.