Skip to content

Commit ccafe3c

Browse files
amalnanavatiamalnanavati
authored
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
1 parent bbeb0da commit ccafe3c

File tree

8 files changed

+657
-118
lines changed

8 files changed

+657
-118
lines changed

‎feeding_web_app_ros2_test/feeding_web_app_ros2_test/FaceDetection.py

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -70,7 +70,6 @@ def __init__(
7070
self.camera_callback,
7171
1,
7272
)
73-
self.subscription # prevent unused variable warning
7473

7574
# Create the publishers
7675
self.publisher_results = self.create_publisher(

‎feeding_web_app_ros2_test/feeding_web_app_ros2_test/FoodOnForkDetection.py

Lines changed: 139 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,139 @@
1+
#!/usr/bin/env python3
2+
from ada_feeding_msgs.msg import FoodOnForkDetection
3+
from std_srvs.srv import SetBool
4+
import rclpy
5+
from rclpy.node import Node
6+
from sensor_msgs.msg import CompressedImage
7+
from threading import Lock
8+
9+
10+
class FoodOnForkDetectionNode(Node):
11+
def __init__(
12+
self,
13+
food_on_fork_detection_interval=90,
14+
num_images_with_food=90,
15+
):
16+
"""
17+
Initializes the FoodOnForkDetection node, which exposes a SetBool
18+
service that can be used to toggle the food on fork detection on or off and
19+
publishes information to the /food_on_fork_detection topic when food-on-fork
20+
detection is on.
21+
22+
After food_on_fork_detection_interval images without food, this dummy function
23+
detects food for num_images_with_food frames.
24+
25+
Parameters:
26+
----------
27+
food_on_fork_detection_interval: The number of frames between each food detection.
28+
num_images_with_food: The number of frames that must have a food in them.
29+
"""
30+
super().__init__("food_on_fork_detection")
31+
32+
# Internal variables to track when food should be detected
33+
self.food_on_fork_detection_interval = food_on_fork_detection_interval
34+
self.num_images_with_food = num_images_with_food
35+
self.num_consecutive_images_without_food = (
36+
self.food_on_fork_detection_interval
37+
) # Start predicting FoF
38+
self.num_consecutive_images_with_food = 0
39+
40+
# Keeps track of whether food on fork detection is on or not
41+
self.is_on = False
42+
self.is_on_lock = Lock()
43+
44+
# Create the service
45+
self.srv = self.create_service(
46+
SetBool,
47+
"toggle_food_on_fork_detection",
48+
self.toggle_food_on_fork_detection_callback,
49+
)
50+
51+
# Subscribe to the camera feed
52+
self.subscription = self.create_subscription(
53+
CompressedImage,
54+
"camera/color/image_raw/compressed",
55+
self.camera_callback,
56+
1,
57+
)
58+
59+
# Create the publishers
60+
self.publisher_results = self.create_publisher(
61+
FoodOnForkDetection, "food_on_fork_detection", 1
62+
)
63+
64+
def toggle_food_on_fork_detection_callback(self, request, response):
65+
"""
66+
Callback function for the SetBool service. Safely toggles
67+
the food on fork detection on or off depending on the request.
68+
"""
69+
self.get_logger().info("Incoming service request. turn_on: %s" % (request.data))
70+
if request.data:
71+
# Reset counters
72+
self.num_consecutive_images_without_food = (
73+
self.food_on_fork_detection_interval
74+
) # Start predicting FoF
75+
self.num_consecutive_images_with_food = 0
76+
# Turn on food-on-fork detection
77+
self.is_on_lock.acquire()
78+
self.is_on = True
79+
self.is_on_lock.release()
80+
response.success = True
81+
response.message = "Succesfully turned food-on-fork detection on"
82+
else:
83+
self.is_on_lock.acquire()
84+
self.is_on = False
85+
self.is_on_lock.release()
86+
response.success = True
87+
response.message = "Succesfully turned food-on-fork detection off"
88+
return response
89+
90+
def camera_callback(self, msg):
91+
"""
92+
Callback function for the camera feed. If food-on-fork detection is on, this
93+
function will detect food in the image and publish information about
94+
them to the /food_on_fork_detection topic.
95+
"""
96+
self.get_logger().debug("Received image")
97+
self.is_on_lock.acquire()
98+
is_on = self.is_on
99+
self.is_on_lock.release()
100+
if is_on:
101+
# Update the number of consecutive images with/without a food
102+
is_food_detected = False
103+
if self.num_consecutive_images_with_food == self.num_images_with_food:
104+
self.num_consecutive_images_without_food = 0
105+
self.num_consecutive_images_with_food = 0
106+
if (
107+
self.num_consecutive_images_without_food
108+
== self.food_on_fork_detection_interval
109+
):
110+
# Detect food on the fork
111+
self.num_consecutive_images_with_food += 1
112+
is_food_detected = True
113+
else:
114+
# Don't detect food
115+
self.num_consecutive_images_without_food += 1
116+
117+
# Publish the food-on-fork detection information
118+
food_on_fork_detection_msg = FoodOnForkDetection()
119+
food_on_fork_detection_msg.header = msg.header
120+
food_on_fork_detection_msg.probability = 1.0 if is_food_detected else 0.0
121+
food_on_fork_detection_msg.status = food_on_fork_detection_msg.SUCCESS
122+
food_on_fork_detection_msg.message = (
123+
"Food detected" if is_food_detected else "No food detected"
124+
)
125+
self.publisher_results.publish(food_on_fork_detection_msg)
126+
127+
128+
def main(args=None):
129+
rclpy.init(args=args)
130+
131+
food_on_fork_detection = FoodOnForkDetectionNode()
132+
133+
rclpy.spin(food_on_fork_detection)
134+
135+
rclpy.shutdown()
136+
137+
138+
if __name__ == "__main__":
139+
main()

‎feeding_web_app_ros2_test/launch/feeding_web_app_dummy_nodes_launch.xml

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
<arg name="run_web_bridge" default="true" description="Whether to run rosbridge and web_video_server" />
55
<arg name="run_food_detection" default="true" description="Whether to run the dummy food detectoion node" />
66
<arg name="run_face_detection" default="true" description="Whether to run the dummy face detection node" />
7+
<arg name="run_food_on_fork_detection" default="true" description="Whether to run the dummy food-on-fork detection node" />
78
<arg name="run_real_sense" default="true" description="Whether to run the dummy RealSense node" />
89
<arg name="run_motion" default="true" description="Whether to run the dummy motion nodes" />
910
<arg name="rgb_path" default="above_plate_2_rgb.jpg" description="The path to the RGB image/video to publish from the dummy node, relative to this node's share/data folder." />
@@ -24,7 +25,7 @@
2425
<remap from="~/aligned_depth" to="/camera/aligned_depth_to_color/image_raw"/>
2526
<remap from="~/camera_info" to="/camera/color/camera_info"/>
2627
<remap from="~/aligned_depth/camera_info" to="/camera/aligned_depth_to_color/camera_info"/>
27-
<param name="fps" value="30"/>
28+
<param name="fps" value="15"/>
2829
<param name="rgb_path" value="$(find-pkg-share feeding_web_app_ros2_test)/../data/$(var rgb_path)"/>
2930
<param name="depth_path" value="$(find-pkg-share feeding_web_app_ros2_test)/../data/$(var depth_path)"/>
3031
</node>
@@ -39,6 +40,11 @@
3940
<!-- Perception: The FaceDetection node -->
4041
<node pkg="feeding_web_app_ros2_test" exec="FaceDetection" name="FaceDetection"/>
4142
</group>
43+
44+
<group if="$(var run_food_on_fork_detection)">
45+
<!-- Perception: The FoodOnForkDetection node -->
46+
<node pkg="feeding_web_app_ros2_test" exec="FoodOnForkDetection" name="FoodOnForkDetection"/>
47+
</group>
4248

4349
<group if="$(var run_motion)">
4450
<!-- Motion: The MoveAbovePlate action -->

‎feeding_web_app_ros2_test/setup.py

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@
3636
"AcquireFoodClient = feeding_web_app_ros2_test.AcquireFoodClient:main",
3737
"DummyRealSense = feeding_web_app_ros2_test.DummyRealSense:main",
3838
"FaceDetection = feeding_web_app_ros2_test.FaceDetection:main",
39+
"FoodOnForkDetection = feeding_web_app_ros2_test.FoodOnForkDetection:main",
3940
"MoveAbovePlate = feeding_web_app_ros2_test.MoveAbovePlate:main",
4041
"MoveToRestingPosition = feeding_web_app_ros2_test.MoveToRestingPosition:main",
4142
"MoveToStagingConfiguration = feeding_web_app_ros2_test.MoveToStagingConfiguration:main",

‎feedingwebapp/src/Pages/Constants.js

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,8 @@ export const CAMERA_FEED_TOPIC = '/local/camera/color/image_raw/compressed'
5050
export const FACE_DETECTION_TOPIC = '/face_detection'
5151
export const FACE_DETECTION_TOPIC_MSG = 'ada_feeding_msgs/FaceDetection'
5252
export const FACE_DETECTION_IMG_TOPIC = '/face_detection_img/compressed'
53+
export const FOOD_ON_FORK_DETECTION_TOPIC = '/food_on_fork_detection'
54+
export const FOOD_ON_FORK_DETECTION_TOPIC_MSG = 'ada_feeding_msgs/FoodOnForkDetection'
5355
export const ROBOT_COMPRESSED_IMG_TOPICS = [CAMERA_FEED_TOPIC, FACE_DETECTION_IMG_TOPIC]
5456

5557
// 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] = {
106108
serviceName: 'toggle_face_detection',
107109
messageType: 'std_srvs/srv/SetBool'
108110
}
111+
ROS_SERVICE_NAMES[MEAL_STATE.U_BiteDone] = {
112+
serviceName: 'toggle_food_on_fork_detection',
113+
messageType: 'std_srvs/srv/SetBool'
114+
}
115+
ROS_SERVICE_NAMES[MEAL_STATE.U_BiteAcquisitionCheck] = {
116+
serviceName: 'toggle_food_on_fork_detection',
117+
messageType: 'std_srvs/srv/SetBool'
118+
}
109119
export { ROS_SERVICE_NAMES }
110120
export const CLEAR_OCTOMAP_SERVICE_NAME = 'clear_octomap'
111121
export const CLEAR_OCTOMAP_SERVICE_TYPE = 'std_srvs/srv/Empty'

‎feedingwebapp/src/Pages/GlobalState.jsx

Lines changed: 38 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -75,27 +75,6 @@ export const SETTINGS_STATE = {
7575
BITE_TRANSFER: 'BITE_TRANSFER'
7676
}
7777

78-
/**
79-
* The parameters that users can set (keys) and a list of human-readable values
80-
* they can take on.
81-
* - stagingPosition: Discrete options for where the robot should wait until
82-
* the user is ready.
83-
* - biteInitiation: Options for the modality the user wants to use to tell
84-
* the robot they are ready for a bite.
85-
* - TODO: Make these checkboxes instead -- users should be able to
86-
* enable multiple buttons if they so desire.
87-
* - biteSelection: Options for how the user wants to tell the robot what food
88-
* item they want next.
89-
*
90-
* TODO (amaln): When we connect this to ROS, each of these settings types and
91-
* value options will have to have corresponding rosparam names and value options.
92-
*/
93-
// export const SETTINGS = {
94-
// stagingPosition: ['In Front of Me', 'On My Right Side'],
95-
// biteInitiation: ['Open Mouth', 'Say "I am Ready"', 'Press Button'],
96-
// biteSelection: ['Name of Food', 'Click on Food']
97-
// }
98-
9978
/**
10079
* useGlobalState is a hook to store and manipulate web app state that we want
10180
* to persist across re-renders and refreshes. It won't persist if cookies are
@@ -129,6 +108,16 @@ export const useGlobalState = create(
129108
teleopIsMoving: false,
130109
// Flag to indicate whether to auto-continue after face detection
131110
faceDetectionAutoContinue: true,
111+
// Flag to indicate whether to auto-continue in bite done after food-on-fork detection
112+
biteDoneAutoContinue: false,
113+
biteDoneAutoContinueSecs: 3.0,
114+
biteDoneAutoContinueProbThresh: 0.25,
115+
// Flags to indicate whether to auto-continue in bite acquisition check based on food-on-fork
116+
// detection
117+
biteAcquisitionCheckAutoContinue: false,
118+
biteAcquisitionCheckAutoContinueSecs: 3.0,
119+
biteAcquisitionCheckAutoContinueProbThreshLower: 0.25,
120+
biteAcquisitionCheckAutoContinueProbThreshUpper: 0.75,
132121
// Whether the settings bite transfer page is currently at the user's face
133122
// or not. This is in the off-chance that the mealState is not at the user's
134123
// face, the settings page is, and the user refreshes -- the page should
@@ -141,11 +130,6 @@ export const useGlobalState = create(
141130
// How much the video on the Bite Selection page should be zoomed in.
142131
biteSelectionZoom: 1.0,
143132

144-
// Settings values
145-
// stagingPosition: SETTINGS.stagingPosition[0],
146-
// biteInitiation: SETTINGS.biteInitiation[0],
147-
// biteSelection: SETTINGS.biteSelection[0],
148-
149133
// Setters for global state
150134
setAppPage: (appPage) =>
151135
set(() => ({
@@ -196,6 +180,34 @@ export const useGlobalState = create(
196180
set(() => ({
197181
faceDetectionAutoContinue: faceDetectionAutoContinue
198182
})),
183+
setBiteDoneAutoContinue: (biteDoneAutoContinue) =>
184+
set(() => ({
185+
biteDoneAutoContinue: biteDoneAutoContinue
186+
})),
187+
setBiteDoneAutoContinueSecs: (biteDoneAutoContinueSecs) =>
188+
set(() => ({
189+
biteDoneAutoContinueSecs: biteDoneAutoContinueSecs
190+
})),
191+
setBiteDoneAutoContinueProbThresh: (biteDoneAutoContinueProbThresh) =>
192+
set(() => ({
193+
biteDoneAutoContinueProbThresh: biteDoneAutoContinueProbThresh
194+
})),
195+
setBiteAcquisitionCheckAutoContinue: (biteAcquisitionCheckAutoContinue) =>
196+
set(() => ({
197+
biteAcquisitionCheckAutoContinue: biteAcquisitionCheckAutoContinue
198+
})),
199+
setBiteAcquisitionCheckAutoContinueSecs: (biteAcquisitionCheckAutoContinueSecs) =>
200+
set(() => ({
201+
biteAcquisitionCheckAutoContinueSecs: biteAcquisitionCheckAutoContinueSecs
202+
})),
203+
setBiteAcquisitionCheckAutoContinueProbThreshLower: (biteAcquisitionCheckAutoContinueProbThreshLower) =>
204+
set(() => ({
205+
biteAcquisitionCheckAutoContinueProbThreshLower: biteAcquisitionCheckAutoContinueProbThreshLower
206+
})),
207+
setBiteAcquisitionCheckAutoContinueProbThreshUpper: (biteAcquisitionCheckAutoContinueProbThreshUpper) =>
208+
set(() => ({
209+
biteAcquisitionCheckAutoContinueProbThreshUpper: biteAcquisitionCheckAutoContinueProbThreshUpper
210+
})),
199211
setBiteTransferPageAtFace: (biteTransferPageAtFace) =>
200212
set(() => ({
201213
biteTransferPageAtFace: biteTransferPageAtFace
@@ -204,18 +216,6 @@ export const useGlobalState = create(
204216
set(() => ({
205217
biteSelectionZoom: biteSelectionZoom
206218
}))
207-
// setStagingPosition: (stagingPosition) =>
208-
// set(() => ({
209-
// stagingPosition: stagingPosition
210-
// })),
211-
// setBiteInitiation: (biteInitiation) =>
212-
// set(() => ({
213-
// biteInitiation: biteInitiation
214-
// })),
215-
// setBiteSelection: (biteSelection) =>
216-
// set(() => ({
217-
// biteSelection: biteSelection
218-
// }))
219219
}),
220220
{ name: 'ada_web_app_global_state' }
221221
)

0 commit comments

Comments
 (0)