Food on Fork Integration with Web App

feedingwebapp/TechDocumentation.md

@@ -11,7 +11,7 @@
 All functions to interact with ROS are defined in [`ros_helpers.jsx`](https://github.com/personalrobotics/feeding_web_interface/tree/main/feedingwebapp/src/ros/ros_helpers.jsx). We followed the following guiding principles when making it:
 - No other piece of code should need to import `roslib`; all functions that use ROSLIB should be in `ros_helpers.jsx`.
 - No other piece of code should need to import `react-ros`. The only exception to that is [`App.jsx`](https://github.com/personalrobotics/feeding_web_interface/tree/main/feedingwebapp/src/App.jsx) which needs to wrap elements that use ROS in the ROS tag.
-- Only `connectToROS()` should call `useROS()`, while other functions should have the `ros` object passed in. This is because hooks can only be used in React components (e.g., not callback functions). So the general idea is that a component that uses ROS will call `let { isConnected, ros } = connectToROS()` within the main code for the component, and will pass `ros` to any subsequent `ros_helpers.jsx` function calls it makes.
+- The best way to connect to ros would be to add the line, `const ros = useRef(useRos().ros)` to any component that needs it. Then, the functions that need `ros` object can have it passed in. This is because hooks can only be used in React components (e.g., not callback functions). So the general idea is that you will have a `ros` object defined and will be able to pass it into any subsequent `ros_helpers.jsx` function calls it makes. Note that you probably will need to import `useRef` for this to work successfully. 
 
 `ros_helpers.jsx` currently supports subscribing to and publishing from ROS topics, calling ROS services, and calling ROS actions. Sample code for each of these features can be found in [`TestROS.jsx`](https://github.com/personalrobotics/feeding_web_interface/tree/main/feedingwebapp/src/ros/TestROS.jsx) and the components it imports, and can be accessed by starting the web app and navigating to `http://localhost:3000/test_ros` in your browser. See [README.md](https://github.com/personalrobotics/feeding_web_interface/tree/main/feedingwebapp/README.md) for more detailed instructions on using the "Test ROS" interface.
 

feedingwebapp/src/Pages/Constants.js

@@ -46,6 +46,8 @@ export const CAMERA_FEED_TOPIC = '/local/camera/color/image_raw'
 export const FACE_DETECTION_TOPIC = '/face_detection'
 export const FACE_DETECTION_TOPIC_MSG = 'ada_feeding_msgs/FaceDetection'
 export const FACE_DETECTION_IMG_TOPIC = '/face_detection_img'
+// Name of the ROS topic and the type of response
+export const FOOD_ON_FORK_TOPIC = { name: '/food_on_fork', type: 'std_msgs/Float32' }
 
 /**
  * For states that call ROS actions, this dictionary contains
@@ -114,3 +116,10 @@ export const ROS_ACTION_STATUS_CANCEL_GOAL = '2'
 export const ROS_ACTION_STATUS_SUCCEED = '3'
 export const ROS_ACTION_STATUS_ABORT = '4'
 export const ROS_ACTION_STATUS_CANCELED = '5'
+
+/**
+ * Constant range of probability values that defines Food on Fork and window size
+ */
+export const FOOD_ON_FORK_PROB_RANGE = { lowerProb: 0.5, higherProb: 0.5 }
+export const FOOD_ON_FORK_BITE_TRANSFER_WINDOW_SIZE = 120
+export const FOOD_ON_FORK_BITE_ACQUISITION_WINDOW_SIZE = 50

feedingwebapp/src/Pages/GlobalState.jsx

@@ -76,14 +76,16 @@ export const MEAL_STATE = {
  *         enable multiple buttons if they so desire.
  *   - biteSelection: Options for how the user wants to tell the robot what food
  *     item they want next.
+ *   - foodOnFork: Options for user to toggle FoF detection on or off
  *
  * TODO (amaln): When we connect this to ROS, each of these settings types and
  * value options will have to have corresponding rosparam names and value options.
  */
 export const SETTINGS = {
   stagingPosition: ['In Front of Me', 'On My Right Side'],
   biteInitiation: ['Open Mouth', 'Say "I am Ready"', 'Press Button'],
-  biteSelection: ['Name of Food', 'Click on Food']
+  biteSelection: ['Name of Food', 'Click on Food'],
+  foodOnFork: ['Yes', 'No']
 }
 
 /**
@@ -110,6 +112,7 @@ export const useGlobalState = create(
       stagingPosition: SETTINGS.stagingPosition[0],
       biteInitiation: SETTINGS.biteInitiation[0],
       biteSelection: SETTINGS.biteSelection[0],
+      foodOnFork: SETTINGS.foodOnFork[0],
 
       // Setters for global state
       setMealState: (mealState) =>
@@ -144,6 +147,10 @@ export const useGlobalState = create(
       setBiteSelection: (biteSelection) =>
         set(() => ({
           biteSelection: biteSelection
+        })),
+      setFoodOnFork: (foodOnFork) =>
+        set(() => ({
+          foodOnFork: foodOnFork
         }))
     }),
     { name: 'ada_web_app_global_state' }

feedingwebapp/src/Pages/Home/MealStates/BiteAcquisitionCheck.jsx

@@ -1,13 +1,21 @@
 // React Imports
-import React, { useCallback } from 'react'
+import React, { useCallback, useEffect, useRef, useState } from 'react'
 import Button from 'react-bootstrap/Button'
 import { useMediaQuery } from 'react-responsive'
 import { View } from 'react-native'
 
 // Local Imports
 import '../Home.css'
 import { useGlobalState, MEAL_STATE } from '../../GlobalState'
-import { MOVING_STATE_ICON_DICT } from '../../Constants'
+import {
+  FOOD_ON_FORK_TOPIC,
+  FOOD_ON_FORK_PROB_RANGE,
+  FOOD_ON_FORK_BITE_ACQUISITION_WINDOW_SIZE,
+  MOVING_STATE_ICON_DICT
+} from '../../Constants'
+
+// Import subscriber to be able to subscribe to FoF topic
+import { subscribeToROSTopic, unsubscribeFromROSTopic, useROS } from '../../../ros/ros_helpers'
 
 /**
  * The BiteAcquisitionCheck component appears after the robot has attempted to
@@ -16,6 +24,8 @@
 const BiteAcquisitionCheck = () => {
   // Get the relevant global variables
   const setMealState = useGlobalState((state) => state.setMealState)
+  const foodOnFork = useGlobalState((state) => state.foodOnFork)
+  const [detectedFood, setDetectedFood] = useState("")
   // Get icon image for move above plate
   let moveAbovePlateImage = MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingAbovePlate]
   // Get icon image for move to mouth
@@ -31,6 +41,46 @@
   let iconWidth = isPortrait ? '28vh' : '28vw'
   let iconHeight = isPortrait ? '18vh' : '18vw'
 
+  // Connect to Ros
+  const ros = useRef(useROS().ros)
+  let window = []
+  const food_on_fork_callback = useCallback((message) => {
+    if (window.length === Number(FOOD_ON_FORK_BITE_ACQUISITION_WINDOW_SIZE)) {
+      console.log('entered')
+      window.shift()
+    }
+    window.push(Number(message.data))
+    let countLessThanRange = 0
+    for (const val of window) {
+      if (val < FOOD_ON_FORK_PROB_RANGE.lowerProb) {
+        countLessThanRange++
+      }
+    }
+    console.log(window.length)
+    if (
+      window.length === FOOD_ON_FORK_BITE_ACQUISITION_WINDOW_SIZE &&
+      countLessThanRange >= 0.75 * FOOD_ON_FORK_BITE_ACQUISITION_WINDOW_SIZE
+    ) {
+      if (foodOnFork === "Yes") {
+        console.log('Detecting no food on fork (Acquisition Failure); moving above plate')
+        acquisitionFailure()
+      } else {
+        setDetectedFood("detected no food")
+      }
+      return
+    } 
+  })
+
+  useEffect(() => {
+    console.log('Subscribed to FoF')
+    const food_on_fork_topic = subscribeToROSTopic(ros.current, FOOD_ON_FORK_TOPIC.name, FOOD_ON_FORK_TOPIC.type, food_on_fork_callback)
+
+    return () => {
+      console.log('unscubscribed from FoF')
+      unsubscribeFromROSTopic(food_on_fork_topic)
+    }
+  }, [setMealState, food_on_fork_callback, foodOnFork])
+
   /**
    * Callback function for when the user indicates that the bite acquisition
    * succeeded.
@@ -140,9 +190,10 @@
           {reacquireBiteText()}
           {reacquireBiteButton()}
         </View>
+        <p>Currently detecting: {detectedFood}</p>
       </View>
     )
   }, [dimension, reacquireBiteButton, reacquireBiteText, readyForBiteButton, readyForBiteText])
 
   // Render the component
   return <>{fullPageView()}</>

feedingwebapp/src/Pages/Home/MealStates/BiteDone.jsx

@@ -1,13 +1,21 @@
 // React Imports
-import React, { useCallback } from 'react'
+import React, { useCallback, useEffect, useRef } from 'react'
 import Button from 'react-bootstrap/Button'
 import { useMediaQuery } from 'react-responsive'
 import { View } from 'react-native'
 
 // Local Imports
 import '../Home.css'
 import { useGlobalState, MEAL_STATE } from '../../GlobalState'
-import { MOVING_STATE_ICON_DICT } from '../../Constants'
+import {
+  FOOD_ON_FORK_TOPIC,
+  FOOD_ON_FORK_PROB_RANGE,
+  FOOD_ON_FORK_BITE_TRANSFER_WINDOW_SIZE,
+  MOVING_STATE_ICON_DICT
+} from '../../Constants'
+
+// Import subscriber to be able to subscribe to FoF topic
+import { subscribeToROSTopic, unsubscribeFromROSTopic, useROS } from '../../../ros/ros_helpers'
 
 /**
  * The BiteDone component appears after the robot has moved to the user's mouth,
@@ -17,6 +25,7 @@
 const BiteDone = () => {
   // Get the relevant global variables
   const setMealState = useGlobalState((state) => state.setMealState)
+  const foodOnFork = useGlobalState((state) => state.foodOnFork)
   // Get icon image for move above plate
   let moveAbovePlateImage = MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingFromMouthToAbovePlate]
   // Get icon image for move to resting position
@@ -32,6 +41,40 @@
   let iconWidth = isPortrait ? '28vh' : '28vw'
   let iconHeight = isPortrait ? '18vh' : '18vw'
 
+  // Connect to Ros
+  const ros = useRef(useROS().ros)
+  let window = []
+  const food_on_fork_callback = useCallback((message) => {
+    console.log('Subscribed to FoF')
+    if (window.length === Number(FOOD_ON_FORK_BITE_TRANSFER_WINDOW_SIZE)) {
+      window.shift()
+    }
+    window.push(Number(message.data))
+    let countLessThanRange = 0
+    for (const val of window) {
+      if (val < FOOD_ON_FORK_PROB_RANGE.lowerProb) {
+        countLessThanRange++
+      }
+    }
+    console.log(window.length)
+    if (window.length === FOOD_ON_FORK_BITE_TRANSFER_WINDOW_SIZE && countLessThanRange >= 0.75 * FOOD_ON_FORK_BITE_TRANSFER_WINDOW_SIZE) {
+      if (foodOnFork === "Yes") {
+        console.log('Detecting no food on fork; moving above plate')
+        moveAbovePlate()
+      }
+      return
+    }
+  })
+
+  useEffect(() => {
+    const food_on_fork_topic = subscribeToROSTopic(ros.current, FOOD_ON_FORK_TOPIC.name, FOOD_ON_FORK_TOPIC.type, food_on_fork_callback)
+
+    return () => {
+      console.log('unscubscribed from FoF')
+      unsubscribeFromROSTopic(food_on_fork_topic)
+    }
+  }, [setMealState, food_on_fork_callback, foodOnFork])
+
   /**
    * Callback function for when the user wants to move above plate.
    */

feedingwebapp/src/Pages/Settings/Settings.jsx

@@ -60,6 +60,15 @@ const Settings = () => {
           valueSetter={useGlobalState((state) => state.setBiteSelection)}
         />
       </Row>
+
+      <Row className='justify-content-center mx-1 my-2'>
+        <Form.Label style={{ fontSize: '30px' }}>Would you like to use the Automatic Food on Fork detection? </Form.Label>
+        <ToggleButtonGroup
+          valueOptions={SETTINGS.foodOnFork}
+          currentValue={useGlobalState((state) => state.foodOnFork)}
+          valueSetter={useGlobalState((state) => state.setFoodOnFork)}
+        />
+      </Row>
     </div>
   )
 }

feedingwebapp/src/ros/TestROS.jsx

@@ -45,9 +45,15 @@ function TestROS() {
       <hr />
 
       {/**
-       * Allow users to subscribe to a topic and display its data
+       * Allow users to subscribe to a topic (of type std_msgs/String) and display its data
        */}
-      <TestROSSubscribe />
+      <TestROSSubscribe topicType={'std_msgs/String'} />
+      <hr />
+
+      {/**
+       * Allow users to subscribe to a topic (of type std_msgs/Float32) and display its data
+       */}
+      <TestROSSubscribe topicType={'std_msgs/Float32'} />
       <hr />
 
       {/**

feedingwebapp/src/ros/TestROSSubscribe.jsx

@@ -1,5 +1,7 @@
 // React imports
 import React, { useState } from 'react'
+// Component
+import PropTypes from 'prop-types'
 
 // Local imports
 import { useROS, subscribeToROSTopic } from './ros_helpers'
@@ -8,7 +10,10 @@ import { useROS, subscribeToROSTopic } from './ros_helpers'
  * The TestROSSubscribe component demonstrates the functionality of subscribing
  * to a ROS topic.
  */
-function TestROSSubscribe() {
+function TestROSSubscribe(props) {
+  // get the value of props
+  let topicType = props.topicType
+
   // The defaults to use on this page
   let defaultTopicName = 'test_topic'
 
@@ -21,7 +26,7 @@ function TestROSSubscribe() {
   let [recvData, setRecvData] = useState('No message received yet.')
 
   // Callback function for when the user clicks the "Subscribe" button
-  function subscribeTopic(event) {
+  function subscribeTopic(event, topicType) {
     // Prevent the browser from reloading the page
     event.preventDefault()
 
@@ -34,7 +39,7 @@ function TestROSSubscribe() {
     }
 
     // Subscribe to the topic
-    subscribeToROSTopic(ros, topicName, 'std_msgs/String', callback)
+    subscribeToROSTopic(ros, topicName, topicType, callback)
   }
 
   // Render the component
@@ -43,8 +48,8 @@ function TestROSSubscribe() {
       {/**
        * Allow users to subscribe to a topic and display its data
        */}
-      <h4>Subscribe to a &apos;std_msgs/String&apos; Topic and Display Its Data:</h4>
-      <form method='post' onSubmit={subscribeTopic}>
+      <h4>Subscribe to a &apos;{topicType}&apos; Topic and Display Its Data:</h4>
+      <form method='post' onSubmit={(e) => subscribeTopic(e, topicType)}>
         Topic Name: <input type='text' name='topicName' defaultValue={defaultTopicName} />
         <button type='submit'>Subscribe</button>
         <br />
@@ -55,5 +60,8 @@ function TestROSSubscribe() {
     </div>
   )
 }
+TestROSSubscribe.propTypes = {
+  topicType: PropTypes.string.isRequired
+}
 
 export default TestROSSubscribe

feedingwebapp/src/ros/ros_helpers.js

@@ -55,17 +55,23 @@ export function createROSTopic(ros, topicName, topicType) {
  * @param {string} topicType The type of the topic to create.
  * @param {function} callback The callback function to call when a message is
  *                   received.
+ * @param {number} interval The interval value in ms (milliseconds) that the
+ *                   topic should be listened at (for instance, 1000 would mean
+ *                   that we subscribe to the particular topic and listen to it
+ *                   every 1000 ms). Default value is 0; which means as soon as
+ *                   the message is received
  * @returns {object} The ROSLIB.Topic, or null if ROS is not connected.
  */
-export function subscribeToROSTopic(ros, topicName, topicType, callback) {
+export function subscribeToROSTopic(ros, topicName, topicType, callback, interval = 0) {
   if (ros === null) {
     console.log('ROS is not connected')
     return null
   }
   let topic = new ROSLIB.Topic({
     ros: ros,
     name: topicName,
-    messageType: topicType
+    messageType: topicType,
+    throttle_rate: interval
   })
   topic.subscribe(callback)
   return topic