Support actions in rosbridge protocol (#886)

Author: sea-bass

ROSBRIDGE_PROTOCOL.md

@@ -52,7 +52,7 @@ Optionally, a message can also provide an arbitrary string or integer ID:
 
 ```json
 { "op": "Example",
-  "id":"fred"
+  "id": "fred"
 }
 ```
 
@@ -81,16 +81,24 @@ Rosbridge status messages:
 
 ROS operations:
 
-  * **advertise** – advertise that you are publishing a topic
-  * **unadvertise** – stop advertising that you are publishing topic
-  * **publish** - a published ROS-message
-  * **subscribe** - a request to subscribe to a topic
-  * **unsubscribe** - a request to unsubscribe from a topic
-  * **call_service** - a service call
-  * **advertise_service** - advertise an external service server
-  * **unadvertise_service** - unadvertise an external service server
-  * **service_request** - a service request
-  * **service_response** - a service response
+  * Topics:
+    * **advertise** – advertise that you are publishing a topic
+    * **unadvertise** – stop advertising that you are publishing topic
+    * **publish** - a published ROS-message
+    * **subscribe** - a request to subscribe to a topic
+    * **unsubscribe** - a request to unsubscribe from a topic
+  * Services:
+    * **advertise_service** - advertise an external service server
+    * **unadvertise_service** - unadvertise an external service server
+    * **call_service** - a service call
+    * **service_response** - a service response
+  * Actions:
+   * **advertise_action** - advertise an external action server
+   * **unadvertise_action** - unadvertise an external action server
+   * **send_action_goal** - a goal sent to an action server
+   * **cancel_action_goal** - cancel an in-progress action goal
+   * **action_feedback** - feedback messages from an action server
+   * **action_result** - an action result
 
 In general, actions or operations that the client takes (such as publishing and
 subscribing) have opcodes which are verbs (subscribe, call_service, unadvertise
@@ -264,8 +272,7 @@ messages that already exist in the current version of rosbridge.
 
 #### 3.3.1 Advertise ( _advertise_ )
 
-If you wish to advertise that you are or will be publishing a topic, then use
-the advertise command.
+If you wish to advertise that you are or will be publishing a topic, then use the advertise command.
 
 ```json
 { "op": "advertise",
@@ -397,7 +404,31 @@ fragmentation size and publishing rate.
 If an id is provided, then only the corresponding subscription is unsubscribed.
 If no ID is provided, then all subscriptions are unsubscribed.
 
-#### 3.3.6 Call Service
+#### 3.3.6 Advertise Service
+
+```json
+{ "op": "advertise_service",
+  "type": <string>,
+  "service": <string>
+}
+```
+
+Advertises an external ROS service server. Requests come to the client via Call Service.
+
+ * **service** – the name of the service to advertise
+ * **type** – the advertised service message type
+
+#### 3.3.7 Unadvertise Service
+
+```json
+{ "op": "unadvertise_service",
+  "service": <string>
+}
+```
+
+#### 3.3.8 Call Service
+
+Calls a ROS service.
 
 ```json
 { "op": "call_service",
@@ -409,8 +440,6 @@ If no ID is provided, then all subscriptions are unsubscribed.
 }
 ```
 
-Calls a ROS service
-
  * **service** – the name of the service to call
  * **args** – if the service has no args, then args does not have to be
     provided, though an empty list is equally acceptable. Args should be a list
@@ -421,34 +450,15 @@ Calls a ROS service
  * **compression** – an optional string to specify the compression scheme to be
     used on messages. Valid values are "none" and "png"
 
-#### 3.3.7 Advertise Service
-
-```json
-{ "op": "advertise_service",
-  "type": <string>,
-  "service": <string>
-}
-```
-
-Advertises an external ROS service server. Requests come to the client via Call Service.
-
- * **service** – the name of the service to advertise
- * **type** – the advertised service message type
-
-#### 3.3.8 Unadvertise Service
-
-```json
-{ "op": "unadvertise_service",
-  "service": <string>
-}
-```
 
 Stops advertising an external ROS service server
 
  * **service** – the name of the service to unadvertise
 
 #### 3.3.9 Service Response
 
+A response to a ROS service call.
+
 ```json
 { "op": "service_response",
   (optional) "id": <string>,
@@ -458,20 +468,115 @@ Stops advertising an external ROS service server
 }
 ```
 
-A response to a ROS service call
-
  * **service** – the name of the service that was called
  * **values** – the return values. If the service had no return values, then
     this field can be omitted (and will be by the rosbridge server)
  * **id** – if an ID was provided to the service request, then the service
     response will contain the ID
  * **result** - return value of service callback. true means success, false failure.
 
+#### 3.3.10 Advertise Action
+
+Advertises an external ROS action server.
+
+```json
+{ "op": "advertise_action",
+  "type": <string>,
+  "action": <string>
+}
+```
+
+Goals come to the client via the Send Action Goal capability.
+
+ * **action** – the name of the action to advertise
+ * **type** – the advertised action message type
+
+#### 3.3.11 Unadvertise Action
+
+```json
+{ "op": "unadvertise_action",
+  "action": <string>
+}
+```
+
+#### 3.3.12 Send Action Goal
+
+Sends a goal to a ROS action server.
+
+```json
+{ "op": "send_action_goal",
+  (optional) "id": <string>,
+  "action": <string>,
+  "action_type: <string>,
+  (optional) "args": <list<json>>,
+  (optional) "feedback": <boolean>
+  (optional) "fragment_size": <int>,
+  (optional) "compression": <string>
+}
+```
+
+ * **action** – the name of the action to send a goal to
+ * **action_type** – the action message type
+ * **args** – if the goal has no args, then args does not have to be
+    provided, though an empty list is equally acceptable. Args should be a list of json objects representing the arguments to the service.
+ * **feedback** – if true, sends feedback messages over rosbridge. Defaults to false.
+ * **id** – an optional id to distinguish this goal handle
+ * **fragment_size** – the maximum size that the result and feedback messages can take before they are fragmented
+ * **compression** – an optional string to specify the compression scheme to be used on messages. Valid values are "none" and "png"
+
+#### 3.3.13 Cancel Action Goal
+
+Cancels an action goal.
+
+```json
+{ "op": "cancel_action_goal",
+  "id": <string>,
+  "action": <string>,
+}
+```
+
+The `id` field must match an already in-progress goal.
+
+#### 3.3.14 Action Feedback
+
+Used to send action feedback for a specific goal handle.
+
+```json
+{ "op": "action_feedback",
+  "id": <string>,
+  "action": <string>,
+  "values": <json>,
+}
+```
+
+The `id` field must match an already in-progress goal.
+
+#### 3.3.15 Action Result
+
+A result for a ROS action.
+
+```json
+{ "op": "action_result",
+  "id": <string>,
+  "action": <string>,
+  "values": <json>,
+  "result": <boolean>
+}
+```
+
+ * **action** – the name of the action that was executed
+ * **values** – the result values. If the service had no return values, then
+    this field can be omitted (and will be by the rosbridge server)
+ * **id** – if an ID was provided to the action goal, then the action result will contain the ID
+ * **result** - return value of the action. true means success, false failure.
+
+---
+
 ## 4 Further considerations
 
 Further considerations for the rosbridge protocol are listed below.
 
-### 4.1 Rosbridge psuedo-services
+### 4.1 Rosbridge pseudo-services
 
 Rosbridge no longer provides the ROS-api introspection pseudo services that it
 previously did. These are, for example rosbridge/topics and rosbridge/services.

rosbridge_library/package.xml

@@ -35,6 +35,7 @@
   <test_depend>builtin_interfaces</test_depend>
   <test_depend>diagnostic_msgs</test_depend>
   <test_depend>example_interfaces</test_depend>
+  <test_depend>control_msgs</test_depend>
   <test_depend>geometry_msgs</test_depend>
   <test_depend>nav_msgs</test_depend>
   <test_depend>sensor_msgs</test_depend>

rosbridge_library/src/rosbridge_library/capabilities/action_feedback.py

@@ -0,0 +1,73 @@
+# 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
+from rosbridge_library.protocol import Protocol
+
+
+class ActionFeedback(Capability):
+
+    action_feedback_msg_fields = [
+        (True, "action", str),
+        (False, "id", str),
+        (False, "values", dict),
+    ]
+
+    def __init__(self, protocol: Protocol) -> None:
+        # 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: dict) -> None:
+        # 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.",
+            )