Implement action feedback capabilities

Author: sea-bass

rosbridge_library/src/rosbridge_library/capabilities/action_feedback.py

@@ -0,0 +1,72 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2023, PickNik Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the copyright holder nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from rosbridge_library.capability import Capability
+from rosbridge_library.internal import message_conversion, ros_loader
+
+
+class ActionFeedback(Capability):
+
+    action_feedback_msg_fields = [
+        (True, "action", str),
+        (False, "id", str),
+        (False, "values", dict),
+    ]
+
+    def __init__(self, protocol):
+        # Call superclass constructor
+        Capability.__init__(self, protocol)
+
+        # Register the operations that this capability provides
+        protocol.register_operation("action_feedback", self.action_feedback)
+
+    def action_feedback(self, message):
+        # Typecheck the args
+        self.basic_type_check(message, self.action_feedback_msg_fields)
+
+        # check for the action
+        action_name = message["action"]
+        if action_name in self.protocol.external_action_list:
+            action_handler = self.protocol.external_action_list[action_name]
+            # parse the message
+            goal_id = message["id"]
+            values = message["values"]
+            # create a message instance
+            feedback = ros_loader.get_action_feedback_instance(action_handler.action_type)
+            message_conversion.populate_instance(values, feedback)
+            # pass along the feedback
+            action_handler.handle_feedback(goal_id, feedback)
+        else:
+            self.protocol.log(
+                "error",
+                f"Action {action_name} has not been advertised via rosbridge.",
+            )

rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py

@@ -47,6 +47,8 @@ class AdvertisedActionHandler:
 
     def __init__(self, action_name, action_type, protocol, sleep_time=0.001):
         self.goal_futures = {}
+        self.goal_handles = {}
+
         self.action_name = action_name
         self.action_type = action_type
         self.protocol = protocol
@@ -70,6 +72,7 @@ def execute_callback(self, goal):
         goal_id = f"action_goal:{self.action_name}:{self.next_id()}"
 
         future = rclpy.task.Future()
+        self.goal_handles[goal_id] = goal
         self.goal_futures[goal_id] = future
 
         # build a request to send to the external client
@@ -88,8 +91,20 @@ def execute_callback(self, goal):
         result = future.result()
         goal.succeed()
         del self.goal_futures[goal_id]
+        del self.goal_handles[goal_id]
         return result
 
+
+    def handle_feedback(self, goal_id, feedback):
+        """
+        Called by the ActionFeedback capability to handle action feedback from the external client.
+        """
+        if goal_id in self.goal_handles:
+            self.goal_handles[goal_id].publish_feedback(feedback)
+        else:
+            self.protocol.log("warning", f"Received action feedback for unrecognized id: {goal_id}")
+
+
     def handle_result(self, goal_id, res):
         """
         Called by the ActionResult capability to handle an action result from the external client.

rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py

@@ -111,8 +111,7 @@ def send_action_goal(self, message):
         # Create the callbacks
         s_cb = partial(self._success, cid, action, fragment_size, compression)
         e_cb = partial(self._failure, cid, action)
-        feedback = True  # TODO: Implement
-        if feedback:
+        if message.get("feedback", False):
             f_cb = partial(self._feedback, cid, action)
         else:
             f_cb = None
@@ -156,7 +155,6 @@ def _feedback(self, cid, action, message):
         if cid is not None:
             outgoing_message["id"] = cid
         # TODO: fragmentation, compression
-        print(outgoing_message)
         self.protocol.send(outgoing_message)
 
 

rosbridge_library/test/capabilities/test_action_capabilities.py

@@ -4,9 +4,13 @@
 from json import dumps, loads
 from threading import Thread
 
+from example_interfaces.action._fibonacci import Fibonacci_FeedbackMessage
+
 import rclpy
-from rclpy.executors import SingleThreadedExecutor
+from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, ReliabilityPolicy, QoSProfile
+from rosbridge_library.capabilities.action_feedback import ActionFeedback
 from rosbridge_library.capabilities.action_result import ActionResult
 from rosbridge_library.capabilities.advertise_action import AdvertiseAction
 from rosbridge_library.capabilities.send_action_goal import SendActionGoal
@@ -20,7 +24,7 @@
 class TestActionCapabilities(unittest.TestCase):
     def setUp(self):
         rclpy.init()
-        self.executor = SingleThreadedExecutor()
+        self.executor = MultiThreadedExecutor(num_threads=2)
         self.node = Node("test_action_capabilities")
         self.executor.add_node(self.node)
 
@@ -36,6 +40,7 @@ def setUp(self):
         # self.unadvertise = UnadvertiseService(self.proto)
         self.result = ActionResult(self.proto)
         self.send_goal = SendActionGoal(self.proto)
+        self.feedback = ActionFeedback(self.proto)
         self.received_message = None
         self.log_entries = []
 
@@ -48,9 +53,11 @@ def tearDown(self):
         rclpy.shutdown()
 
     def local_send_cb(self, msg):
-        print(f"Received: {msg}")
         self.received_message = msg
 
+    def feedback_subscriber_cb(self, msg):
+        self.latest_feedback = msg
+
     def mock_log(self, loglevel, message, _=None):
         self.log_entries.append((loglevel, message))
 
@@ -102,7 +109,6 @@ def test_execute_advertised_action(self):
         )
         self.received_message = None
         self.advertise.advertise_action(advertise_msg)
-        # rclpy.spin_once(self.node, timeout_sec=0.1)
 
         # Send a goal to the advertised action using rosbridge
         self.received_message = None
@@ -131,7 +137,40 @@ def test_execute_advertised_action(self):
         self.assertTrue(self.received_message["op"] == "send_action_goal")
         self.assertTrue("id" in self.received_message)
 
-        # TODO: Send feedback
+        # Send feedback message
+        self.latest_feedback = None
+        sub_qos_profile = QoSProfile(
+            depth=10,
+            durability=DurabilityPolicy.VOLATILE,
+            reliability=ReliabilityPolicy.RELIABLE,
+        )
+        self.subscription = self.node.create_subscription(
+            Fibonacci_FeedbackMessage,
+            f"{action_path}/_action/feedback",
+            self.feedback_subscriber_cb,
+            sub_qos_profile)
+        time.sleep(0.1)
+
+        feedback_msg = loads(
+            dumps(
+                {
+                    "op": "action_feedback",
+                    "action": action_path,
+                    "id": self.received_message["id"],
+                    "values": {"sequence": [1, 1, 2]},
+                }
+            )
+        )
+        self.feedback.action_feedback(feedback_msg)
+        loop_iterations = 0
+        while self.latest_feedback is None:
+            time.sleep(0.5)
+            loop_iterations += 1
+            if loop_iterations > 3:
+                self.fail("Timed out waiting for action feedback message.")
+
+        self.assertIsNotNone(self.latest_feedback)
+        self.assertEqual(list(self.latest_feedback.feedback.sequence), [1, 1, 2])
 
         # Now send the result
         result_msg = loads(