apply review comments and add tests

Author: gonzalocasas

src/roslibpy/actionlib.py

@@ -432,24 +432,12 @@ def set_succeeded(self, result):
         LOGGER.info("Action server {} setting current goal to SUCCEEDED".format(self.server_name))
 
         with self._lock:
-
-            self.status_message["status_list"] = [
-                dict(
-                    goal_id=self.current_goal["goal_id"],
-                    status=GoalStatus.SUCCEEDED)
-            ]
+            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": {
-                        "goal_id": self.current_goal["goal_id"],
-                        "status": GoalStatus.SUCCEEDED
-                    },
-                    "result": result
-                }
-            )
+            result_message = Message({"status": status, "result": result})
             self.result_publisher.publish(result_message)
 
             if self.next_goal:
@@ -481,23 +469,12 @@ def set_preempted(self):
         LOGGER.info("Action server {} preempting current goal".format(self.server_name))
 
         with self._lock:
-
-            self.status_message["status_list"] = [
-                dict(
-                    goal_id=self.current_goal["goal_id"],
-                    status=GoalStatus.PREEMPTED)
-            ]
+            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, SimpleActionServer, GoalStatus
+
+
+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()