Add ros2 action client capability

ROSBRIDGE_PROTOCOL.md

@@ -467,6 +467,54 @@ A response to a ROS service call
     response will contain the ID
  * **result** - return value of service callback. true means success, false failure.
 
+ #### 3.3.10 Send Goal
+
+```json
+{ "op": "send_goal",
+  (optional) "id": <string>,
+  "action_name": <string>,
+  "action_type": <string>,
+  (optional) "feedback": <bool>,
+  (optional) "goal_msg": <list<json>>
+}
+```
+
+This command creates an action client with the specified action name and type if not already created; sends the goal msg to the action server and returns  the feedback and result of the goal.
+
+ * **action_name** – the name of the action to which the goal will be sent (ex. /navigate_to_pose)
+ * **action_type** – the type of the action (ex. /nav2_msgs/action/NavigateToPose)
+  * **feedback** – an optional boolean specifies whether to receive the feedback for this goal or not. defaults to true)
+ * **goal_msg** – if the Goal has no args, then goal msg does not have to be
+    provided, though an empty list is equally acceptable. goal msg should be a list of json objects representing the arguments to the action
+ * **id** – an optional id to distinguish this goal call
+
+ #### 3.3.11 Cancel Goal
+
+```json
+{ "op": "cancel_goal",
+  (optional) "id": <string>,
+  "action_name": <string>
+}
+```
+
+This command cancels all the executing and pending goals of the specified action and return the result of cancel call.
+
+ * **action_name** – the name of the action (ex. /navigate_to_pose)
+ * **id** – an optional id to distinguish this cancel goal call.
+
+ #### 3.3.12 Destroy Client
+
+```json
+{ "op": "destroy_client",
+  (optional) "id": <string>,
+  "action_name": <string>
+}
+```
+This command destroys the action client if the action client was created on earlier send_goal calls.
+
+ * **action_name** – the name of the action (ex. /navigate_to_pose)
+ * **id** – an optional id to distinguish this cancel goal call.
+
 ## 4 Further considerations
 
 Further considerations for the rosbridge protocol are listed below.

rosbridge_library/src/rosbridge_library/capabilities/action_client.py

@@ -0,0 +1,184 @@
+import fnmatch
+from functools import partial
+
+from rosbridge_library.capability import Capability
+from rosbridge_library.internal.actions import ActionClientHandle, GoalHandle
+from rosbridge_library.internal.message_conversion import extract_values
+
+
+class ActionClientRequests(Capability):
+
+    send_goal_msg_fields = [
+        (True, "action_name", str),
+        (True, "action_type", str),
+        (False, "feedback", bool),
+    ]
+
+    destroy_client_msg_fields = [(True, "action_name", str)]
+
+    cancel_goal_msg_fields = [(True, "action_name", str)]
+
+    actions_glob = None
+
+    def __init__(self, protocol):
+        # Call superclass constructor
+        Capability.__init__(self, protocol)
+
+        # Register the operations that this capability provides
+        protocol.register_operation("send_goal", self.send_goal)
+        protocol.register_operation("destroy_client", self.destroy_client)
+        protocol.register_operation("cancel_goal", self.cancel_goal)
+
+        self._action_clients = {}
+
+    def send_goal(self, msg):
+        # Check the args
+        self.basic_type_check(msg, self.send_goal_msg_fields)
+        action_name = msg.get("action_name")
+        action_type = msg.get("action_type")
+        goal_msg = msg.get("goal_msg", [])
+        feedback = msg.get("feedback", True)
+
+        if ActionClientRequests.actions_glob is not None and ActionClientRequests.actions_glob:
+            self.protocol.log(
+                "debug", "Action security glob enabled, checking action clients: " + action_name
+            )
+            match = False
+            for glob in ActionClientRequests.actions_glob:
+                if fnmatch.fnmatch(action_name, glob):
+                    self.protocol.log(
+                        "debug",
+                        "Found match with glob " + glob + ", creating Action client",
+                    )
+                    match = True
+                    break
+            if not match:
+                self.protocol.log(
+                    "warn",
+                    "No match found for action, cancelling creation of action client: "
+                    + action_name,
+                )
+                return
+        else:
+            self.protocol.log("debug", "No action security glob, not checking Action client.")
+
+        client_id = msg.get("id", None)
+        s_cb = partial(self._success, client_id, action_name)
+        e_cb = partial(self._failure, client_id, action_name)
+        if feedback:
+            f_cb = partial(self._feedback, client_id, action_name)
+        else:
+            f_cb = None
+
+        if action_name not in self._action_clients:
+            self._action_clients[action_name] = ActionClientHandle(
+                action_name, action_type, self.protocol.node_handle
+            )
+
+        GoalHandle(self._action_clients[action_name], goal_msg, s_cb, e_cb, f_cb).start()
+
+    def _success(self, cid, action_name, message):
+        outgoing_message = {
+            "op": "action_response",
+            "response_type": "result",
+            "name": action_name,
+            "values": message,
+        }
+        if cid is not None:
+            outgoing_message["id"] = cid
+        # TODO: fragmentation, compression
+        self.protocol.send(outgoing_message)
+
+    def _failure(self, cid, action_name, exc):
+        outgoing_message = {
+            "op": "action_response",
+            "response_type": "error",
+            "name": action_name,
+            "values": str(exc),
+        }
+        if cid is not None:
+            outgoing_message["id"] = cid
+        # TODO: fragmentation, compression
+        self.protocol.send(outgoing_message)
+
+    def _feedback(self, cid, action_name, message):
+        outgoing_message = {
+            "op": "action_response",
+            "response_type": "feedback",
+            "name": action_name,
+            "values": extract_values(message),
+        }
+        if cid is not None:
+            outgoing_message["id"] = cid
+        # TODO: fragmentation, compression
+        self.protocol.send(outgoing_message)
+
+    def destroy_client(self, msg):
+        self.basic_type_check(msg, self.destroy_client_msg_fields)
+        action_name = msg.get("action_name")
+
+        if ActionClientRequests.actions_glob is not None and ActionClientRequests.actions_glob:
+            self.protocol.log(
+                "debug",
+                "Action security glob enabled, checking action clients: " + action_name,
+            )
+            match = False
+            for glob in ActionClientRequests.actions_glob:
+                if fnmatch.fnmatch(action_name, glob):
+                    self.protocol.log(
+                        "debug",
+                        "Found match with glob " + glob + ", killing client",
+                    )
+                    match = True
+                    break
+            if not match:
+                self.protocol.log(
+                    "warn",
+                    "No match found for action client, cancelling destruction of action client: "
+                    + action_name,
+                )
+                return
+        else:
+            self.protocol.log("debug", "No action security glob, not checking Action client.")
+
+        if action_name not in self._action_clients:
+            self.protocol.log("info", "action client %s not available" % action_name)
+            return
+        self._action_clients[action_name].unregister()
+        del self._action_clients[action_name]
+
+        if len(self._action_clients) == 0:
+            self._action_clients.clear()
+
+        self.protocol.log("info", "Destroyed Action Client %s" % action_name)
+
+    def cancel_goal(self, msg):
+        self.basic_type_check(msg, self.cancel_goal_msg_fields)
+        action_name = msg.get("action_name")
+        cid = msg.get("id", None)
+
+        if action_name not in self._action_clients:
+            self.protocol.log("info", "action client %s not available" % action_name)
+            return
+
+        result = self._action_clients[action_name].cancel_goal_call()
+
+        outgoing_message = {
+            "op": "action_response",
+            "response_type": "cancel",
+            "name": action_name,
+            "values": result,
+        }
+        if cid is not None:
+            outgoing_message["id"] = cid
+        self.protocol.send(outgoing_message)
+
+        self.protocol.log("info", "cancelled goals of %s" % action_name)
+
+    def finish(self):
+        for clients in self._action_clients.values():
+            clients.unregister()
+        self._action_clients.clear()
+        self.protocol.unregister_operation("send_goal")
+        self.protocol.unregister_operation("destroy_client")
+        self.protocol.unregister_operation("cancel_goal")

rosbridge_library/src/rosbridge_library/internal/actions.py

@@ -0,0 +1,156 @@
+from threading import Thread
+
+import rclpy
+from action_msgs.msg import GoalStatus
+from action_msgs.srv import CancelGoal
+from rclpy.action import ActionClient
+from rosbridge_library.internal.message_conversion import (
+    extract_values,
+    populate_instance,
+)
+from rosbridge_library.internal.ros_loader import (
+    get_action_class,
+    get_action_goal_instance,
+)
+from unique_identifier_msgs.msg import UUID
+
+
+class InvalidActionException(Exception):
+    def __init__(self, actiontype):
+        Exception.__init__(self, "Action %s does not exist" % actiontype)
+
+
+class ActionClientHandle:
+    def __init__(self, action_name, action_type, node_handle):
+        self.action_name = action_name
+        self.action_type = action_type
+        self.node_handle = node_handle
+
+        # raise exception if action type specified is None
+        if self.action_type is None:
+            raise InvalidActionException(action_type)
+
+        # loads the class of action type
+        action_class = get_action_class(action_type)
+        self.action_client = ActionClient(node_handle, action_class, action_name)
+        self.node_handle.get_logger().info(
+            f" Created Action Client: {action_name} of type: {action_type}"
+        )
+
+        # create a client for cancel goal service call
+        self.cancel_client = self.node_handle.create_client(
+            CancelGoal, self.action_name + "/_action/cancel_goal"
+        )
+
+    def cancel_goal_call(self):
+        """
+        Sends a cancel goal service call.It cancels all active and pending goals
+         of the action server by providing zeros to both goal id and stamp.
+        """
+        msg = CancelGoal.Request()
+        msg.goal_info.goal_id = UUID(uuid=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
+        msg.goal_info.stamp.nanosec = 0
+        msg.goal_info.stamp.sec = 0
+
+        # create a call with the cancel request
+        result = self.cancel_client.call(msg)
+        if result is not None:
+            # Turn the response into JSON and pass to the callback
+            json_response = extract_values(result)
+        else:
+            raise Exception(result)
+
+        return json_response
+
+    def unregister(self):
+        # cancel goals if any before destroy the client
+        self.cancel_goal_call()
+        self.cancel_client.destroy()
+        self.action_client.destroy()
+
+
+class GoalHandle(Thread):
+    def __init__(
+        self, action_client, goal_msg, success_callback, error_callback, feedback_callback
+    ):
+        """
+        Create a goal handle for the specified action.
+        Use start() to start in a separate thread or run() to run in this thread.
+
+        Keyword Arguments:
+        -----------------
+        action_client -- the action client on which the goal is handled
+        goal_msg -- arguments to pass to the action.  Can be an
+        ordered list, or a dict of name-value pairs.  Anything else will be
+        treated as though no arguments were provided (which is still valid for
+        some kinds of action).
+        success_callback -- a callback to call with the JSON result of the
+        goal.
+        error_callback -- a callback to call if an error occurs. The
+        callback will be passed the exception that caused the failure of goal.
+        feedback_callback -- a callback to call with the feedback while the goal is executing if opted.
+        """
+        Thread.__init__(self)
+        self.daemon = True
+        self.goal_msg = goal_msg
+        self.success = success_callback
+        self.error = error_callback
+        self.feedback = feedback_callback
+        self.client = action_client
+
+    def run(self):
+        try:
+            # Call the action and pass the result to the success handler
+            self.success(self.start_goal(self.goal_msg))
+        except Exception as e:
+            # On error, just pass the exception to the error handler
+            self.error(e)
+
+    def args_to_action_goal_instance(self, inst, args):
+        """
+        Populate a action goal instance with the provided args.
+        args can be a dictionary of values, or a list, or None
+        Propagates any exceptions that may be raised.
+        """
+        msg = {}
+        if isinstance(args, dict):
+            msg = args
+        elif isinstance(args, list):
+            msg = dict(zip(inst.get_fields_and_field_types().keys(), args))
+
+        # Populate the provided instance, propagating any exceptions
+        populate_instance(msg, inst)
+
+    def start_goal(self, goal_msg):
+        if not self.client.action_client.wait_for_server(timeout_sec=10.0):
+            self.client.node_handle.get_logger().warning(
+                f" Timeout: Action Server for Client: {self.client.action_name} not available. Goal is ignored "
+            )
+            raise Exception("Action Server Not Available")
+
+        inst = get_action_goal_instance(self.client.action_type)
+        # Populate the goal instance with the provided goal args
+        self.args_to_action_goal_instance(inst, goal_msg)
+
+        # send the goal and wait for the goal future to be accepted
+        send_goal_future = self.client.action_client.send_goal_async(inst, self.feedback)
+        rclpy.spin_until_future_complete(self.client.node_handle, send_goal_future)
+        goal_handle = send_goal_future.result()
+        if not goal_handle.accepted:
+            raise Exception("Action Goal was rejected!")
+        self.client.node_handle.get_logger().info(
+            f"Goal is accepted by the action server: {self.client.action_name}."
+        )
+
+        # get the result
+        result = goal_handle.get_result()
+
+        # return the result of the goal if succeeded.
+        status = result.status
+        if status == GoalStatus.STATUS_SUCCEEDED:
+            # Turn the response into JSON and pass to the callback
+            json_response = extract_values(result.result)
+        else:
+            raise Exception(status)
+
+        return json_response