Updates to ROS2 support

.github/workflows/test-ros2.yml

@@ -50,7 +50,7 @@ jobs:
       - name: Set up docker containers
         run: |
           docker build -t gramaziokohler/rosbridge:integration_tests_ros2 ./docker/ros2
-          docker run -d -p 9090:9090 --name rosbridge gramaziokohler/rosbridge:integration_tests_ros2 /bin/bash -c "ros2 launch /integration-tests.launch"
+          docker run -d -p 9090:9090 --name rosbridge gramaziokohler/rosbridge:integration_tests_ros2 /bin/bash -c "ros2 launch /integration-tests-launch.py"
           docker ps -a
       - name: Run linter
         run: |

CHANGELOG.rst

@@ -11,12 +11,17 @@ Unreleased
 ----------
 
 **Added**
+
 * Added ROS 2 action client support.
+* Added ``wait_goal`` method to ``ActionClient`` to block until a goal reaches a terminal state.
+* Moved ``actionlib`` module to ``roslibpy.ros1`` namespace.
 
 **Changed**
 
 **Fixed**
 
+* Fixed KeyError in ROS 2 ActionClient when receiving action results with different message formats from rosbridge.
+
 **Deprecated**
 
 **Removed**

docker/ros2/Dockerfile

@@ -8,13 +8,15 @@ RUN apt-get update && apt-get install -y \
     ros-${ROS_DISTRO}-rosbridge-suite \
     # ros-${ROS_DISTRO}-tf2-web-republisher \
     # ros-${ROS_DISTRO}-ros-tutorials \
-    # ros-${ROS_DISTRO}-actionlib-tutorials \
+    ros-${ROS_DISTRO}-demo-nodes-py \
+    ros-${ROS_DISTRO}-example-interfaces \
     --no-install-recommends \
     # Clear apt-cache to reduce image size
     && rm -rf /var/lib/apt/lists/*
 
-# Copy launch
-COPY ./integration-tests.launch /
+# Copy launch and example action server
+COPY ./integration-tests-launch.py /
+COPY ./fibonacci_server.py /
 
 EXPOSE 9090
 

docker/ros2/fibonacci_server.py

@@ -0,0 +1,132 @@
+# Copyright 2019 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import threading
+import time
+
+from example_interfaces.action import Fibonacci
+
+import rclpy
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import ExternalShutdownException
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+
+
+class MinimalActionServer(Node):
+    """Minimal action server that processes one goal at a time."""
+
+    def __init__(self):
+        super().__init__('minimal_action_server')
+        self._goal_handle = None
+        self._goal_lock = threading.Lock()
+        self._action_server = ActionServer(
+            self,
+            Fibonacci,
+            'fibonacci',
+            execute_callback=self.execute_callback,
+            goal_callback=self.goal_callback,
+            handle_accepted_callback=self.handle_accepted_callback,
+            cancel_callback=self.cancel_callback,
+            callback_group=ReentrantCallbackGroup())
+        self.get_logger().info('Starting fibonacci action server..')
+
+    def destroy(self):
+        self._action_server.destroy()
+        super().destroy_node()
+
+    def goal_callback(self, goal_request):
+        """Accept or reject a client request to begin an action."""
+        self.get_logger().info('Received goal request')
+        return GoalResponse.ACCEPT
+
+    def handle_accepted_callback(self, goal_handle):
+        with self._goal_lock:
+            # This server only allows one goal at a time
+            if self._goal_handle is not None and self._goal_handle.is_active:
+                self.get_logger().info('Aborting previous goal')
+                # Abort the existing goal
+                self._goal_handle.abort()
+            self._goal_handle = goal_handle
+
+        goal_handle.execute()
+
+    def cancel_callback(self, goal):
+        """Accept or reject a client request to cancel an action."""
+        self.get_logger().info('Received cancel request')
+        return CancelResponse.ACCEPT
+
+    def execute_callback(self, goal_handle):
+        """Execute the goal."""
+        self.get_logger().info('Executing goal...')
+
+        # Append the seeds for the Fibonacci sequence
+        feedback_msg = Fibonacci.Feedback()
+        feedback_msg.sequence = [0, 1]
+
+        # Start executing the action
+        for i in range(1, goal_handle.request.order):
+            # If goal is flagged as no longer active (ie. another goal was accepted),
+            # then stop executing
+            if not goal_handle.is_active:
+                self.get_logger().info('Goal aborted')
+                return Fibonacci.Result()
+
+            if goal_handle.is_cancel_requested:
+                goal_handle.canceled()
+                self.get_logger().info('Goal canceled')
+                return Fibonacci.Result()
+
+            # Update Fibonacci sequence
+            feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i-1])
+
+            self.get_logger().info('Publishing feedback: {0}'.format(feedback_msg.sequence))
+
+            # Publish the feedback
+            goal_handle.publish_feedback(feedback_msg)
+
+            # Sleep for demonstration purposes
+            time.sleep(1)
+
+        with self._goal_lock:
+            if not goal_handle.is_active:
+                self.get_logger().info('Goal aborted')
+                return Fibonacci.Result()
+
+            goal_handle.succeed()
+
+        # Populate result message
+        result = Fibonacci.Result()
+        result.sequence = feedback_msg.sequence
+
+        self.get_logger().info('Returning result: {0}'.format(result.sequence))
+
+        return result
+
+
+def main(args=None):
+    try:
+        rclpy.init(args=args)
+        action_server = MinimalActionServer()
+
+        # We use a MultiThreadedExecutor to handle incoming goal requests concurrently
+        executor = MultiThreadedExecutor()
+        rclpy.spin(action_server, executor=executor)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+
+
+if __name__ == '__main__':
+    main()

docker/ros2/integration-tests-launch.py

@@ -0,0 +1,22 @@
+from launch import LaunchDescription
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch.actions import ExecuteProcess, IncludeLaunchDescription
+
+def generate_launch_description():
+    return LaunchDescription([
+        # Start rosbridge_websocket
+        IncludeLaunchDescription(
+            PathJoinSubstitution([
+                FindPackageShare('rosbridge_server'),
+                'launch',
+                'rosbridge_websocket_launch.xml'
+            ])
+        ),
+
+        # Start fibonacci_server.py with python3
+        ExecuteProcess(
+            cmd=['python3', "/fibonacci_server.py"],
+            output='screen'
+        )
+    ])

docs/files/ros2-action-client.py

@@ -11,7 +11,7 @@ def result_callback(msg):
     print("Action result:", msg)
 
 def feedback_callback(msg):
-    print(f"Action feedback: {msg['partial_sequence']}")
+    print(f"Action feedback: {msg['sequence']}")
 
 def fail_callback(msg):
     print(f"Action failed: {msg}")
@@ -23,7 +23,8 @@ def test_action_success(action_client):
     global result
     result = None
 
-    action_client.send_goal(roslibpy.ActionGoal({"order": 8}),
+    goal = roslibpy.Goal({"order": 8})
+    action_client.send_goal(goal,
                                 result_callback,
                                 feedback_callback,
                                 fail_callback)
@@ -44,7 +45,8 @@ def test_action_cancel(action_client):
     global result
     result = None
 
-    goal_id = action_client.send_goal(roslibpy.ActionGoal({"order": 8}),
+    goal = roslibpy.Goal({"order": 8})
+    goal_id = action_client.send_goal(goal,
                                     result_callback,
                                     feedback_callback,
                                     fail_callback)
@@ -66,7 +68,7 @@ def test_action_cancel(action_client):
 
     action_client = roslibpy.ActionClient(client,
                                         "/fibonacci",
-                                      "custom_action_interfaces/action/Fibonacci")
+                                      "example_interfaces/action/Fibonacci")
     print("\n** Starting action client test **")
     test_action_success(action_client)
 

docs/reference/index.rst

@@ -8,6 +8,6 @@ API Reference
     from roslibpy import *
 
 .. automodule:: roslibpy
-.. automodule:: roslibpy.actionlib
 .. automodule:: roslibpy.tf
+.. automodule:: roslibpy.ros1.actionlib
 .. automodule:: roslibpy.ros2

src/roslibpy/__init__.py

@@ -42,7 +42,7 @@
 =================
 
 ROS 1 vs ROS 2
-------------
+--------------
 
 This library has been tested to work with ROS 1. ROS 2 should work, but it is still
 in the works.
@@ -82,19 +82,21 @@ class and are passed around via :class:`Topics <Topic>` using a **publish/subscr
 .. autoclass:: ServiceResponse
    :members:
 
-Actions
---------
+Actions (ROS 2)
+---------------
 
 An Action client for ROS 2 Actions can be used by managing goal/feedback/result
 messages via :class:`ActionClient <ActionClient>`.
 
 .. autoclass:: ActionClient
    :members:
-.. autoclass:: ActionGoal
+.. autoclass:: Goal
+   :members:
+.. autoclass:: GoalStatus
    :members:
-.. autoclass:: ActionFeedback
+.. autoclass:: Feedback
    :members:
-.. autoclass:: ActionResult
+.. autoclass:: Result
    :members:
 
 Parameter server
@@ -129,13 +131,13 @@ class and are passed around via :class:`Topics <Topic>` using a **publish/subscr
 )
 from .core import (
     ActionClient,
-    ActionFeedback,
-    ActionGoal,
-    ActionGoalStatus,
-    ActionResult,
+    Feedback,
+    Goal,
+    GoalStatus,
     Header,
     Message,
     Param,
+    Result,
     Service,
     ServiceRequest,
     ServiceResponse,
@@ -153,19 +155,19 @@ class and are passed around via :class:`Topics <Topic>` using a **publish/subscr
     "__title__",
     "__url__",
     "__version__",
+    "ActionClient",
+    "Feedback",
+    "Goal",
+    "GoalStatus",
     "Header",
     "Message",
     "Param",
+    "Result",
+    "Ros",
     "Service",
     "ServiceRequest",
     "ServiceResponse",
-    "ActionClient",
-    "ActionGoal",
-    "ActionGoalStatus",
-    "ActionFeedback",
-    "ActionResult",
+    "set_rosapi_timeout",
     "Time",
     "Topic",
-    "set_rosapi_timeout",
-    "Ros",
 ]