From e7ca7ebb42294736179a327f328026c90dc602ee Mon Sep 17 00:00:00 2001
From: Amal Nanavati <amaln@uw.edu>
Date: Sun, 25 Aug 2024 15:22:01 -0700
Subject: [PATCH] Allow retaring the FT sensor from the teleop subcomponent
 (#142)

---
 feedingwebapp/src/Pages/Constants.js          |  2 +
 feedingwebapp/src/Pages/Header/InfoModal.jsx  |  2 +-
 .../src/Pages/Header/TeleopSubcomponent.jsx   | 58 +++++++++++++++++--
 3 files changed, 57 insertions(+), 5 deletions(-)

diff --git a/feedingwebapp/src/Pages/Constants.js b/feedingwebapp/src/Pages/Constants.js
index f526e93..cc7aae9 100644
--- a/feedingwebapp/src/Pages/Constants.js
+++ b/feedingwebapp/src/Pages/Constants.js
@@ -118,6 +118,8 @@ ROS_SERVICE_NAMES[MEAL_STATE.U_BiteAcquisitionCheck] = {
 export { ROS_SERVICE_NAMES }
 export const CLEAR_OCTOMAP_SERVICE_NAME = 'clear_octomap'
 export const CLEAR_OCTOMAP_SERVICE_TYPE = 'std_srvs/srv/Empty'
+export const RETARE_FT_SENSOR_SERVICE_NAME = 'wireless_ft/set_bias'
+export const RETARE_FT_SENSOR_SERVICE_TYPE = 'std_srvs/srv/SetBool'
 export const ACQUISITION_REPORT_SERVICE_NAME = 'ada_feeding_action_select/action_report'
 export const ACQUISITION_REPORT_SERVICE_TYPE = 'ada_feeding_msgs/srv/AcquisitionReport'
 export const GET_ROBOT_STATE_SERVICE_NAME = 'get_robot_state'
diff --git a/feedingwebapp/src/Pages/Header/InfoModal.jsx b/feedingwebapp/src/Pages/Header/InfoModal.jsx
index ea494a6..8d428b2 100644
--- a/feedingwebapp/src/Pages/Header/InfoModal.jsx
+++ b/feedingwebapp/src/Pages/Header/InfoModal.jsx
@@ -124,7 +124,7 @@ function InfoModal(props) {
           {mode === VIDEO_MODE ? (
             <VideoFeed topic={CAMERA_FEED_TOPIC} updateRateHz={10} webrtcURL={props.webrtcURL} />
           ) : mode === TELEOP_MODE ? (
-            <TeleopSubcomponent allowIncreasingForceThreshold={true} />
+            <TeleopSubcomponent allowIncreasingForceThreshold={true} allowRetaringFTSensor={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
index 2a6b22c..32c5f57 100644
--- a/feedingwebapp/src/Pages/Header/TeleopSubcomponent.jsx
+++ b/feedingwebapp/src/Pages/Header/TeleopSubcomponent.jsx
@@ -30,7 +30,9 @@ import {
   SERVO_JOINT_TOPIC_MSG,
   ACTIVATE_CONTROLLER_ACTION_NAME,
   ACTIVATE_CONTROLLER_ACTION_TYPE,
-  SET_PARAMETERS_SERVICE_NAME,
+  SET_PARAMETERS_SERVICE_TYPE,
+  RETARE_FT_SENSOR_SERVICE_NAME,
+  RETARE_FT_SENSOR_SERVICE_TYPE,
   INCREASED_FORCE_THRESHOLD,
   DEFAULT_FORCE_THRESHOLD,
   FORCE_THRESHOLD_PARAM
@@ -561,7 +563,7 @@ const TeleopSubcomponent = (props) => {
   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)
+    return createROSService(ros.current, activeController + '/set_parameters_atomically', SET_PARAMETERS_SERVICE_TYPE)
   }, [ros, teleopMode])
   const setForceThreshold = useCallback(
     (threshold) => {
@@ -585,6 +587,21 @@ const TeleopSubcomponent = (props) => {
     }
   }, [props.allowIncreasingForceThreshold, setForceThreshold])
 
+  /**
+   * Service and callback for retaring the force-torque sensor.
+   */
+  let reTareService = useMemo(() => {
+    return createROSService(ros.current, RETARE_FT_SENSOR_SERVICE_NAME, RETARE_FT_SENSOR_SERVICE_TYPE)
+  }, [ros])
+  const reTareFTSensor = useCallback(() => {
+    let service = reTareService
+    let request = createROSServiceRequest({ data: true })
+    console.log('Calling reTareFTSensor with request', request)
+    service.callService(request, (response) => {
+      console.log('For reTareFTSensor request', request, 'received response', response)
+    })
+  }, [reTareService])
+
   // Render the component
   return (
     <View
@@ -727,6 +744,36 @@ const TeleopSubcomponent = (props) => {
         ) : (
           <></>
         )}
+        {/* If the props specify, show a button to re-tare the force-torque sensor */}
+        {props.allowRetaringFTSensor ? (
+          <View
+            style={{
+              flex: 1,
+              flexDirection: 'row',
+              justifyContent: 'center',
+              alignItems: 'center',
+              width: '100%'
+            }}
+          >
+            <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={reTareFTSensor}
+            >
+              &#x2696; Re-Zero Force Sensor
+            </Button>
+          </View>
+        ) : (
+          <></>
+        )}
         {/* Render the controls for the mode */}
         <View
           style={{
@@ -753,11 +800,14 @@ TeleopSubcomponent.propTypes = {
   // A function to be called when one of the teleop buttons are released
   teleopButtonOnReleaseCallback: PropTypes.func,
   // Whether to allow the user to increase the force threshold
-  allowIncreasingForceThreshold: PropTypes.bool
+  allowIncreasingForceThreshold: PropTypes.bool,
+  // Whether to allow the user to retare the force-torque sensor
+  allowRetaringFTSensor: PropTypes.bool
 }
 TeleopSubcomponent.defaultProps = {
   unmountCallback: { current: () => {} },
   teleopButtonOnReleaseCallback: () => {},
-  allowIncreasingForceThreshold: false
+  allowIncreasingForceThreshold: false,
+  allowRetaringFTSensor: false
 }
 export default TeleopSubcomponent