Merge pull request #102 from MisterOwlPT/fix_action_server

Fix bug with action server (issue #95)

Author: gonzalocasas

.github/workflows/build.yml

@@ -41,8 +41,8 @@ jobs:
           python -m pip install --no-cache-dir -r requirements-dev.txt
       - name: Set up docker containers
         run: |
-          docker build -t gramaziokohler/rosbridge ./docker
-          docker run -d -p 9090:9090 --name rosbridge gramaziokohler/rosbridge /bin/bash -c "roslaunch /integration-tests.launch"
+          docker build -t gramaziokohler/rosbridge:integration_tests ./docker
+          docker run -d -p 9090:9090 --name rosbridge gramaziokohler/rosbridge:integration_tests /bin/bash -c "roslaunch /integration-tests.launch"
           docker ps -a
       - name: Run linter
         run: |

AUTHORS.rst

@@ -8,4 +8,5 @@ Authors
 * Beverly Lytle `@beverlylytle <https://github.com/beverlylytle>`_
 * Alexis Jeandeau `@jeandeaual <https://github.com/jeandeaual>`_
 * Hiroyuki Obinata `@obi-t4 <https://github.com/obi-t4>`_
+* Pedro Pereira `@MisterOwlPT <https://github.com/MisterOwlPT>`_
 * Domenic Rodriguez `@DomenicP <https://github.com/DomenicP>`_

CHANGELOG.rst

@@ -16,6 +16,8 @@ Unreleased
 
 **Fixed**
 
+* Fixed bug with action client/server and now they work as expected.
+
 **Deprecated**
 
 **Removed**

docker/Dockerfile

@@ -1,11 +1,14 @@
-FROM ros:kinetic
+FROM ros:noetic
 LABEL maintainer "Gonzalo Casas <[email protected]>"
 
+SHELL ["/bin/bash","-c"]
+
 # Install rosbridge
 RUN apt-get update && apt-get install -y \
-    ros-kinetic-rosbridge-suite \
-    ros-kinetic-tf2-web-republisher \
-    ros-kinetic-ros-tutorials \
+    ros-noetic-rosbridge-suite \
+    ros-noetic-tf2-web-republisher \
+    ros-noetic-ros-tutorials \
+    ros-noetic-actionlib-tutorials \
     --no-install-recommends \
     # Clear apt-cache to reduce image size
     && rm -rf /var/lib/apt/lists/*
@@ -17,4 +20,4 @@ COPY ./integration-tests.launch /
 EXPOSE 9090
 
 ENTRYPOINT ["/ros_entrypoint.sh"]
-CMD ["/bin/bash"]
+CMD ["bash"]

src/roslibpy/actionlib.py

@@ -326,7 +326,7 @@ def __init__(self, ros, server_name, action_name):
 
         # Intentionally not publishing immediately and instead
         # waiting one interval for the first message
-        self.ros.call_later(self.STATUS_PUBLISH_INTERVAL, self._publish_status)
+        self.ros.call_later(self.STATUS_PUBLISH_INTERVAL, self._periodic_publish_status)
 
     def start(self, action_callback):
         """Start the action server.
@@ -349,18 +349,22 @@ def _internal_preempt_callback():
         self.on("cancel", _internal_preempt_callback)
 
     def _publish_status(self):
+        current_time = time.time()
+        secs = int(math.floor(current_time))
+        nsecs = int(round(1e9 * (current_time - secs)))
+
+        self.status_message["header"]["stamp"]["secs"] = secs
+        self.status_message["header"]["stamp"]["nsecs"] = nsecs
+
+        self.status_publisher.publish(self.status_message)
+
+    def _periodic_publish_status(self):
         # Status publishing is required for clients to know they've connected
         with self._lock:
-            current_time = time.time()
-            secs = int(math.floor(current_time))
-            nsecs = int(round(1e9 * (current_time - secs)))
-
-            self.status_message["header"]["stamp"]["secs"] = secs
-            self.status_message["header"]["stamp"]["nsecs"] = nsecs
-            self.status_publisher.publish(self.status_message)
+            self._publish_status()
 
         # Invoke again in the defined interval
-        self.ros.call_later(self.STATUS_PUBLISH_INTERVAL, self._publish_status)
+        self.ros.call_later(self.STATUS_PUBLISH_INTERVAL, self._periodic_publish_status)
 
     def _on_goal_message(self, message):
         will_cancel = False
@@ -428,14 +432,14 @@ def set_succeeded(self, result):
         LOGGER.info("Action server {} setting current goal to SUCCEEDED".format(self.server_name))
 
         with self._lock:
-            result_message = Message(
-                {"status": {"goal_id": self.current_goal["goal_id"], "status": GoalStatus.SUCCEEDED}, "result": result}
-            )
+            status = dict(goal_id=self.current_goal["goal_id"], status=GoalStatus.SUCCEEDED)
+            self.status_message["status_list"] = [status]
+            self._publish_status()
+            self.status_message["status_list"] = []
 
+            result_message = Message({"status": status, "result": result})
             self.result_publisher.publish(result_message)
 
-            self.status_message["status_list"] = []
-
             if self.next_goal:
                 self.current_goal = self.next_goal
                 self.next_goal = None
@@ -465,11 +469,12 @@ def set_preempted(self):
         LOGGER.info("Action server {} preempting current goal".format(self.server_name))
 
         with self._lock:
+            status = dict(goal_id=self.current_goal["goal_id"], status=GoalStatus.PREEMPTED)
+            self.status_message["status_list"] = [status]
+            self._publish_status()
             self.status_message["status_list"] = []
-            result_message = Message(
-                {"status": {"goal_id": self.current_goal["goal_id"], "status": GoalStatus.PREEMPTED}}
-            )
 
+            result_message = Message({"status": status})
             self.result_publisher.publish(result_message)
 
             if self.next_goal:

tests/test_actionlib.py

@@ -0,0 +1,61 @@
+from __future__ import print_function
+
+import time
+
+from roslibpy import Ros
+from roslibpy.actionlib import ActionClient, Goal, GoalStatus, SimpleActionServer
+
+
+def test_action_success():
+    ros = Ros("127.0.0.1", 9090)
+    ros.run()
+
+    server = SimpleActionServer(ros, "/test_action", "actionlib/TestAction")
+
+    def execute(goal):
+        server.set_succeeded({"result": goal["goal"]})
+
+    server.start(execute)
+
+    client = ActionClient(ros, "/test_action", "actionlib/TestAction")
+    goal = Goal(client, {"goal": 13})
+
+    goal.send()
+    result = goal.wait(10)
+
+    assert result["result"] == 13
+    assert goal.status["status"] == GoalStatus.SUCCEEDED
+
+    client.dispose()
+    time.sleep(2)
+    ros.close()
+
+
+def test_action_preemt():
+    ros = Ros("127.0.0.1", 9090)
+    ros.run()
+
+    server = SimpleActionServer(ros, "/test_action", "actionlib/TestAction")
+
+    def execute(_goal):
+        while not server.is_preempt_requested():
+            time.sleep(0.1)
+        server.set_preempted()
+
+    server.start(execute)
+
+    client = ActionClient(ros, "/test_action", "actionlib/TestAction")
+    goal = Goal(client, {"goal": 13})
+
+    goal.send()
+    time.sleep(0.5)
+    goal.cancel()
+
+    result = goal.wait(10)
+
+    assert result["result"] == 0
+    assert goal.status["status"] == GoalStatus.PREEMPTED
+
+    client.dispose()
+    time.sleep(2)
+    ros.close()