Added dummy TableDetection and web app toggling of table detection (#130)

Co-authored-by: Sriram Kutty <sriramku@outlook.com>

Author: amalnanavati

feeding_web_app_ros2_test/feeding_web_app_ros2_test/TableDetection.py

@@ -0,0 +1,103 @@
+#!/usr/bin/env python3
+from builtin_interfaces.msg import Time
+from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
+from std_srvs.srv import SetBool
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import CompressedImage
+from std_msgs.msg import Header
+from threading import Lock
+
+
+class TableDetectionNode(Node):
+    DEFAULT_POSE_STAMPED = PoseStamped(
+        header=Header(
+            stamp=Time(sec=0, nanosec=0),
+            frame_id="root",
+        ),
+        # NOTE: This pose should be the same as the default pose in
+        # `ada_planning_scene.yaml`
+        pose=Pose(
+            position=Point(x=0.08, y=-0.5, z=-0.48),
+            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
+        ),
+    )
+
+    def __init__(self):
+        """
+        Initializes the TableDetectionNode.
+        """
+        super().__init__("table_detection")
+
+        # Keeps track of whether table detection is on or not
+        self.is_on = False
+        self.is_on_lock = Lock()
+
+        # Create the service
+        self.srv = self.create_service(
+            SetBool,
+            "toggle_table_detection",
+            self.toggle_table_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(
+            PoseStamped, "table_detection", 1
+        )
+
+    def toggle_table_detection_callback(self, request, response):
+        """
+        Callback function for the SetBool service. Safely toggles
+        the table detection on or off depending on the request.
+        """
+        self.get_logger().info("Incoming service request. turn_on: %s" % (request.data))
+        if request.data:
+            # Turn on table detection
+            self.is_on_lock.acquire()
+            self.is_on = True
+            self.is_on_lock.release()
+            response.success = True
+            response.message = "Succesfully turned table detection on"
+        else:
+            self.is_on_lock.acquire()
+            self.is_on = False
+            self.is_on_lock.release()
+            response.success = True
+            response.message = "Succesfully turned table detection off"
+        return response
+
+    def camera_callback(self, msg):
+        """
+        Callback function for the camera feed. If table detection is on, this
+        function will detect the table in the image and publish information about
+        them to the /table_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:
+            TableDetectionNode.DEFAULT_POSE_STAMPED.header.stamp = msg.header.stamp
+            self.publisher_results.publish(TableDetectionNode.DEFAULT_POSE_STAMPED)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    table_detection = TableDetectionNode()
+
+    rclpy.spin(table_detection)
+
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

feeding_web_app_ros2_test/launch/feeding_web_app_dummy_nodes_launch.xml

@@ -5,6 +5,7 @@
   <arg name="run_food_detection" default="true" description="Whether to run the dummy food detectoion node" />
   <arg name="run_face_detection" default="true" description="Whether to run the dummy face detection node" />
   <arg name="run_food_on_fork_detection" default="true" description="Whether to run the dummy food-on-fork detection node" />
+  <arg name="run_table_detection" default="true" description="Whether to run the dummy table detection node" />
   <arg name="run_real_sense" default="true" description="Whether to run the dummy RealSense node" />
   <arg name="run_motion" default="true"  description="Whether to run the dummy motion nodes" />
   <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." />
@@ -45,6 +46,11 @@
     <!-- Perception: The FoodOnForkDetection node -->
     <node pkg="feeding_web_app_ros2_test" exec="FoodOnForkDetection" name="FoodOnForkDetection"/>
   </group>
+
+  <group if="$(var run_table_detection)">
+    <!-- Perception: The TableDetection node -->
+    <node pkg="feeding_web_app_ros2_test" exec="TableDetection" name="TableDetection"/>
+  </group>
 
   <group if="$(var run_motion)">
     <!-- Motion: The MoveAbovePlate action -->

feeding_web_app_ros2_test/setup.py

@@ -47,6 +47,7 @@
             "MoveFromMouthToRestingPosition = feeding_web_app_ros2_test.MoveFromMouthToRestingPosition:main",
             "MoveToStowLocation = feeding_web_app_ros2_test.MoveToStowLocation:main",
             "SegmentFromPoint = feeding_web_app_ros2_test.SegmentFromPoint:main",
+            "TableDetection = feeding_web_app_ros2_test.TableDetection:main",
             # Scripts for the "TestROS" component
             "listener = feeding_web_app_ros2_test.subscriber:main",
             "reverse_string = feeding_web_app_ros2_test.reverse_string_service:main",

feedingwebapp/src/Pages/Constants.js

@@ -97,6 +97,10 @@ export const START_JOINT_CONTROLLER_ACTION_TYPE = 'ada_feeding_msgs/action/Trigg
  * the service name and the message type
  */
 let ROS_SERVICE_NAMES = {}
+ROS_SERVICE_NAMES[MEAL_STATE.U_BiteSelection] = {
+  serviceName: 'toggle_table_detection',
+  messageType: 'std_srvs/srv/SetBool'
+}
 ROS_SERVICE_NAMES[MEAL_STATE.R_DetectingFace] = {
   serviceName: 'toggle_face_detection',
   messageType: 'std_srvs/srv/SetBool'

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

@@ -9,7 +9,14 @@ import PropTypes from 'prop-types'
 
 // Local Imports
 import '../Home.css'
-import { useROS, createROSActionClient, callROSAction, destroyActionClient } from '../../../ros/ros_helpers'
+import {
+  useROS,
+  createROSActionClient,
+  callROSAction,
+  createROSService,
+  createROSServiceRequest,
+  destroyActionClient
+} from '../../../ros/ros_helpers'
 import { useWindowSize, convertRemToPixels } from '../../../helpers'
 import MaskButton from '../../../buttons/MaskButton'
 import {
@@ -19,6 +26,7 @@ import {
   ROS_ACTION_STATUS_SUCCEED,
   ROS_ACTION_STATUS_ABORT,
   ROS_ACTION_STATUS_CANCELED,
+  ROS_SERVICE_NAMES,
   SEGMENTATION_STATUS_SUCCESS,
   MOVING_STATE_ICON_DICT
 } from '../../Constants'
@@ -82,8 +90,15 @@ const BiteSelection = (props) => {
    * Create the ROS Action Client. This is created in useRef to avoid
    * re-creating it upon re-renders.
    */
-  let { actionName, messageType } = ROS_ACTIONS_NAMES[MEAL_STATE.U_BiteSelection]
-  let segmentFromPointAction = useRef(createROSActionClient(ros.current, actionName, messageType))
+  let actionDetails = ROS_ACTIONS_NAMES[MEAL_STATE.U_BiteSelection]
+  let segmentFromPointAction = useRef(createROSActionClient(ros.current, actionDetails.actionName, actionDetails.messageType))
+
+  /**
+   * Create the ROS Service to toggle on/off table detection. This is created in
+   * local state to avoid re-creating it upon every re-render.
+   */
+  let serviceDetails = ROS_SERVICE_NAMES[MEAL_STATE.U_BiteSelection]
+  let toggleTableDetectionService = useRef(createROSService(ros.current, serviceDetails.serviceName, serviceDetails.messageType))
 
   /**
    * Callback function for when the user indicates that they are done with their
@@ -212,14 +227,27 @@ const BiteSelection = (props) => {
   )
 
   /**
-   * Cancel any running actions when the component unmounts
+   * Cancel any running actions when the component unmounts. Also, toggle on table
+   * detection when the component first mounts and toggle it off when the component
+   * unmounts.
    */
   useEffect(() => {
+    // Create a service request
+    let request = createROSServiceRequest({ data: true })
+    // Call the service
+    let service = toggleTableDetectionService.current
+    service.callService(request, (response) => console.log('Got toggle table detection service response', response))
+
     let action = segmentFromPointAction.current
     return () => {
+      // Create a service request
+      let request = createROSServiceRequest({ data: false })
+      // Call the service
+      service.callService(request, (response) => console.log('Got toggle face detection service response', response))
+      // Destroy the action client
       destroyActionClient(action)
     }
-  }, [segmentFromPointAction])
+  }, [segmentFromPointAction, toggleTableDetectionService])
 
   /** Get the continue button when debug mode is enabled
    *