Create Dummy ROS2 Nodes for MoveAbovePlate, Integrates into App

feeding_web_app_ros2_msgs/package.xml

@@ -5,7 +5,7 @@
   <version>0.0.0</version>
   <description>Defines the message types used by `feeding_web_app_ros2_test`.</description>
   <maintainer email="[email protected]">Amal Nanavati</maintainer>
-  <license>TODO: License declaration</license>
+  <license>BSD-3-Clause</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <test_depend>ament_lint_auto</test_depend>

feeding_web_app_ros2_test/feeding_web_app_ros2_test/MoveAbovePlate.py

@@ -0,0 +1,20 @@
+#!/usr/bin/env python3
+from ada_feeding_msgs.action import MoveTo
+from feeding_web_app_ros2_test.MoveToDummy import MoveToDummy
+import rclpy
+from rclpy.executors import MultiThreadedExecutor
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    move_above_plate = MoveToDummy("MoveAbovePlate", MoveTo)
+
+    # Use a MultiThreadedExecutor to enable processing goals concurrently
+    executor = MultiThreadedExecutor()
+
+    rclpy.spin(move_above_plate, executor=executor)
+
+
+if __name__ == "__main__":
+    main()

feeding_web_app_ros2_test/feeding_web_app_ros2_test/MoveToDummy.py

@@ -0,0 +1,223 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
+from rclpy.node import Node
+import threading
+import time
+
+
+class MoveToDummy(Node):
+    def __init__(
+        self,
+        name,
+        action_class,
+        send_feedback_hz=10,
+        dummy_plan_time=2.5,
+        dummy_motion_time=10.0,
+    ):
+        """
+        Initialize the MoveToDummy action node.
+
+        Parameters
+        ----------
+        name: The name of the action server.
+        action_class: The action class to use for the action server.
+        send_feedback_hz: The target frequency at which to send feedback.
+        dummy_plan_time: How many seconds this dummy node should spend in planning.
+        dummy_motion_time: How many seconds this dummy node should spend in motion.
+        """
+        super().__init__(name)
+
+        self.send_feedback_hz = send_feedback_hz
+        self.dummy_plan_time = dummy_plan_time
+        self.dummy_motion_time = dummy_motion_time
+        self.action_class = action_class
+
+        self.active_goal_request = None
+
+        self._action_server = ActionServer(
+            self,
+            action_class,
+            name,
+            self.execute_callback,
+            goal_callback=self.goal_callback,
+            cancel_callback=self.cancel_callback,
+        )
+
+    def goal_callback(self, goal_request):
+        """
+        Accept a goal if this action does not already have an active goal,
+        else reject.
+
+        Parameters
+        ----------
+        goal_request: The goal request message.
+        """
+        self.get_logger().info("Received goal request")
+        if self.active_goal_request is None:
+            self.get_logger().info("Accepting goal request")
+            self.active_goal_request = goal_request
+            return GoalResponse.ACCEPT
+        self.get_logger().info("Rejecting goal request")
+        return GoalResponse.REJECT
+
+    def cancel_callback(self, goal_handle):
+        """
+        Always accept client requests to cancel the active goal. Note that this
+        function should not actually impelement the cancel; that is handled in
+        `execute_callback`
+
+        Parameters
+        ----------
+        goal_handle: The goal handle.
+        """
+        self.get_logger().info("Received cancel request, accepting")
+        return CancelResponse.ACCEPT
+
+    def plan(self, plan, success):
+        """
+        A dummy thread for planning to the target position. This thread
+        will sleep for `self.dummy_plan_time` sec and then set the plan to None.
+
+        Parameters
+        ----------
+        plan: A mutable object, which will contain the plan once the thread has
+              finished. For this dummy thread, it contains None.
+        success: A mutable object, which will contain the success status once
+                 the thread has finished.
+        """
+        time.sleep(self.dummy_plan_time)
+        plan.append(None)
+        success[0] = True
+
+    def move(self, plan, success):
+        """
+        A dummy thread for moving the robot arm along the plan. This thread
+        will sleep for `self.dummy_motion_time` sec and then return success.
+
+        Parameters
+        ----------
+        plan: Contains the plan.
+        success: A mutable object, which will contain the success status once
+                 the thread has finished.
+        """
+        time.sleep(self.dummy_motion_time)
+        success[0] = True
+
+    async def execute_callback(self, goal_handle):
+        """
+        First, plan to the target position. Then, move to that position.
+        As a "dummy node," this specific node will spend `self.dummy_plan_time`
+        sec in planning and `self.dummy_motion_time` sec in motion.
+
+        NOTE: In the actual (not dummy) implementation, we will be calling a
+        ROS action defined by MoveIt to do both planning and execution. Because
+        actions are non-blocking, we won't need to break off separate threads.
+        Using separate threads is an artiface of using time.sleep() in this
+        dummy implementation.
+        """
+        self.get_logger().info("Executing goal...%s" % (goal_handle,))
+
+        # Load the feedback parameters
+        feedback_rate = self.create_rate(self.send_feedback_hz)
+        feedback_msg = self.action_class.Feedback()
+
+        # Start the planning thread
+        plan = []
+        plan_success = [False]
+        planning_thread = threading.Thread(
+            target=self.plan, args=(plan, plan_success), daemon=True
+        )
+        planning_thread.start()
+        planning_start_time = self.get_clock().now()
+        is_planning = True
+
+        # Create (but don't yet start) the motion thread
+        is_moving = False
+        motion_success = [False]
+        motion_thread = threading.Thread(
+            target=self.move, args=(plan, motion_success), daemon=True
+        )
+
+        # Monitor the planning and motion threads, and send feedback
+        while rclpy.ok() and (is_planning or is_moving):
+            # Check if there is a cancel request
+            if goal_handle.is_cancel_requested:
+                self.get_logger().info("Goal canceled")
+                goal_handle.canceled()
+                result = self.action_class.Result()
+                result.status = result.STATUS_CANCELED
+                self.active_goal_request = None  # Clear the active goal
+                return result
+
+            # Check if the planning thread has finished
+            if is_planning:
+                if not planning_thread.is_alive():
+                    is_planning = False
+                    if plan_success[0]:  # Plan succeeded
+                        self.get_logger().info(
+                            "Planning succeeded, proceeding to motion"
+                        )
+                        # Start the motion thread
+                        motion_thread.start()
+                        motion_start_time = self.get_clock().now()
+                        is_moving = True
+                        continue
+                    else:  # Plan failed
+                        self.get_logger().info("Planning failed, aborting")
+                        # Abort the goal
+                        goal_handle.abort()
+                        result = self.action_class.Result()
+                        result.status = result.STATUS_PLANNING_FAILED
+                        self.active_goal_request = None  # Clear the active goal
+                        return result
+
+            # Check if the motion thread has finished
+            if is_moving:
+                if not motion_thread.is_alive():
+                    is_moving = False
+                    if motion_success[0]:
+                        self.get_logger().info("Motion succeeded, returning")
+                        # Succeed the goal
+                        goal_handle.succeed()
+                        result = self.action_class.Result()
+                        result.status = result.STATUS_SUCCESS
+                        self.active_goal_request = None  # Clear the active goal
+                        return result
+                    else:
+                        self.get_logger().info("Motion failed, aborting")
+                        # Abort the goal
+                        goal_handle.abort()
+                        result = self.action_class.Result()
+                        result.status = result.STATUS_MOTION_FAILED
+                        self.active_goal_request = None  # Clear the active goal
+                        return result
+
+            # Send feedback
+            feedback_msg.is_planning = is_planning
+            if is_planning:
+                feedback_msg.planning_time = (
+                    self.get_clock().now() - planning_start_time
+                ).to_msg()
+            elif is_moving:
+                # TODO: In the actual (not dummy) implementation, this should
+                # return the distance (not time) the robot has yet to move.
+                feedback_msg.motion_initial_distance = self.dummy_motion_time
+                elapsed_time = self.get_clock().now() - motion_start_time
+                elapsed_time_float = elapsed_time.nanoseconds / 1.0e9
+                feedback_msg.motion_curr_distance = (
+                    self.dummy_motion_time - elapsed_time_float
+                )
+            self.get_logger().info("Feedback: %s" % feedback_msg)
+            goal_handle.publish_feedback(feedback_msg)
+
+            # Sleep for the specified feedback rate
+            feedback_rate.sleep()
+
+        # If we get here, something went wrong
+        self.get_logger().info("Unknown error, aborting")
+        goal_handle.abort()
+        result = self.action_class.Result()
+        result.status = result.STATUS_UNKNOWN
+        self.active_goal_request = None  # Clear the active goal
+        return result

feeding_web_app_ros2_test/package.xml

@@ -5,7 +5,7 @@
   <version>0.0.0</version>
   <description>A minimal ROS publisher, subscriber, service, and action to use to test the web app.</description>
   <maintainer email="[email protected]">Amal Nanavati</maintainer>
-  <license>TODO: License declaration</license>
+  <license>BSD-3-Clause</license>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

feeding_web_app_ros2_test/setup.py

@@ -15,10 +15,13 @@
     maintainer="Amal Nanavati",
     maintainer_email="[email protected]",
     description="A minimal ROS publisher, subscriber, service, and action to use to test the web app.",
-    license="TODO: License declaration",
+    license="BSD-3-Clause",
     tests_require=["pytest"],
     entry_points={
         "console_scripts": [
+            # Scripts for the main app
+            "MoveAbovePlate = feeding_web_app_ros2_test.MoveAbovePlate: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",
             "sort_by_character_frequency = feeding_web_app_ros2_test.sort_by_character_frequency_action:main",

feedingwebapp/README.md

@@ -14,6 +14,7 @@ The overall user flow for this robot can be seen below.
 - [Node.js](https://nodejs.org/en/download/package-manager)
 - [ROS2 Humble](https://docs.ros.org/en/humble/Installation.html)
 - [PRL fork of rosbridge_suite](https://github.com/personalrobotics/rosbridge_suite). This fork enables rosbridge_suite to communicate with ROS2 actions.
+- [ada_feeding (branch: ros2-devel)](https://github.com/personalrobotics/ada_feeding/tree/ros2-devel).
 
 ## Getting Started in Computer
 
@@ -31,14 +32,29 @@ The overall user flow for this robot can be seen below.
   - Note that if you're not running the robot code alongside the app, set [`debug = true` in `App.jsx`](https://github.com/personalrobotics/feeding_web_interface/tree/main/feedingwebapp/src/App.jsx#L17) to be able to move past screens where the app is waiting on the robot. Since the robot is not yet connected, the default is `debug = true`
 3. Use a web browser to navigate to `localhost:3000` to see the application.
 
+#### Launching Dummy Nodes
+This repository includes several dummy nodes that match the interface that the robot nodes will use. By running the dummy nodes alongside the app, we can test the app's communication with the robot even without actual robot code running.
+
+The below instructions are for `MoveAbovePlate`; we will add to the instructions as more dummy nodes get implemented.
+1. Navigate to your ROS2 workspace: `cd {path/to/your/ros2/workspace}`
+2. Build your workspace: `colcon build`
+3. Launch rosbridge: `source install/setup.bash; ros2 launch rosbridge_server rosbridge_websocket_launch.xml`
+4. In another terminal, run the MoveAbovePlate action: `source install/setup.bash; ros2 run feeding_web_app_ros2_test MoveAbovePlate`
+5. In another terminal, navigate to the web app folder: `cd {path/to/feeding_web_interface}/feedingwebapp`
+6. Start the app: `npm start`
+7. Use a web browser to navigate to `localhost:3000`.
+
+You should now see that the web browser is connected to ROS. Further, you should see that when the `MoveAbovePlate` page starts, it should call the action (exactly once), and render feedback. "Pause" should cancel the action, and "Resume" should re-call it. Refreshing the page should cancel the action. When the action returns success, the app should automatically transition to the next page.
+
 ### Usage (Test ROS)
 There is a special page in the app intended for developers to: (a) test that their setup of the app and ROS2 enables the two to communicate as expected; and (b) gain familiarity with the library of ROS helper functions we use in the web app (see [TechDocumentation.md](https://github.com/personalrobotics/feeding_web_interface/tree/main/feedingwebapp/TechDocumentation.md)). Below are instructions to use this page:
 1. Navigate to your ROS2 workspace: `cd {path/to/your/ros2/workspace}`
 2. Build your workspace: `colcon build`
-3. Launch rosbridge: `ros2 launch rosbridge_server rosbridge_websocket_launch.xml`
-4. In another terminal, navigate to the web app folder: `cd {path/to/feeding_web_interface}/feedingwebapp`
-5. Start the app: `npm start`
-6. Use a web browser to navigate to `localhost:3000/test_ros`.
+3. Source your workspace: `source install/setup.bash`
+4. Launch rosbridge: `ros2 launch rosbridge_server rosbridge_websocket_launch.xml`
+5. In another terminal, navigate to the web app folder: `cd {path/to/feeding_web_interface}/feedingwebapp`
+6. Start the app: `npm start`
+7. Use a web browser to navigate to `localhost:3000/test_ros`.
 
 The following are checks to ensure the app is interacting with ROS as expected. If any of them fails, you'll have to do additional troubleshooting to get the web app and ROS2 smoothly communicating with each other.
 1. First, check if the page says `Connected` at the top of the screen. If not, the web app is not communicating with ROS2.

feedingwebapp/TechDocumentation.md

@@ -19,6 +19,14 @@ All functions to interact with ROS are defined in [`ros_helpers.jsx`](https://gi
 
 Although the web app's settings menu seem parameter-esque it is not a good idea to update settings by getting/setting ROS parameters. That is because ROS2 parameters are owned by individual nodes, and do not persist beyond the lifetime of a node. Further, we likely want to run downstream code after updating any setting (e.g., writing it to a file as backup), which cannot easily be done when setting ROS parameters. Therefore, **updating settings should be done by ROS service calls, not by getting/setting ROS parameters**. Hence, `ros_helpers.jsx` does not even implement parameter getting/setting.
 
+### Using ROS Services, Actions, etc. Within React
+
+React, by default, will re-render a component any time local state changes. In other words, it will re-run all code in the function that defines that component. However, this doesn't work well for ROS Services, Actions, etc. because: (a) we don't want to keep re-calling the service/action each time the UI re-renders; and (b) we don't want to keep re-creating a service/action client each time the UI re-renders. Therefore, it is crucial to use React Hooks intentionally and to double and triple check that the service/action is only getting once, to avoid bugs.
+
+These resources that can help you understand React Hooks in general:
+- https://medium.com/@guptagaruda/react-hooks-understanding-component-re-renders-9708ddee9928
+- https://stackoverflow.com/a/69264685
+
 ## Frequently Asked Questions (FAQ)
 
 Q: Why are we using global state as opposed to a [`Router`](https://www.w3schools.com/react/react_router.asp) to decide which app components to render?

feedingwebapp/src/Pages/Constants.js

@@ -1,4 +1,5 @@
 import { MEAL_STATE } from './GlobalState'
+
 // The RealSense's default video stream is 640x480
 export const REALSENSE_WIDTH = 640
 export const REALSENSE_HEIGHT = 480
@@ -21,3 +22,28 @@ FOOTER_STATE_ICON_DICT[MEAL_STATE.R_MovingToStagingLocation] = '/robot_state_img
 FOOTER_STATE_ICON_DICT[MEAL_STATE.R_MovingToMouth] = '/robot_state_imgs/move_to_mouth_position.svg'
 FOOTER_STATE_ICON_DICT[MEAL_STATE.R_StowingArm] = '/robot_state_imgs/stowing_arm_position.svg'
 export { FOOTER_STATE_ICON_DICT }
+
+// For states that call ROS actions, this dictionary contains
+// the action name and the message type
+let ROS_ACTIONS_NAMES = {}
+ROS_ACTIONS_NAMES[MEAL_STATE.R_MovingAbovePlate] = {
+  actionName: 'MoveAbovePlate',
+  messageType: 'ada_feeding_msgs/action/MoveTo'
+}
+export { ROS_ACTIONS_NAMES }
+
+// The meaning of the status that motion actions return in their results.
+// These should match the action definition(s)
+export const MOTION_STATUS_SUCCESS = 0
+export const MOTION_STATUS_PLANNING_FAILED = 1
+export const MOTION_STATUS_MOTION_FAILED = 2
+export const MOTION_STATUS_CANCELED = 3
+export const MOTION_STATUS_UNKNOWN = 99
+
+// The meaning of ROS Action statuses.
+// https://docs.ros2.org/latest/api/rclpy/api/actions.html#rclpy.action.server.GoalEvent
+export const ROS_ACTION_STATUS_EXECUTE = '1'
+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'