Allow increasing F/T threshold from InfoModal

personalrobotics · May 13, 2024 · 6600333 · 6600333
1 parent d5cf3cc
commit 6600333
Show file tree

Hide file tree

Showing 4 changed files with 117 additions and 15 deletions.
diff --git a/feedingwebapp/src/Pages/Constants.js b/feedingwebapp/src/Pages/Constants.js
@@ -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
@@ -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',
@@ -112,7 +112,7 @@ function InfoModal(props) {
         </View>
         <View
           style={{
-            flex: 9,
+            flex: 18,
             flexDirection: 'row',
             justifyContent: 'center',
             alignItems: 'center',
@@ -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>
           ) : (

diff --git a/feedingwebapp/src/Pages/Header/TeleopSubcomponent.jsx b/feedingwebapp/src/Pages/Header/TeleopSubcomponent.jsx
@@ -7,7 +7,17 @@ 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,
@@ -19,7 +29,11 @@ import {
   SERVO_JOINT_TOPIC,
   SERVO_JOINT_TOPIC_MSG,
   ACTIVATE_CONTROLLER_ACTION_NAME,
-  ACTIVATE_CONTROLLER_ACTION_TYPE
+  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'
@@ -539,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
@@ -567,7 +613,7 @@ const TeleopSubcomponent = (props) => {
       </View>
       <View
         style={{
-          flex: 9,
+          flex: 14,
           justifyContent: 'center',
           alignItems: 'center',
           width: '100%',
@@ -633,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',
@@ -657,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
diff --git 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