Add unadvertise action capability

sea-bass · sea-bass · commit 63806c078506 · 2023-10-31T19:21:30.000-04:00
diff --git a/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py b/rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py
@@ -87,7 +87,6 @@ def execute_callback(self, goal):
 
         while not future.done():
             time.sleep(self.sleep_time)
-
         result = future.result()
         goal.succeed()
         del self.goal_futures[goal_id]
@@ -125,7 +124,7 @@ def graceful_shutdown(self):
             )
             for future_id in self.goal_futures:
                 future = self.goal_futures[future_id]
-                future.set_exception(RuntimeError(f"Goal {self.action_name} was unadvertised"))
+                future.set_exception(RuntimeError(f"Action {self.action_name} was unadvertised"))
         self.action_server.destroy()
 
 
diff --git a/rosbridge_library/src/rosbridge_library/capabilities/unadvertise_action.py b/rosbridge_library/src/rosbridge_library/capabilities/unadvertise_action.py
@@ -0,0 +1,88 @@
+# 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.
+
+import fnmatch
+
+from rosbridge_library.capability import Capability
+
+
+class UnadvertiseAction(Capability):
+
+    actions_glob = None
+
+    def __init__(self, protocol):
+        # Call superclass constructor
+        Capability.__init__(self, protocol)
+
+        # Register the operations that this capability provides
+        protocol.register_operation("unadvertise_action", self.unadvertise_action)
+
+    def unadvertise_action(self, message):
+        # parse the message
+        action_name = message["action"]
+
+        if UnadvertiseAction.actions_glob is not None and UnadvertiseAction.actions_glob:
+            self.protocol.log(
+                "debug",
+                f"Action security glob enabled, checking action: {action_name}",
+            )
+            match = False
+            for glob in UnadvertiseAction.actions_glob:
+                if fnmatch.fnmatch(action_name, glob):
+                    self.protocol.log(
+                        "debug",
+                        "Found match with glob " + glob + ", continuing action unadvertisement...",
+                    )
+                    match = True
+                    break
+            if not match:
+                self.protocol.log(
+                    "warn",
+                    f"No match found for action, cancelling action unadvertisement for:{action_name}",
+                )
+                return
+        else:
+            self.protocol.log(
+                "debug",
+                "No action security glob, not checking action unadvertisement...",
+            )
+
+        # unregister action in ROS
+        if action_name in self.protocol.external_action_list.keys():
+            self.protocol.external_action_list[action_name].graceful_shutdown()
+            del self.protocol.external_action_list[action_name]
+            self.protocol.log("info", f"Unadvertised action {action_name}")
+        else:
+            self.protocol.log(
+                "error",
+                f"Action {action_name} has not been advertised via rosbridge, can't unadvertise.",
+            )
diff --git a/rosbridge_library/src/rosbridge_library/rosbridge_protocol.py b/rosbridge_library/src/rosbridge_library/rosbridge_protocol.py
@@ -41,6 +41,7 @@
 from rosbridge_library.capabilities.send_action_goal import SendActionGoal
 from rosbridge_library.capabilities.service_response import ServiceResponse
 from rosbridge_library.capabilities.subscribe import Subscribe
+from rosbridge_library.capabilities.unadvertise_action import UnadvertiseAction
 from rosbridge_library.capabilities.unadvertise_service import UnadvertiseService
 from rosbridge_library.protocol import Protocol
 
@@ -61,6 +62,7 @@ class RosbridgeProtocol(Protocol):
         ActionFeedback,
         ActionResult,
         SendActionGoal,
+        UnadvertiseAction,
     ]
 
     print("registered capabilities (classes):")
diff --git a/rosbridge_library/test/capabilities/test_action_capabilities.py b/rosbridge_library/test/capabilities/test_action_capabilities.py
@@ -13,6 +13,7 @@
 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
+from rosbridge_library.capabilities.unadvertise_action import UnadvertiseAction
 from rosbridge_library.internal.exceptions import (
     InvalidArgumentException,
     MissingArgumentException,
@@ -36,7 +37,7 @@ def setUp(self):
         # being sent
         self.proto.send = self.local_send_cb
         self.advertise = AdvertiseAction(self.proto)
-        # self.unadvertise = UnadvertiseService(self.proto)
+        self.unadvertise = UnadvertiseAction(self.proto)
         self.result = ActionResult(self.proto)
         self.send_goal = SendActionGoal(self.proto)
         self.feedback = ActionFeedback(self.proto)
@@ -198,6 +199,64 @@ def test_execute_advertised_action(self):
         self.assertEqual(self.received_message["op"], "action_result")
         self.assertEqual(self.received_message["values"]["result"]["sequence"], [1, 1, 2, 3, 5])
 
+    @unittest.skip("Currently raises an exception not catchable by unittest, need to fix this")
+    def test_unadvertise_action(self):
+        # Advertise the action
+        action_path = "/fibonacci_3"
+        advertise_msg = loads(
+            dumps(
+                {
+                    "op": "advertise_action",
+                    "type": "example_interfaces/Fibonacci",
+                    "action": action_path,
+                }
+            )
+        )
+        self.received_message = None
+        self.advertise.advertise_action(advertise_msg)
+
+        # Send a goal to the advertised action using rosbridge
+        self.received_message = None
+        goal_msg = loads(
+            dumps(
+                {
+                    "op": "call_service",
+                    "id": "foo",
+                    "action": action_path,
+                    "action_type": "example_interfaces/Fibonacci",
+                    "args": {"order": 5},
+                }
+            )
+        )
+        Thread(target=self.send_goal.send_action_goal, args=(goal_msg,)).start()
+
+        loop_iterations = 0
+        while self.received_message is None:
+            time.sleep(0.5)
+            loop_iterations += 1
+            if loop_iterations > 3:
+                self.fail("Timed out waiting for action goal message.")
+
+        self.assertIsNotNone(self.received_message)
+        self.assertTrue("op" in self.received_message)
+        self.assertTrue(self.received_message["op"] == "send_action_goal")
+        self.assertTrue("id" in self.received_message)
+
+        # Now unadvertise the action
+        # TODO: This raises an exception, likely because of the following rclpy issue:
+        # https://github.com/ros2/rclpy/issues/1098
+        unadvertise_msg = loads(dumps({"op": "unadvertise_action", "action": action_path}))
+        self.received_message = None
+        self.unadvertise.unadvertise_action(unadvertise_msg)
+
+        loop_iterations = 0
+        while self.received_message is None:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+            time.sleep(0.5)
+            loop_iterations += 1
+            if loop_iterations > 3:
+                self.fail("Timed out waiting for unadvertise action message.")
+
 
 if __name__ == "__main__":
     unittest.main()
diff --git a/rosbridge_library/test/capabilities/test_service_capabilities.py b/rosbridge_library/test/capabilities/test_service_capabilities.py
@@ -152,7 +152,6 @@ def test_call_advertised_service(self):
         self.assertEqual(self.received_message["op"], "service_response")
         self.assertTrue(self.received_message["result"])
 
-    @unittest.skip("This test currently raises an exception, need to fix this")
     def test_unadvertise_with_live_request(self):
         # Advertise the service
         service_path = "/set_bool_3"
@@ -198,19 +197,21 @@ def test_unadvertise_with_live_request(self):
         # Now unadvertise the service
         # TODO: This raises an exception, likely because of the following rclpy issue:
         # https://github.com/ros2/rclpy/issues/1098
-        response_msg = loads(dumps({"op": "unadvertise_service", "service": service_path}))
+        unadvertise_msg = loads(dumps({"op": "unadvertise_service", "service": service_path}))
         self.received_message = None
-        self.unadvertise.unadvertise_service(response_msg)
+        self.unadvertise.unadvertise_service(unadvertise_msg)
 
-        loop_iterations = 0
-        while self.received_message is None:
-            rclpy.spin_once(self.node, timeout_sec=0.1)
-            time.sleep(0.5)
-            loop_iterations += 1
-            if loop_iterations > 3:
-                self.fail("Timed out waiting for unadvertise service message.")
+        with self.assertRaises(RuntimeError) as context:
+            loop_iterations = 0
+            while self.received_message is None:
+                rclpy.spin_once(self.node, timeout_sec=0.1)
+                time.sleep(0.5)
+                loop_iterations += 1
+                if loop_iterations > 3:
+                    self.fail("Timed out waiting for unadvertise service message.")
 
-        self.assertFalse(self.received_message is None)
-        self.assertTrue("op" in self.received_message)
-        self.assertEqual(self.received_message["op"], "service_response")
-        self.assertFalse(self.received_message["result"])
+            self.assertTrue(f"Service {service_path} was unadvertised" in context.exception)
+
+
+if __name__ == "__main__":
+    unittest.main()
