Merge branch 'main' into amaln/table_detector

sriramk117 · sriramk117 · commit 1e50cfd381a6 · 2024-04-23T00:08:10.000-07:00
diff --git a/feeding_web_app_ros2_test/feeding_web_app_ros2_test/DummyRealSense.py b/feeding_web_app_ros2_test/feeding_web_app_ros2_test/DummyRealSense.py
@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 
 # Standard imports
+import array
 import threading
 
 # Third-party imports
@@ -13,6 +14,16 @@
 from sensor_msgs.msg import CompressedImage, Image, CameraInfo
 
 
+# The fixed header that ROS2 Humble's compressed depth image transport plugin prepends to
+# the data. The exact value was empirically determined, but the below link shows the code
+# that prepends additional data:
+#
+# https://github.com/ros-perception/image_transport_plugins/blob/5ef79d74c4347e6a2d151df63230d5fea1357137/compressed_depth_image_transport/src/codec.cpp#L337
+COMPRESSED_DEPTH_16UC1_HEADER = array.array(
+    "B", [0, 0, 0, 0, 46, 32, 133, 4, 192, 24, 60, 78]
+)
+
+
 class DummyRealSense(Node):
     """
     Reads in a video file and publishes the frames as ROS2 messages.
@@ -80,7 +91,7 @@ def __init__(self):
             CompressedImage, "~/compressed_image", 1
         )
         self.aligned_depth_publisher = self.create_publisher(
-            Image, "~/aligned_depth", 1
+            CompressedImage, "~/aligned_depth", 1
         )
         self.camera_info_publisher = self.create_publisher(
             CameraInfo, "~/camera_info", 1
@@ -163,11 +174,24 @@ def publish_frames(self):
             )
 
             # Configure the depth Image message
-            depth_frame_msg = self.bridge.cv2_to_imgmsg(self.depth_frame, "passthrough")
+            depth_frame_msg = CompressedImage()
             depth_frame_msg.header.frame_id = "camera_color_optical_frame"
             depth_frame_msg.header.stamp = (
                 self.get_clock().now() - rclpy.duration.Duration(seconds=0.15)
             ).to_msg()
+            depth_frame_msg.format = "16UC1; compressedDepth"
+            success, data = cv2.imencode(
+                ".png",
+                self.depth_frame,
+                # PNG compression 1 is the best speed setting, and is the setting
+                # we use for our RealSense.
+                [cv2.IMWRITE_PNG_COMPRESSION, 1],
+            )
+            if not success:
+                raise RuntimeError("Failed to compress image")
+            depth_frame_msg.data = (
+                COMPRESSED_DEPTH_16UC1_HEADER.tobytes() + data.tobytes()
+            )
 
             # Configure the Camera Info
             self.camera_info_msg.header.stamp = depth_frame_msg.header.stamp
diff --git a/feeding_web_app_ros2_test/feeding_web_app_ros2_test/FaceDetection.py b/feeding_web_app_ros2_test/feeding_web_app_ros2_test/FaceDetection.py
@@ -14,10 +14,10 @@
 class FaceDetectionNode(Node):
     def __init__(
         self,
-        face_detection_interval=90,
-        num_images_with_face=60,
-        open_mouth_interval=90,
-        num_images_with_open_mouth=30,
+        face_detection_interval=45,
+        num_images_with_face=30,
+        open_mouth_interval=45,
+        num_images_with_open_mouth=15,
     ):
         """
         Initializes the FaceDetection node, which exposes a SetBool
diff --git a/feeding_web_app_ros2_test/launch/feeding_web_app_dummy_nodes_launch.xml b/feeding_web_app_ros2_test/launch/feeding_web_app_dummy_nodes_launch.xml
@@ -23,7 +23,7 @@
     <node pkg="feeding_web_app_ros2_test" exec="DummyRealSense" name="DummyRealSense">
       <remap from="~/image" to="/camera/color/image_raw"/>
       <remap from="~/compressed_image" to="/camera/color/image_raw/compressed"/>
-      <remap from="~/aligned_depth" to="/camera/aligned_depth_to_color/image_raw"/>
+      <remap from="~/aligned_depth" to="/camera/aligned_depth_to_color/image_raw/compressedDepth"/>
       <remap from="~/camera_info" to="/camera/color/camera_info"/>
       <remap from="~/aligned_depth/camera_info" to="/camera/aligned_depth_to_color/camera_info"/>
       <param name="fps" value="15"/>
diff --git a/feedingwebapp/src/Pages/Constants.js b/feedingwebapp/src/Pages/Constants.js
@@ -130,6 +130,9 @@ export const GET_PARAMETERS_SERVICE_TYPE = 'rcl_interfaces/srv/GetParameters'
 export const SET_PARAMETERS_SERVICE_NAME = 'ada_feeding_action_servers/set_parameters'
 export const SET_PARAMETERS_SERVICE_TYPE = 'rcl_interfaces/srv/SetParameters'
 
+// The names of parameters users can change in the settings menu
+export const DISTANCE_TO_MOUTH_PARAM = 'MoveToMouth.tree_kwargs.plan_distance_from_mouth'
+
 /**
  * The meaning of the status that motion actions return in their results.
  * These should match the action definition(s).
diff --git a/feedingwebapp/src/Pages/GlobalState.jsx b/feedingwebapp/src/Pages/GlobalState.jsx
@@ -75,6 +75,9 @@ export const SETTINGS_STATE = {
   BITE_TRANSFER: 'BITE_TRANSFER'
 }
 
+// The name of the default parameter namespace
+export const DEFAULT_NAMESPACE = 'default'
+
 /**
  * useGlobalState is a hook to store and manipulate web app state that we want
  * to persist across re-renders and refreshes. It won't persist if cookies are
@@ -93,6 +96,7 @@ export const useGlobalState = create(
       mealStateTransitionTime: Date.now(),
       // The currently displayed settings page
       settingsState: SETTINGS_STATE.MAIN,
+      settingsPresets: { current: DEFAULT_NAMESPACE, customNames: [] },
       // The goal for the bite acquisition action, including the most recent
       // food item that the user selected in "bite selection"
       biteAcquisitionActionGoal: null,
@@ -156,6 +160,10 @@ export const useGlobalState = create(
         set(() => ({
           settingsState: settingsState
         })),
+      setSettingsPresets: (settingsPresets) =>
+        set(() => ({
+          settingsPresets: settingsPresets
+        })),
       setBiteAcquisitionActionGoal: (biteAcquisitionActionGoal) =>
         set(() => ({
           biteAcquisitionActionGoal: biteAcquisitionActionGoal
diff --git a/feedingwebapp/src/Pages/Home/MealStates/BiteAcquisitionCheck.jsx b/feedingwebapp/src/Pages/Home/MealStates/BiteAcquisitionCheck.jsx
@@ -137,24 +137,24 @@ const BiteAcquisitionCheck = () => {
     (message) => {
       console.log('Got food-on-fork detection message', message)
       // Only auto-continue if the previous state was Bite Acquisition
-      if (biteAcquisitionCheckAutoContinue && prevMealState === MEAL_STATE.R_BiteAcquisition && message.status === 1) {
+      if (biteAcquisitionCheckAutoContinue && prevMealState === MEAL_STATE.R_BiteAcquisition) {
         let callbackFn = null
-        if (message.probability < biteAcquisitionCheckAutoContinueProbThreshLower) {
+        if (message.status === -1 || (message.status === 1 && message.probability < biteAcquisitionCheckAutoContinueProbThreshLower)) {
           console.log('No FoF. Auto-continuing in ', remainingSeconds, ' seconds')
           if (timerWasForFof.current === true) {
             clearTimer()
           }
           timerWasForFof.current = false
           callbackFn = acquisitionFailure
-        } else if (message.probability > biteAcquisitionCheckAutoContinueProbThreshUpper) {
+        } else if (message.status === 1 && message.probability > biteAcquisitionCheckAutoContinueProbThreshUpper) {
           console.log('FoF. Auto-continuing in ', remainingSeconds, ' seconds')
           if (timerWasForFof.current === false) {
             clearTimer()
           }
           timerWasForFof.current = true
           callbackFn = acquisitionSuccess
         } else {
-          console.log('Not auto-continuing due to probability between thresholds')
+          console.log('Not auto-continuing due to probability between thresholds or errors')
           clearTimer()
         }
         // Don't override an existing timer
diff --git a/feedingwebapp/src/Pages/Settings/BiteTransfer.jsx b/feedingwebapp/src/Pages/Settings/BiteTransfer.jsx
@@ -3,6 +3,8 @@ import React, { useCallback, useEffect, useMemo, useRef, useState } from 'react'
 import PropTypes from 'prop-types'
 import { useId, Label, SpinButton } from '@fluentui/react-components'
 import Button from 'react-bootstrap/Button'
+import Dropdown from 'react-bootstrap/Dropdown'
+import SplitButton from 'react-bootstrap/SplitButton'
 // The Modal is a screen that appears on top of the main app, and can be toggled
 // on and off.
 import Modal from 'react-bootstrap/Modal'
@@ -14,9 +16,10 @@ import {
   GET_PARAMETERS_SERVICE_NAME,
   GET_PARAMETERS_SERVICE_TYPE,
   SET_PARAMETERS_SERVICE_NAME,
-  SET_PARAMETERS_SERVICE_TYPE
+  SET_PARAMETERS_SERVICE_TYPE,
+  DISTANCE_TO_MOUTH_PARAM
 } from '../Constants'
-import { useGlobalState, MEAL_STATE, SETTINGS_STATE } from '../GlobalState'
+import { useGlobalState, MEAL_STATE, SETTINGS_STATE, DEFAULT_NAMESPACE } from '../GlobalState'
 import RobotMotion from '../Home/MealStates/RobotMotion'
 import DetectingFaceSubcomponent from '../Home/MealStates/DetectingFaceSubcomponent'
 
@@ -26,6 +29,7 @@ import DetectingFaceSubcomponent from '../Home/MealStates/DetectingFaceSubcompon
  */
 const BiteTransfer = (props) => {
   // Get relevant global state variables
+  const settingsPresets = useGlobalState((state) => state.settingsPresets)
   const setSettingsState = useGlobalState((state) => state.setSettingsState)
   const globalMealState = useGlobalState((state) => state.mealState)
   const setPaused = useGlobalState((state) => state.setPaused)
@@ -130,27 +134,27 @@ const BiteTransfer = (props) => {
     let service = getParametersService.current
     // First, attempt to get the current distance to mouth
     let currentRequest = createROSServiceRequest({
-      names: ['current.MoveToMouth.tree_kwargs.plan_distance_from_mouth']
+      names: [settingsPresets.current.concat('.', DISTANCE_TO_MOUTH_PARAM)]
     })
     service.callService(currentRequest, (response) => {
       console.log('Got current plan_distance_from_mouth response', response)
-      if (response.values[0].type === 0) {
+      if (response.values.length === 0 || response.values[0].type === 0) {
         // Parameter not set
         // Second, attempt to get the default distance to mouth
         let defaultRequest = createROSServiceRequest({
-          names: ['default.MoveToMouth.tree_kwargs.plan_distance_from_mouth']
+          names: [DEFAULT_NAMESPACE.concat('.', DISTANCE_TO_MOUTH_PARAM)]
         })
         service.callService(defaultRequest, (response) => {
           console.log('Got default plan_distance_from_mouth response', response)
-          if (response.values.length > 0) {
+          if (response.values.length > 0 && response.values[0].type === 8) {
             setCurrentDistanceToMouth(getParameterValue(response.values[0]))
           }
         })
       } else {
         setCurrentDistanceToMouth(getParameterValue(response.values[0]))
       }
     })
-  }, [getParametersService, setCurrentDistanceToMouth, setDoneButtonIsClicked, setPaused])
+  }, [getParametersService, setCurrentDistanceToMouth, setDoneButtonIsClicked, setPaused, settingsPresets])
 
   // Callback to set the distance to mouth parameter
   const setDistanceToMouth = useCallback(
@@ -159,7 +163,7 @@ const BiteTransfer = (props) => {
       let request = createROSServiceRequest({
         parameters: [
           {
-            name: 'current.MoveToMouth.tree_kwargs.plan_distance_from_mouth',
+            name: settingsPresets.current.concat('.', DISTANCE_TO_MOUTH_PARAM),
             value: {
               type: 8, // double array
               double_array_value: fullDistanceToMouth
@@ -174,23 +178,29 @@ const BiteTransfer = (props) => {
         }
       })
     },
-    [setParametersService, setCurrentDistanceToMouth]
+    [setParametersService, setCurrentDistanceToMouth, settingsPresets]
   )
 
   // Callback to restore the distance to mouth to the default
-  const restoreToDefaultButtonClicked = useCallback(() => {
-    let service = getParametersService.current
-    // Attempt to get the default distance to mouth
-    let defaultRequest = createROSServiceRequest({
-      names: ['default.MoveToMouth.tree_kwargs.plan_distance_from_mouth']
-    })
-    service.callService(defaultRequest, (response) => {
-      console.log('Got default plan_distance_from_mouth response', response)
-      if (response.values.length > 0) {
-        setDistanceToMouth(getParameterValue(response.values[0]))
-      }
-    })
-  }, [getParametersService, setDistanceToMouth])
+  const restoreToPreset = useCallback(
+    (preset) => {
+      console.log('restoreToPreset called with', preset)
+      let service = getParametersService.current
+      // Attempt to get the default distance to mouth
+      let defaultRequest = createROSServiceRequest({
+        names: [preset.concat('.', DISTANCE_TO_MOUTH_PARAM)]
+      })
+      service.callService(defaultRequest, (response) => {
+        console.log('Got plan_distance_from_mouth response', response)
+        if (response.values.length > 0 && response.values[0].type === 8) {
+          setDistanceToMouth(getParameterValue(response.values[0]))
+        } else {
+          restoreToPreset(DEFAULT_NAMESPACE)
+        }
+      })
+    },
+    [getParametersService, setDistanceToMouth]
+  )
 
   // Callback to move the robot to the mouth
   const moveToMouthButtonClicked = useCallback(() => {
@@ -275,7 +285,8 @@ const BiteTransfer = (props) => {
               flexDirection: 'column',
               justifyContent: 'center',
               alignItems: 'center',
-              width: '100%'
+              width: '100%',
+              zIndex: 1
             }}
           >
             <Label
@@ -306,7 +317,7 @@ const BiteTransfer = (props) => {
                 size: 'large'
               }}
             />
-            <Button
+            <SplitButton
               variant='warning'
               className='mx-2 mb-2 btn-huge'
               size='lg'
@@ -315,10 +326,17 @@ const BiteTransfer = (props) => {
                 width: '60%',
                 color: 'black'
               }}
-              onClick={restoreToDefaultButtonClicked}
+              title={'Set to '.concat(DEFAULT_NAMESPACE)}
+              onClick={() => restoreToPreset(DEFAULT_NAMESPACE)}
             >
-              Set to Default
-            </Button>
+              {settingsPresets.customNames
+                .filter((x) => x !== settingsPresets.current)
+                .map((preset) => (
+                  <Dropdown.Item key={preset} onClick={() => restoreToPreset(preset)}>
+                    Set to {preset}
+                  </Dropdown.Item>
+                ))}
+            </SplitButton>
           </View>
           <View
             style={{
@@ -396,7 +414,8 @@ const BiteTransfer = (props) => {
     distanceToMouthId,
     moveToMouthButtonClicked,
     moveAwayFromMouthButtonClicked,
-    restoreToDefaultButtonClicked
+    restoreToPreset,
+    settingsPresets
   ])
 
   // When a face is detected, switch to MoveToMouth
@@ -435,18 +454,52 @@ const BiteTransfer = (props) => {
     <>
       <View
         style={{
-          flex: 2,
+          flex: 4,
           flexDirection: 'column',
           justifyContent: 'center',
           alignItems: 'center',
           width: '100%'
         }}
       >
-        <h5 style={{ textAlign: 'center', fontSize: textFontSize }}>Customize Bite Transfer</h5>
+        <p style={{ textAlign: 'center', fontSize: textFontSize, margin: 0 }} className='txt-huge'>
+          Bite Transfer Settings
+        </p>
+      </View>
+      <View
+        style={{
+          flex: 3,
+          flexDirection: 'row',
+          justifyContent: 'center',
+          alignItems: 'center',
+          width: '100%'
+        }}
+      >
+        <View
+          style={{
+            flex: 1,
+            justifyContent: 'center',
+            alignItems: 'end',
+            width: '100%'
+          }}
+        >
+          <p style={{ fontSize: textFontSize, textAlign: 'right', margin: '0rem' }}>Preset:</p>
+        </View>
+        <View
+          style={{
+            flex: 1,
+            justifyContent: 'center',
+            alignItems: 'start',
+            width: '100%'
+          }}
+        >
+          <Button variant='secondary' disabled size='lg' style={{ marginLeft: '1rem' }}>
+            {settingsPresets.current}
+          </Button>
+        </View>
       </View>
       <View
         style={{
-          flex: 16,
+          flex: 32,
           flexDirection: 'column',
           justifyContent: 'center',
           alignItems: 'center',
@@ -457,7 +510,7 @@ const BiteTransfer = (props) => {
       </View>
       <View
         style={{
-          flex: 2,
+          flex: 4,
           flexDirection: 'column',
           justifyContent: 'center',
           alignItems: 'center',
diff --git a/feedingwebapp/src/Pages/Settings/Main.jsx b/feedingwebapp/src/Pages/Settings/Main.jsx
