[ros] Implement action cancellation and preemption

Updated accordingly ros/actions.py unit-tests.

Closes #119

src/morse/core/abstractobject.py

@@ -85,7 +85,11 @@ def interrupt(self):
         the task has been interrupted.
 
         It can be overriden in each component to provide a true
-        interrupt, for exemple resseting the speed command to 0.0
+        interrupt, for exemple resseting the speed command to 0.0.
+
+        If you override it, do not forget to call ``self.completed`` with
+        ``status.PREEMPTED``.
+
         """
         import morse.core.status
 

src/morse/core/request_manager.py

@@ -166,8 +166,8 @@ def register_service(self, component_name, callback, service_name = None, async
                     " service '" + service_name + "' for component '" + \
                     component_name + "' successfully registered")
             else:
-                logger.error(str(self) + ": Error while registering a new service: " + \
-                        "could not complete the post-registration step.")
+                logger.info(str(self) + ": Did not register service <%s> " % service_name + \
+                        "(could not complete the post-registration step).")
 
         else:
             logger.error(str(self) + ": Error while registering a new service: " + str(callback) + \

src/morse/middleware/ros_request_manager.py

@@ -124,7 +124,7 @@ def on_goal(self, goal):
 
         is_sync, morse_id = self.manager.on_incoming_request(self.component, self._action, [goal.goal])
 
-        # is_sync should be always True for ROS services!
+        # is_sync should be only True for ROS services!
         if is_sync:
             # TODO: clean terminated goals 'after a few seconds' (as
             # said by actionlib doc)
@@ -152,6 +152,7 @@ def on_cancel(self, goal_id):
             self.setstatus(goal_id.id, actionlib_msgs.msg.GoalStatus.RECALLING)
         else: #current status = ACTIVE (or smth else...)
             self.setstatus(goal_id.id, actionlib_msgs.msg.GoalStatus.PREEMPTING)
+            self.manager.abort_request(self._pending_goals[goal_id.id]['morse_id'])
 
     def on_result(self, morse_id, state, result):
 
@@ -170,7 +171,8 @@ def on_result(self, morse_id, state, result):
             logger.debug("The action " + self.name + " has succeeded. Reporting"
                          " it to ROS system and publishing results.")
             self.setstatus(id, actionlib_msgs.msg.GoalStatus.SUCCEEDED)
-            self.publish_result(id, result)
+
+        self.publish_result(id, result)
 
     def publish_result(self, goal_id, result):
 

testing/middlewares/ros/actions.py

@@ -14,11 +14,16 @@
 
 import os
 import sys
+import time
 
-import roslib; roslib.load_manifest("morsetesting")
 import rospy
 import actionlib
-from morsetesting.msg import *
+import nav_msgs.msg # do not conflict with morse builder
+
+from morse.middleware.ros.helpers import ros_add_to_syspath
+ros_add_to_syspath("move_base_msgs")
+from move_base_msgs.msg import *
+
 from geometry_msgs.msg import *
 
 class RosActionsTest(RosTestCase):
@@ -28,7 +33,11 @@ def setUpEnv(self):
 
         print("Adding a robot...")
         robot = ATRV()
-
+
+        odometry = Odometry()
+        robot.append(odometry)
+        odometry.add_stream('ros')
+
         waypoint = Waypoint()
         robot.append(waypoint)
 
@@ -45,20 +54,84 @@ def test_no_action(self):
             self.assertFalse(client.wait_for_server(rospy.Duration(5)))
 
     def test_move_base(self):
-            
+
             rospy.loginfo("Starting ROS test case for actions.")
             rospy.init_node('move_base_client')
             client = actionlib.SimpleActionClient('robot/waypoint/move_base', MoveBaseAction)
             self.assertTrue(client.wait_for_server(rospy.Duration(5)))
 
-            goal = MoveBaseGoal(Pose(Point(0.1,3.0,0.0), Quaternion(0.0,0.0,0.0,1.0)))
-            
+            goal = MoveBaseGoal(PoseStamped(pose = Pose(Point(0.1,3.0,0.0), Quaternion(0.0,0.0,0.0,1.0))))
+
             print("Sending a first goal to the robot...(timeout=5sec)")
             status = client.send_goal_and_wait(goal, rospy.Duration(5))
 
             print("Got this status: " + str(status))
             self.assertEqual(status, actionlib.GoalStatus.SUCCEEDED)
 
+    def get_pose(self):
+        msg = rospy.client.wait_for_message('/robot/odometry', nav_msgs.msg.Odometry, timeout = 10)
+        return (msg.pose.pose.position.x, msg.pose.pose.position.y)
+
+    def check_not_moving(self):
+        pos1 = self.get_pose()
+        time.sleep(0.5)
+        pos2 = self.get_pose()
+        self.assertAlmostEqual(pos1[0], pos2[0], delta=0.05)
+        self.assertAlmostEqual(pos1[1], pos2[1], delta=0.05)
+
+    def cb_preempted(self, status, res):
+        self.cb_fired = True
+        self.assertEqual(status, actionlib.GoalStatus.PREEMPTED)
+
+    def cb_succeeded(self, status, res):
+        self.cb_fired = True
+        self.assertEqual(status, actionlib.GoalStatus.SUCCEEDED)
+
+
+    def test_move_advanced(self):
+
+        rospy.loginfo("Starting ROS test case for actions (advanced behaviour).")
+        rospy.init_node('move_base_client')
+        client = actionlib.SimpleActionClient('robot/waypoint/move_base', MoveBaseAction)
+        client2 = actionlib.SimpleActionClient('robot/waypoint/move_base', MoveBaseAction)
+        self.assertTrue(client.wait_for_server(rospy.Duration(5)))
+        self.assertTrue(client2.wait_for_server(rospy.Duration(5)))
+
+        self.assertIsNotNone(self.get_pose())
+
+        goal1 = MoveBaseGoal(PoseStamped(pose = Pose(Point(0.1,10.0,0.0), Quaternion(0.0,0.0,0.0,1.0))))
+        goal2 = MoveBaseGoal(PoseStamped(pose = Pose(Point(0.1,2.0,0.0), Quaternion(0.0,0.0,0.0,1.0))))
+
+
+        # send goal with not enough time to complete -> should get a preemption and the robot actually stopping
+        status = client.send_goal_and_wait(goal1, rospy.Duration(1))
+        self.assertEqual(status, actionlib.GoalStatus.PREEMPTED)
+        self.check_not_moving()
+        ######
+
+        # sending again the goal, and ask for cancellation
+        self.cb_fired = False
+        client.send_goal(goal1, done_cb = self.cb_preempted)
+        time.sleep(1)
+        client.cancel_goal()
+        self.check_not_moving()
+        self.assertTrue(self.cb_fired)
+        ######
+
+        # sending again the goal, this time, interrupted by another one
+        self.cb_fired = False
+        client.send_goal(goal1, done_cb = self.cb_preempted)
+        time.sleep(1)
+        client2.send_goal(goal2, done_cb = self.cb_succeeded)
+        time.sleep(0.5)
+        self.assertTrue(self.cb_fired)
+        self.cb_fired = False
+        time.sleep(5)
+        self.assertTrue(self.cb_fired)
+        ######
+
+
+
 
 ########################## Run these tests ##########################
 if __name__ == "__main__":