From ccafe3c80e7d6aafdb7f603faae2f13e5998b08e Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 22 Mar 2024 20:00:41 -0700 Subject: [PATCH] Added FoodOnFork Detection to Web App (#127) * Added a FoodOnFork dummy node * Added and tested FoF detection on web app * Change to detecting no FoF * Start with predicting FoF * Check for FoF Success * Start with predicting FoF in the dummy node * Add more descriptive text * Added and tested auto-continue for BiteAcquisitionCheck * Adjusted the moving away text --- .../FaceDetection.py | 1 - .../FoodOnForkDetection.py | 139 ++++++++ .../feeding_web_app_dummy_nodes_launch.xml | 8 +- feeding_web_app_ros2_test/setup.py | 1 + feedingwebapp/src/Pages/Constants.js | 10 + feedingwebapp/src/Pages/GlobalState.jsx | 76 ++--- .../Home/MealStates/BiteAcquisitionCheck.jsx | 230 ++++++++++++- .../src/Pages/Home/MealStates/BiteDone.jsx | 310 ++++++++++++++---- 8 files changed, 657 insertions(+), 118 deletions(-) create mode 100755 feeding_web_app_ros2_test/feeding_web_app_ros2_test/FoodOnForkDetection.py 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 index cd5da709..3fce9884 100755 --- 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 @@ -70,7 +70,6 @@ def __init__( self.camera_callback, 1, ) - self.subscription # prevent unused variable warning # Create the publishers self.publisher_results = self.create_publisher( diff --git a/feeding_web_app_ros2_test/feeding_web_app_ros2_test/FoodOnForkDetection.py b/feeding_web_app_ros2_test/feeding_web_app_ros2_test/FoodOnForkDetection.py new file mode 100755 index 00000000..e1b12964 --- /dev/null +++ b/feeding_web_app_ros2_test/feeding_web_app_ros2_test/FoodOnForkDetection.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +from ada_feeding_msgs.msg import FoodOnForkDetection +from std_srvs.srv import SetBool +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CompressedImage +from threading import Lock + + +class FoodOnForkDetectionNode(Node): + def __init__( + self, + food_on_fork_detection_interval=90, + num_images_with_food=90, + ): + """ + Initializes the FoodOnForkDetection node, which exposes a SetBool + service that can be used to toggle the food on fork detection on or off and + publishes information to the /food_on_fork_detection topic when food-on-fork + detection is on. + + After food_on_fork_detection_interval images without food, this dummy function + detects food for num_images_with_food frames. + + Parameters: + ---------- + food_on_fork_detection_interval: The number of frames between each food detection. + num_images_with_food: The number of frames that must have a food in them. + """ + super().__init__("food_on_fork_detection") + + # Internal variables to track when food should be detected + self.food_on_fork_detection_interval = food_on_fork_detection_interval + self.num_images_with_food = num_images_with_food + self.num_consecutive_images_without_food = ( + self.food_on_fork_detection_interval + ) # Start predicting FoF + self.num_consecutive_images_with_food = 0 + + # Keeps track of whether food on fork detection is on or not + self.is_on = False + self.is_on_lock = Lock() + + # Create the service + self.srv = self.create_service( + SetBool, + "toggle_food_on_fork_detection", + self.toggle_food_on_fork_detection_callback, + ) + + # Subscribe to the camera feed + self.subscription = self.create_subscription( + CompressedImage, + "camera/color/image_raw/compressed", + self.camera_callback, + 1, + ) + + # Create the publishers + self.publisher_results = self.create_publisher( + FoodOnForkDetection, "food_on_fork_detection", 1 + ) + + def toggle_food_on_fork_detection_callback(self, request, response): + """ + Callback function for the SetBool service. Safely toggles + the food on fork detection on or off depending on the request. + """ + self.get_logger().info("Incoming service request. turn_on: %s" % (request.data)) + if request.data: + # Reset counters + self.num_consecutive_images_without_food = ( + self.food_on_fork_detection_interval + ) # Start predicting FoF + self.num_consecutive_images_with_food = 0 + # Turn on food-on-fork detection + self.is_on_lock.acquire() + self.is_on = True + self.is_on_lock.release() + response.success = True + response.message = "Succesfully turned food-on-fork detection on" + else: + self.is_on_lock.acquire() + self.is_on = False + self.is_on_lock.release() + response.success = True + response.message = "Succesfully turned food-on-fork detection off" + return response + + def camera_callback(self, msg): + """ + Callback function for the camera feed. If food-on-fork detection is on, this + function will detect food in the image and publish information about + them to the /food_on_fork_detection topic. + """ + self.get_logger().debug("Received image") + self.is_on_lock.acquire() + is_on = self.is_on + self.is_on_lock.release() + if is_on: + # Update the number of consecutive images with/without a food + is_food_detected = False + if self.num_consecutive_images_with_food == self.num_images_with_food: + self.num_consecutive_images_without_food = 0 + self.num_consecutive_images_with_food = 0 + if ( + self.num_consecutive_images_without_food + == self.food_on_fork_detection_interval + ): + # Detect food on the fork + self.num_consecutive_images_with_food += 1 + is_food_detected = True + else: + # Don't detect food + self.num_consecutive_images_without_food += 1 + + # Publish the food-on-fork detection information + food_on_fork_detection_msg = FoodOnForkDetection() + food_on_fork_detection_msg.header = msg.header + food_on_fork_detection_msg.probability = 1.0 if is_food_detected else 0.0 + food_on_fork_detection_msg.status = food_on_fork_detection_msg.SUCCESS + food_on_fork_detection_msg.message = ( + "Food detected" if is_food_detected else "No food detected" + ) + self.publisher_results.publish(food_on_fork_detection_msg) + + +def main(args=None): + rclpy.init(args=args) + + food_on_fork_detection = FoodOnForkDetectionNode() + + rclpy.spin(food_on_fork_detection) + + rclpy.shutdown() + + +if __name__ == "__main__": + main() 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 index d85730e5..9e6e1653 100644 --- 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 @@ -4,6 +4,7 @@ + @@ -24,7 +25,7 @@ - + @@ -39,6 +40,11 @@ + + + + + diff --git a/feeding_web_app_ros2_test/setup.py b/feeding_web_app_ros2_test/setup.py index de7ca981..b77228cb 100644 --- a/feeding_web_app_ros2_test/setup.py +++ b/feeding_web_app_ros2_test/setup.py @@ -36,6 +36,7 @@ "AcquireFoodClient = feeding_web_app_ros2_test.AcquireFoodClient:main", "DummyRealSense = feeding_web_app_ros2_test.DummyRealSense:main", "FaceDetection = feeding_web_app_ros2_test.FaceDetection:main", + "FoodOnForkDetection = feeding_web_app_ros2_test.FoodOnForkDetection:main", "MoveAbovePlate = feeding_web_app_ros2_test.MoveAbovePlate:main", "MoveToRestingPosition = feeding_web_app_ros2_test.MoveToRestingPosition:main", "MoveToStagingConfiguration = feeding_web_app_ros2_test.MoveToStagingConfiguration:main", diff --git a/feedingwebapp/src/Pages/Constants.js b/feedingwebapp/src/Pages/Constants.js index 3e237a49..55a55c33 100644 --- a/feedingwebapp/src/Pages/Constants.js +++ b/feedingwebapp/src/Pages/Constants.js @@ -50,6 +50,8 @@ export const CAMERA_FEED_TOPIC = '/local/camera/color/image_raw/compressed' 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/compressed' +export const FOOD_ON_FORK_DETECTION_TOPIC = '/food_on_fork_detection' +export const FOOD_ON_FORK_DETECTION_TOPIC_MSG = 'ada_feeding_msgs/FoodOnForkDetection' export const ROBOT_COMPRESSED_IMG_TOPICS = [CAMERA_FEED_TOPIC, FACE_DETECTION_IMG_TOPIC] // States from which, if they fail, it is NOT okay for the user to retry the @@ -106,6 +108,14 @@ ROS_SERVICE_NAMES[MEAL_STATE.R_DetectingFace] = { serviceName: 'toggle_face_detection', messageType: 'std_srvs/srv/SetBool' } +ROS_SERVICE_NAMES[MEAL_STATE.U_BiteDone] = { + serviceName: 'toggle_food_on_fork_detection', + messageType: 'std_srvs/srv/SetBool' +} +ROS_SERVICE_NAMES[MEAL_STATE.U_BiteAcquisitionCheck] = { + serviceName: 'toggle_food_on_fork_detection', + messageType: 'std_srvs/srv/SetBool' +} export { ROS_SERVICE_NAMES } export const CLEAR_OCTOMAP_SERVICE_NAME = 'clear_octomap' export const CLEAR_OCTOMAP_SERVICE_TYPE = 'std_srvs/srv/Empty' diff --git a/feedingwebapp/src/Pages/GlobalState.jsx b/feedingwebapp/src/Pages/GlobalState.jsx index 36372cc3..d193dd01 100644 --- a/feedingwebapp/src/Pages/GlobalState.jsx +++ b/feedingwebapp/src/Pages/GlobalState.jsx @@ -75,27 +75,6 @@ export const SETTINGS_STATE = { BITE_TRANSFER: 'BITE_TRANSFER' } -/** - * The parameters that users can set (keys) and a list of human-readable values - * they can take on. - * - stagingPosition: Discrete options for where the robot should wait until - * the user is ready. - * - biteInitiation: Options for the modality the user wants to use to tell - * the robot they are ready for a bite. - * - TODO: Make these checkboxes instead -- users should be able to - * enable multiple buttons if they so desire. - * - biteSelection: Options for how the user wants to tell the robot what food - * item they want next. - * - * 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'] -// } - /** * 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 @@ -129,6 +108,16 @@ export const useGlobalState = create( teleopIsMoving: false, // Flag to indicate whether to auto-continue after face detection faceDetectionAutoContinue: true, + // Flag to indicate whether to auto-continue in bite done after food-on-fork detection + biteDoneAutoContinue: false, + biteDoneAutoContinueSecs: 3.0, + biteDoneAutoContinueProbThresh: 0.25, + // Flags to indicate whether to auto-continue in bite acquisition check based on food-on-fork + // detection + biteAcquisitionCheckAutoContinue: false, + biteAcquisitionCheckAutoContinueSecs: 3.0, + biteAcquisitionCheckAutoContinueProbThreshLower: 0.25, + biteAcquisitionCheckAutoContinueProbThreshUpper: 0.75, // Whether the settings bite transfer page is currently at the user's face // or not. This is in the off-chance that the mealState is not at the user's // face, the settings page is, and the user refreshes -- the page should @@ -141,11 +130,6 @@ export const useGlobalState = create( // How much the video on the Bite Selection page should be zoomed in. biteSelectionZoom: 1.0, - // Settings values - // stagingPosition: SETTINGS.stagingPosition[0], - // biteInitiation: SETTINGS.biteInitiation[0], - // biteSelection: SETTINGS.biteSelection[0], - // Setters for global state setAppPage: (appPage) => set(() => ({ @@ -196,6 +180,34 @@ export const useGlobalState = create( set(() => ({ faceDetectionAutoContinue: faceDetectionAutoContinue })), + setBiteDoneAutoContinue: (biteDoneAutoContinue) => + set(() => ({ + biteDoneAutoContinue: biteDoneAutoContinue + })), + setBiteDoneAutoContinueSecs: (biteDoneAutoContinueSecs) => + set(() => ({ + biteDoneAutoContinueSecs: biteDoneAutoContinueSecs + })), + setBiteDoneAutoContinueProbThresh: (biteDoneAutoContinueProbThresh) => + set(() => ({ + biteDoneAutoContinueProbThresh: biteDoneAutoContinueProbThresh + })), + setBiteAcquisitionCheckAutoContinue: (biteAcquisitionCheckAutoContinue) => + set(() => ({ + biteAcquisitionCheckAutoContinue: biteAcquisitionCheckAutoContinue + })), + setBiteAcquisitionCheckAutoContinueSecs: (biteAcquisitionCheckAutoContinueSecs) => + set(() => ({ + biteAcquisitionCheckAutoContinueSecs: biteAcquisitionCheckAutoContinueSecs + })), + setBiteAcquisitionCheckAutoContinueProbThreshLower: (biteAcquisitionCheckAutoContinueProbThreshLower) => + set(() => ({ + biteAcquisitionCheckAutoContinueProbThreshLower: biteAcquisitionCheckAutoContinueProbThreshLower + })), + setBiteAcquisitionCheckAutoContinueProbThreshUpper: (biteAcquisitionCheckAutoContinueProbThreshUpper) => + set(() => ({ + biteAcquisitionCheckAutoContinueProbThreshUpper: biteAcquisitionCheckAutoContinueProbThreshUpper + })), setBiteTransferPageAtFace: (biteTransferPageAtFace) => set(() => ({ biteTransferPageAtFace: biteTransferPageAtFace @@ -204,18 +216,6 @@ export const useGlobalState = create( set(() => ({ biteSelectionZoom: biteSelectionZoom })) - // setStagingPosition: (stagingPosition) => - // set(() => ({ - // stagingPosition: stagingPosition - // })), - // setBiteInitiation: (biteInitiation) => - // set(() => ({ - // biteInitiation: biteInitiation - // })), - // setBiteSelection: (biteSelection) => - // set(() => ({ - // biteSelection: biteSelection - // })) }), { name: 'ada_web_app_global_state' } ) diff --git a/feedingwebapp/src/Pages/Home/MealStates/BiteAcquisitionCheck.jsx b/feedingwebapp/src/Pages/Home/MealStates/BiteAcquisitionCheck.jsx index de7eb9ad..68c29cc7 100644 --- a/feedingwebapp/src/Pages/Home/MealStates/BiteAcquisitionCheck.jsx +++ b/feedingwebapp/src/Pages/Home/MealStates/BiteAcquisitionCheck.jsx @@ -1,5 +1,5 @@ // React Imports -import React, { useCallback, useRef } from 'react' +import React, { useCallback, useEffect, useState, useRef } from 'react' import Button from 'react-bootstrap/Button' import { useMediaQuery } from 'react-responsive' import { toast } from 'react-toastify' @@ -9,16 +9,30 @@ import { View } from 'react-native' import '../Home.css' import { useGlobalState, MEAL_STATE } from '../../GlobalState' import { MOVING_STATE_ICON_DICT } from '../../Constants' -import { useROS, createROSService, createROSServiceRequest } from '../../../ros/ros_helpers' -import { ACQUISITION_REPORT_SERVICE_NAME, ACQUISITION_REPORT_SERVICE_TYPE } from '../../Constants' +import { useROS, createROSService, createROSServiceRequest, subscribeToROSTopic, unsubscribeFromROSTopic } from '../../../ros/ros_helpers' +import { + ACQUISITION_REPORT_SERVICE_NAME, + ACQUISITION_REPORT_SERVICE_TYPE, + FOOD_ON_FORK_DETECTION_TOPIC, + FOOD_ON_FORK_DETECTION_TOPIC_MSG, + ROS_SERVICE_NAMES +} from '../../Constants' /** * The BiteAcquisitionCheck component appears after the robot has attempted to * acquire a bite, and asks the user whether it succeeded at acquiring the bite. */ const BiteAcquisitionCheck = () => { + // Store the remining time before auto-continuing + const [remainingSeconds, setRemainingSeconds] = useState(null) // Get the relevant global variables + const prevMealState = useGlobalState((state) => state.prevMealState) const setMealState = useGlobalState((state) => state.setMealState) + const biteAcquisitionCheckAutoContinue = useGlobalState((state) => state.biteAcquisitionCheckAutoContinue) + const setBiteAcquisitionCheckAutoContinue = useGlobalState((state) => state.setBiteAcquisitionCheckAutoContinue) + const biteAcquisitionCheckAutoContinueSecs = useGlobalState((state) => state.biteAcquisitionCheckAutoContinueSecs) + const biteAcquisitionCheckAutoContinueProbThreshLower = useGlobalState((state) => state.biteAcquisitionCheckAutoContinueProbThreshLower) + const biteAcquisitionCheckAutoContinueProbThreshUpper = useGlobalState((state) => state.biteAcquisitionCheckAutoContinueProbThreshUpper) // Get icon image for move above plate let moveAbovePlateImage = MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingAbovePlate] // Get icon image for move to mouth @@ -45,6 +59,12 @@ const BiteAcquisitionCheck = () => { * Create the ROS Service Client for reporting success/failure */ let acquisitionReportService = useRef(createROSService(ros.current, ACQUISITION_REPORT_SERVICE_NAME, ACQUISITION_REPORT_SERVICE_TYPE)) + /** + * Create the ROS Service. This is created in local state to avoid re-creating + * it upon every re-render. + */ + let { serviceName, messageType } = ROS_SERVICE_NAMES[MEAL_STATE.U_BiteAcquisitionCheck] + let toggleFoodOnForkDetectionService = useRef(createROSService(ros.current, serviceName, messageType)) /** * Callback function for when the user indicates that the bite acquisition @@ -86,6 +106,136 @@ const BiteAcquisitionCheck = () => { setMealState(MEAL_STATE.R_MovingAbovePlate) }, [lastMotionActionResponse, setMealState]) + /* + * Create refs to store the interval for the food-on-fork detection timers. + * Note we need two timers, because the first timer set's remainingTime, whereas + * we can't set remainingTime in the second timer otherwise it will attempt to + * set state on an unmounted component. + **/ + const timerWasForFof = useRef(null) + const timerRef = useRef(null) + const finalTimerRef = useRef(null) + const clearTimer = useCallback(() => { + console.log('Clearing timer') + if (finalTimerRef.current) { + clearInterval(finalTimerRef.current) + finalTimerRef.current = null + } + if (timerRef.current) { + clearInterval(timerRef.current) + timerRef.current = null + setRemainingSeconds(null) + timerWasForFof.current = null + } + }, [setRemainingSeconds, timerRef, timerWasForFof, finalTimerRef]) + + /** + * Subscribe to the ROS Topic with the food-on-fork detection result. This is + * created in local state to avoid re-creating it upon every re-render. + */ + const foodOnForkDetectionCallback = useCallback( + (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) { + let callbackFn = null + if (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) { + 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') + clearTimer() + } + // Don't override an existing timer + if (!timerRef.current && callbackFn !== null) { + setRemainingSeconds(biteAcquisitionCheckAutoContinueSecs) + timerRef.current = setInterval(() => { + setRemainingSeconds((prev) => { + if (prev <= 1) { + clearTimer() + // In the remaining time, move above plate + finalTimerRef.current = setInterval(() => { + clearInterval(finalTimerRef.current) + callbackFn() + }, (prev - 1) * 1000) + return null + } else { + return prev - 1 + } + }) + }, 1000) + } + } + }, + [ + acquisitionSuccess, + acquisitionFailure, + biteAcquisitionCheckAutoContinue, + biteAcquisitionCheckAutoContinueProbThreshLower, + biteAcquisitionCheckAutoContinueProbThreshUpper, + biteAcquisitionCheckAutoContinueSecs, + finalTimerRef, + remainingSeconds, + clearTimer, + prevMealState, + setRemainingSeconds, + timerRef, + timerWasForFof + ] + ) + useEffect(() => { + let topic = subscribeToROSTopic( + ros.current, + FOOD_ON_FORK_DETECTION_TOPIC, + FOOD_ON_FORK_DETECTION_TOPIC_MSG, + foodOnForkDetectionCallback + ) + /** + * In practice, because the values passed in in the second argument of + * useEffect will not change on re-renders, this return statement will + * only be called when the component unmounts. + */ + return () => { + unsubscribeFromROSTopic(topic, foodOnForkDetectionCallback) + } + }, [foodOnForkDetectionCallback]) + + /** + * Toggles food-on-fork detection on the first time this component is rendered, + * but not upon additional re-renders. See here for more details on how `useEffect` + * achieves this goal: https://stackoverflow.com/a/69264685 + */ + useEffect(() => { + // Create a service request + let request = createROSServiceRequest({ data: true }) + // Call the service + let service = toggleFoodOnForkDetectionService.current + service.callService(request, (response) => console.log('Got toggle food on fork detection service response', response)) + + /** + * In practice, because the values passed in in the second argument of + * useEffect will not change on re-renders, this return statement will + * only be called when the component unmounts. + */ + return () => { + // Create a service request + let request = createROSServiceRequest({ data: false }) + // Call the service + service.callService(request, (response) => console.log('Got toggle food on fork detection service response', response)) + } + }, [toggleFoodOnForkDetectionService]) + /** * Get the ready for bite text to render. * @@ -173,18 +323,74 @@ const BiteAcquisitionCheck = () => { */ const fullPageView = useCallback(() => { return ( - - - {readyForBiteText()} - {readyForBiteButton()} + <> + +

+ { + clearTimer() + setBiteAcquisitionCheckAutoContinue(e.target.checked) + }} + style={{ transform: 'scale(2.0)', verticalAlign: 'middle', marginRight: '15px' }} + /> + Auto-continue +

+ + +

+ {biteAcquisitionCheckAutoContinue && prevMealState === MEAL_STATE.R_BiteAcquisition + ? remainingSeconds === null + ? 'Checking for food on fork...' + : timerWasForFof.current + ? 'Moving to your face in ' + remainingSeconds + ' secs' + : 'Moving above plate in ' + remainingSeconds + ' secs' + : ''} +

- - {reacquireBiteText()} - {reacquireBiteButton()} + + + {readyForBiteText()} + {readyForBiteButton()} + + + {reacquireBiteText()} + {reacquireBiteButton()} + - + ) - }, [dimension, reacquireBiteButton, reacquireBiteText, readyForBiteButton, readyForBiteText]) + }, [ + biteAcquisitionCheckAutoContinue, + clearTimer, + dimension, + prevMealState, + readyForBiteButton, + readyForBiteText, + reacquireBiteButton, + reacquireBiteText, + remainingSeconds, + setBiteAcquisitionCheckAutoContinue, + textFontSize + ]) // Render the component return <>{fullPageView()} diff --git a/feedingwebapp/src/Pages/Home/MealStates/BiteDone.jsx b/feedingwebapp/src/Pages/Home/MealStates/BiteDone.jsx index b2538deb..eafaccb1 100644 --- a/feedingwebapp/src/Pages/Home/MealStates/BiteDone.jsx +++ b/feedingwebapp/src/Pages/Home/MealStates/BiteDone.jsx @@ -1,13 +1,14 @@ // 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 { useROS, createROSService, createROSServiceRequest, subscribeToROSTopic, unsubscribeFromROSTopic } from '../../../ros/ros_helpers' import '../Home.css' import { useGlobalState, MEAL_STATE } from '../../GlobalState' -import { MOVING_STATE_ICON_DICT } from '../../Constants' +import { FOOD_ON_FORK_DETECTION_TOPIC, FOOD_ON_FORK_DETECTION_TOPIC_MSG, ROS_SERVICE_NAMES, MOVING_STATE_ICON_DICT } from '../../Constants' /** * The BiteDone component appears after the robot has moved to the user's mouth, @@ -15,8 +16,14 @@ import { MOVING_STATE_ICON_DICT } from '../../Constants' * moving back to above plate. */ const BiteDone = () => { + // Store the remining time before auto-continuing + const [remainingSeconds, setRemainingSeconds] = useState(null) // Get the relevant global variables const setMealState = useGlobalState((state) => state.setMealState) + const biteDoneAutoContinue = useGlobalState((state) => state.biteDoneAutoContinue) + const setBiteDoneAutoContinue = useGlobalState((state) => state.setBiteDoneAutoContinue) + const biteDoneAutoContinueSecs = useGlobalState((state) => state.biteDoneAutoContinueSecs) + const biteDoneAutoContinueProbThresh = useGlobalState((state) => state.biteDoneAutoContinueProbThresh) // Get icon image for move above plate let moveAbovePlateImage = MOVING_STATE_ICON_DICT[MEAL_STATE.R_MovingAbovePlate] // Get icon image for move to resting position @@ -28,11 +35,12 @@ const BiteDone = () => { // Indicator of how to arrange screen elements based on orientation let dimension = isPortrait ? 'column' : 'row' // Font size for text - let textFontSize = isPortrait ? '3vh' : '2.5vw' + let textFontSize = isPortrait ? 3 : 2.5 + let sizeSuffix = isPortrait ? 'vh' : 'vw' let buttonWidth = isPortrait ? '30vh' : '30vw' - let buttonHeight = isPortrait ? '20vh' : '20vw' + let buttonHeight = isPortrait ? '16vh' : '16vw' let iconWidth = isPortrait ? '28vh' : '28vw' - let iconHeight = isPortrait ? '18vh' : '18vw' + let iconHeight = isPortrait ? '14vh' : '14vw' /** * Callback function for when the user wants to move above plate. @@ -55,79 +63,245 @@ const BiteDone = () => { setMealState(MEAL_STATE.R_MovingFromMouth, MEAL_STATE.R_DetectingFace) }, [setMealState]) + /* + * Create refs to store the interval for the food-on-fork detection timers. + * Note we need two timers, because the first timer set's remainingTime, whereas + * we can't set remainingTime in the second timer otherwise it will attempt to + * set state on an unmounted component. + **/ + const timerRef = useRef(null) + const finalTimerRef = useRef(null) + const clearTimer = useCallback(() => { + console.log('Clearing timer') + if (finalTimerRef.current) { + clearInterval(finalTimerRef.current) + finalTimerRef.current = null + } + if (timerRef.current) { + clearInterval(timerRef.current) + timerRef.current = null + setRemainingSeconds(null) + } + }, [setRemainingSeconds, timerRef]) + + /** + * Connect to ROS, if not already connected. Put this in useRef to avoid + * re-connecting upon re-renders. + */ + const ros = useRef(useROS().ros) + + /** + * Subscribe to the ROS Topic with the food-on-fork detection result. This is + * created in local state to avoid re-creating it upon every re-render. + */ + const foodOnForkDetectionCallback = useCallback( + (message) => { + console.log('Got food-on-fork detection message', message) + if (biteDoneAutoContinue && message.status === 1) { + if (message.probability < biteDoneAutoContinueProbThresh) { + console.log('Auto-continuing in ', remainingSeconds, ' seconds') + // Don't override an existing timer + if (!timerRef.current) { + setRemainingSeconds(biteDoneAutoContinueSecs) + timerRef.current = setInterval(() => { + setRemainingSeconds((prev) => { + if (prev <= 1) { + clearTimer() + // In the remaining time, move above plate + finalTimerRef.current = setInterval(() => { + clearInterval(finalTimerRef.current) + moveAbovePlate() + }, (prev - 1) * 1000) + return null + } else { + return prev - 1 + } + }) + }, 1000) + } + } else { + console.log('Not auto-continuing') + clearTimer() + } + } + }, + [ + biteDoneAutoContinue, + biteDoneAutoContinueProbThresh, + biteDoneAutoContinueSecs, + finalTimerRef, + remainingSeconds, + clearTimer, + moveAbovePlate, + setRemainingSeconds, + timerRef + ] + ) + useEffect(() => { + let topic = subscribeToROSTopic( + ros.current, + FOOD_ON_FORK_DETECTION_TOPIC, + FOOD_ON_FORK_DETECTION_TOPIC_MSG, + foodOnForkDetectionCallback + ) + /** + * In practice, because the values passed in in the second argument of + * useEffect will not change on re-renders, this return statement will + * only be called when the component unmounts. + */ + return () => { + unsubscribeFromROSTopic(topic, foodOnForkDetectionCallback) + } + }, [foodOnForkDetectionCallback]) + + /** + * Create the ROS Service. This is created in local state to avoid re-creating + * it upon every re-render. + */ + let { serviceName, messageType } = ROS_SERVICE_NAMES[MEAL_STATE.U_BiteDone] + let toggleFoodOnForkDetectionService = useRef(createROSService(ros.current, serviceName, messageType)) + + /** + * Toggles food-on-fork detection on the first time this component is rendered, + * but not upon additional re-renders. See here for more details on how `useEffect` + * achieves this goal: https://stackoverflow.com/a/69264685 + */ + useEffect(() => { + // Create a service request + let request = createROSServiceRequest({ data: true }) + // Call the service + let service = toggleFoodOnForkDetectionService.current + service.callService(request, (response) => console.log('Got toggle food on fork detection service response', response)) + + /** + * In practice, because the values passed in in the second argument of + * useEffect will not change on re-renders, this return statement will + * only be called when the component unmounts. + */ + return () => { + // Create a service request + let request = createROSServiceRequest({ data: false }) + // Call the service + service.callService(request, (response) => console.log('Got toggle food on fork detection service response', response)) + } + }, [toggleFoodOnForkDetectionService]) + /** Get the full page view * * @returns {JSX.Element} the the full page view */ const fullPageView = useCallback(() => { return ( - - - {/* Ask the user whether they want to move to above plate position */} -

- Move above plate -

- {/* Icon to move above plate */} - - - - {/* Ask the user whether they want to move to resting position */} -

- Rest to the side + Auto-continue

- {/* Icon to move to resting position */} - - - {/* Ask the user whether they want to move to resting position */} -

- Move away from mouth + +

+ {biteDoneAutoContinue + ? remainingSeconds === null + ? 'Waiting for the fork to be empty' + : 'Moving away in ' + remainingSeconds + ' secs' + : ''}

- {/* Icon to move to resting position */} - - + + + {/* Ask the user whether they want to move to above plate position */} +

+ Move above plate +

+ {/* Icon to move above plate */} + + + + {/* Ask the user whether they want to move to resting position */} +

+ Rest to the side +

+ {/* Icon to move to resting position */} + + + + {/* Ask the user whether they want to move to resting position */} +

+ Move away from mouth +

+ {/* Icon to move to resting position */} + + + + ) }, [ + biteDoneAutoContinue, buttonHeight, buttonWidth, dimension, @@ -139,7 +313,11 @@ const BiteDone = () => { moveToRestingPositionImage, moveToStagingConfiguration, moveToStagingConfigurationImage, - textFontSize + remainingSeconds, + clearTimer, + setBiteDoneAutoContinue, + textFontSize, + sizeSuffix ]) // Render the component