Merge branch 'ros2' into dependabot/github_actions/actions/setup-pyth…

…on-5.0.0

.github/workflows/ci.yml

@@ -12,7 +12,7 @@ jobs:
         with:
           python-version: '3.8'
 
-      - uses: pre-commit/action@v3.0.0
+      - uses: pre-commit/action@v3.0.1
 
   test:
     strategy:

ROSBRIDGE_PROTOCOL.md

@@ -304,7 +304,7 @@ This stops advertising that you are publishing a topic.
   "topic": <string>
 }
 ```
-
+
  * **topic** – the string name of the topic being unadvertised
 
    * If the topic does not exist, a warning status message is sent and this
@@ -507,9 +507,9 @@ Sends a goal to a ROS action server.
 { "op": "send_action_goal",
   (optional) "id": <string>,
   "action": <string>,
-  "action_type: <string>,
+  "action_type": <string>,
   (optional) "args": <list<json>>,
-  (optional) "feedback": <boolean>
+  (optional) "feedback": <boolean>,
   (optional) "fragment_size": <int>,
   (optional) "compression": <string>
 }
@@ -531,7 +531,7 @@ Cancels an action goal.
 ```json
 { "op": "cancel_action_goal",
   "id": <string>,
-  "action": <string>,
+  "action": <string>
 }
 ```
 
@@ -545,7 +545,7 @@ Used to send action feedback for a specific goal handle.
 { "op": "action_feedback",
   "id": <string>,
   "action": <string>,
-  "values": <json>,
+  "values": <json>
 }
 ```
 
@@ -609,4 +609,3 @@ The meta-package will contain the following packages:
     tornado, a python server implementation.
  * **rosapi** – provides ROS services for various master API calls, such as
     listing all the topics, services, types currently in ROS
-

rosbridge_library/src/rosbridge_library/capabilities/advertise_action.py

@@ -35,6 +35,7 @@
 
 import rclpy
 from rclpy.action import ActionServer
+from rclpy.action.server import CancelResponse, ServerGoalHandle
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rosbridge_library.capability import Capability
 from rosbridge_library.internal import message_conversion
@@ -62,6 +63,7 @@ def __init__(
             get_action_class(action_type),
             action_name,
             self.execute_callback,
+            cancel_callback=self.cancel_callback,
             callback_group=ReentrantCallbackGroup(),  # https://github.com/ros2/rclpy/issues/834#issuecomment-961331870
         )
 
@@ -71,13 +73,16 @@ def next_id(self) -> int:
         return id
 
     async def execute_callback(self, goal: Any) -> Any:
+        """Action server goal callback function."""
         # generate a unique ID
         goal_id = f"action_goal:{self.action_name}:{self.next_id()}"
 
         def done_callback(fut: rclpy.task.Future()) -> None:
             if fut.cancelled():
                 goal.abort()
-                fut.set_exception(RuntimeError(f"Aborted goal {goal_id}"))
+                self.protocol.log("info", f"Aborted goal {goal_id}")
+                # Send an empty result to avoid stack traces
+                fut.set_result(get_action_class(self.action_type).Result())
             else:
                 goal.succeed()
 
@@ -103,6 +108,20 @@ def done_callback(fut: rclpy.task.Future()) -> None:
             del self.goal_futures[goal_id]
             del self.goal_handles[goal_id]
 
+    def cancel_callback(self, cancel_request: ServerGoalHandle) -> CancelResponse:
+        """Action server cancel callback function."""
+        for goal_id, goal_handle in self.goal_handles.items():
+            if cancel_request.goal_id == goal_handle.goal_id:
+                self.protocol.log("warning", f"Canceling action {goal_id}")
+                self.goal_futures[goal_id].cancel()
+                cancel_message = {
+                    "op": "cancel_action_goal",
+                    "id": goal_id,
+                    "action": self.action_name,
+                }
+                self.protocol.send(cancel_message)
+        return CancelResponse.ACCEPT
+
     def handle_feedback(self, goal_id: str, feedback: Any) -> None:
         """
         Called by the ActionFeedback capability to handle action feedback from the external client.
@@ -146,7 +165,10 @@ def graceful_shutdown(self) -> None:
             for future_id in self.goal_futures:
                 future = self.goal_futures[future_id]
                 future.set_exception(RuntimeError(f"Action {self.action_name} was unadvertised"))
-        self.action_server.destroy()
+
+        # Uncommenting this, you may get a segfault.
+        # See https://github.com/ros2/rclcpp/issues/2163#issuecomment-1850925883
+        # self.action_server.destroy()
 
 
 class AdvertiseAction(Capability):

rosbridge_library/src/rosbridge_library/internal/message_conversion.py

@@ -169,12 +169,12 @@ def extract_values(inst):
     return _from_inst(inst, rostype)
 
 
-def populate_instance(msg, inst):
+def populate_instance(msg, inst, clock=ROSClock()):
     """Returns an instance of the provided class, with its fields populated
     according to the values in msg"""
     inst_type = msg_instance_type_repr(inst)
 
-    return _to_inst(msg, inst_type, inst_type, inst)
+    return _to_inst(msg, inst_type, inst_type, clock, inst)
 
 
 def msg_instance_type_repr(msg_inst):
@@ -269,29 +269,29 @@ def _from_object_inst(inst, rostype):
     return msg
 
 
-def _to_inst(msg, rostype, roottype, inst=None, stack=[]):
+def _to_inst(msg, rostype, roottype, clock=ROSClock(), inst=None, stack=[]):
     # Check if it's uint8[], and if it's a string, try to b64decode
     for binary_type, expression in ros_binary_types_list_braces:
         if expression.sub(binary_type, rostype) in ros_binary_types:
             return _to_binary_inst(msg)
 
     # Check the type for time or rostime
     if rostype in ros_time_types:
-        return _to_time_inst(msg, rostype, inst)
+        return _to_time_inst(msg, rostype, clock, inst)
 
     # Check to see whether this is a primitive type
     if rostype in ros_primitive_types:
         return _to_primitive_inst(msg, rostype, roottype, stack)
 
     # Check whether we're dealing with a list type
     if inst is not None and type(inst) in list_types:
-        return _to_list_inst(msg, rostype, roottype, inst, stack)
+        return _to_list_inst(msg, rostype, roottype, clock, inst, stack)
 
     # Otherwise, the type has to be a full ros msg type, so msg must be a dict
     if inst is None:
         inst = ros_loader.get_message_instance(rostype)
 
-    return _to_object_inst(msg, rostype, roottype, inst, stack)
+    return _to_object_inst(msg, rostype, roottype, clock, inst, stack)
 
 
 def _to_binary_inst(msg):
@@ -307,11 +307,11 @@ def _to_binary_inst(msg):
     return bytes(bytearray(msg))
 
 
-def _to_time_inst(msg, rostype, inst=None):
+def _to_time_inst(msg, rostype, clock, inst=None):
     # Create an instance if we haven't been provided with one
 
     if rostype == "builtin_interfaces/Time" and msg == "now":
-        return ROSClock().now().to_msg()
+        return clock.now().to_msg()
 
     if inst is None:
         if rostype == "builtin_interfaces/Time":
@@ -353,7 +353,7 @@ def _to_primitive_inst(msg, rostype, roottype, stack):
     raise FieldTypeMismatchException(roottype, stack, rostype, msgtype)
 
 
-def _to_list_inst(msg, rostype, roottype, inst, stack):
+def _to_list_inst(msg, rostype, roottype, clock, inst, stack):
     # Typecheck the msg
     if type(msg) not in list_types:
         raise FieldTypeMismatchException(roottype, stack, rostype, type(msg))
@@ -378,18 +378,18 @@ def _to_list_inst(msg, rostype, roottype, inst, stack):
         rostype = re.search(bounded_array_tokens, rostype).group(1)
 
     # Call to _to_inst for every element of the list
-    return [_to_inst(x, rostype, roottype, None, stack) for x in msg]
+    return [_to_inst(x, rostype, roottype, clock, None, stack) for x in msg]
 
 
-def _to_object_inst(msg, rostype, roottype, inst, stack):
+def _to_object_inst(msg, rostype, roottype, clock, inst, stack):
 
     # Typecheck the msg
     if not isinstance(msg, dict):
         raise FieldTypeMismatchException(roottype, stack, rostype, type(msg))
 
     # Substitute the correct time if we're an std_msgs/Header
     if rostype in ros_header_types:
-        inst.stamp = ROSClock().now().to_msg()
+        inst.stamp = clock.now().to_msg()
 
     inst_fields = inst.get_fields_and_field_types()
 
@@ -404,7 +404,9 @@ def _to_object_inst(msg, rostype, roottype, inst, stack):
         field_rostype = inst_fields[field_name]
         field_inst = getattr(inst, field_name)
 
-        field_value = _to_inst(msg[field_name], field_rostype, roottype, field_inst, field_stack)
+        field_value = _to_inst(
+            msg[field_name], field_rostype, roottype, clock, field_inst, field_stack
+        )
 
         setattr(inst, field_name, field_value)
 

rosbridge_library/src/rosbridge_library/internal/publishers.py

@@ -155,7 +155,7 @@ def publish(self, msg):
         inst = self.msg_class()
 
         # Populate the instance, propagating any exceptions that may be thrown
-        message_conversion.populate_instance(msg, inst)
+        message_conversion.populate_instance(msg, inst, self.node_handle.get_clock())
 
         # Publish the message
         self.publisher.publish(inst)

rosbridge_library/src/rosbridge_library/internal/services.py

@@ -125,6 +125,7 @@ def call_service(
     service_type = service_names_and_types.get(service)
     if service_type is None:
         raise InvalidServiceException(service)
+
     # service_type is a tuple of types at this point; only one type is supported.
     if len(service_type) > 1:
         node_handle.get_logger().warning(f"More than one service type detected: {service_type}")
@@ -139,7 +140,10 @@ def call_service(
     client = node_handle.create_client(
         service_class, service, callback_group=ReentrantCallbackGroup()
     )
-    client.wait_for_service(server_timeout_time)
+
+    if not client.wait_for_service(server_timeout_time):
+        node_handle.destroy_client(client)
+        raise InvalidServiceException(service)
 
     future = client.call_async(inst)
     while rclpy.ok() and not future.done():

rosbridge_library/test/capabilities/test_service_capabilities.py

@@ -116,7 +116,7 @@ def test_call_advertised_service(self):
         loop_iterations = 0
         while self.received_message is None:
             rclpy.spin_once(self.node, timeout_sec=0.1)
-            time.sleep(0.5)
+            time.sleep(0.1)
             loop_iterations += 1
             if loop_iterations > 3:
                 self.fail("Timed out waiting for service call message.")