Merge pull request #253 from floweisshardt/feature/emulator_move_base_melodic

melodic/noetic: add optional move_base interface to base emulator

Author: floweisshardt

cob_hardware_emulation/package.xml

@@ -15,7 +15,9 @@
   <depend>rosgraph_msgs</depend>
 
   <exec_depend>actionlib</exec_depend>
+  <exec_depend>actionlib_msgs</exec_depend>
   <exec_depend>control_msgs</exec_depend>
+  <exec_depend>move_base_msgs</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>

cob_hardware_emulation/scripts/emulation_base.py

@@ -3,11 +3,14 @@
 import copy
 import math
 
+import actionlib
 import rospy
 import tf
 import tf2_ros
-from geometry_msgs.msg import Twist, Transform, TransformStamped
+from actionlib_msgs.msg import GoalStatus
+from geometry_msgs.msg import Twist, Transform, TransformStamped, PoseStamped
 from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Quaternion
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseResult
 from nav_msgs.msg import Odometry
 from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
 
@@ -22,11 +25,14 @@ def __init__(self):
         # - publishers:
         #   - /base/odometry_controller/odometry [nav_msgs/Odometry]
         #   - tf (odom_combined --> base_footprint, map --> odom_combined)
+        # - actions:
+        #   - move_base [move_base_msgs/MoveBaseAction] (optional)
 
 
         # TODO
         # - add service reset odometry (/base/odometry_controller/reset_odometry [std_srvs::Trigger])
         # - speed factor
+        # - handle cancel on move_base action
 
         rospy.Subscriber("/base/twist_controller/command", Twist, self.twist_callback, queue_size=1)
         rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.initalpose_callback, queue_size=1)
@@ -68,6 +74,19 @@ def __init__(self):
 
         rospy.loginfo("Emulation for base running")
 
+        # Optional move_base action
+        self.move_base_mode = rospy.get_param("~move_base_mode", None)
+        if self.move_base_mode == None:
+            rospy.loginfo("Emulation running without move_base")
+        elif self.move_base_mode == "beam" or self.move_base_mode == "linear_nav":
+            self.move_base_action_name = "/move_base"
+            rospy.Subscriber(self.move_base_action_name + "_simple/goal", PoseStamped, self.move_base_simple_callback, queue_size=1)
+            self.as_move_base = actionlib.SimpleActionServer(self.move_base_action_name, MoveBaseAction, execute_cb=self.move_base_cb, auto_start = False)
+            self.as_move_base.start()
+            rospy.loginfo("Emulation running for action %s of type MoveBaseAction with mode '%s'"%(self.move_base_action_name, self.move_base_mode))
+        else:
+            rospy.logwarn("Emulation running without move_base due to invalid value for parameter move_base_mode: '%s'", self.move_base_mode)
+
     def initalpose_callback(self, msg):
         self.initial_pose.translation.x = msg.pose.pose.position.x
         self.initial_pose.translation.y = msg.pose.pose.position.y
@@ -158,6 +177,49 @@ def timer_cb(self, event):
 
         self.br.sendTransform(transforms)
 
+    def move_base_cb(self, goal):
+        pwcs = PoseWithCovarianceStamped()
+        pwcs.header = goal.target_pose.header
+        pwcs.pose.pose = goal.target_pose.pose
+        if self.move_base_mode == "beam":
+            rospy.loginfo("move_base: beaming robot to new goal")
+            self.initalpose_callback(pwcs)
+        elif self.move_base_mode == "linear_nav":
+            move_base_linear_action_name = "/move_base_linear"
+            ac_move_base_linear = actionlib.SimpleActionClient(move_base_linear_action_name, MoveBaseAction)
+            rospy.loginfo("Waiting for ActionServer: %s", move_base_linear_action_name)
+            if not ac_move_base_linear.wait_for_server(rospy.Duration(1)):
+                rospy.logerr("Emulator move_base failed because move_base_linear action server is not available")
+                self.as_move_base.set_aborted()
+                return
+            rospy.loginfo("send goal to %s", move_base_linear_action_name)
+            ac_move_base_linear.send_goal(goal)
+            ac_move_base_linear.wait_for_result()
+            ac_move_base_linear_status = ac_move_base_linear.get_state()
+            ac_move_base_linear_result = ac_move_base_linear.get_result()
+            if ac_move_base_linear_status != GoalStatus.SUCCEEDED:
+                rospy.logerr("Emulator move_base failed because move_base_linear failed")
+                self.as_move_base.set_aborted()
+                return
+        else:
+            rospy.logerr("Invalid move_base_action_mode")
+            self.as_move_base.set_aborted()
+            return
+        rospy.loginfo("Emulator move_base succeeded")
+        self.as_move_base.set_succeeded(MoveBaseResult())
+
+    def move_base_simple_callback(self, msg):
+        goal = MoveBaseGoal()
+        goal.target_pose = msg
+        ac_move_base = actionlib.SimpleActionClient(self.move_base_action_name, MoveBaseAction)
+        rospy.loginfo("Waiting for ActionServer: %s", self.move_base_action_name)
+        if not ac_move_base.wait_for_server(rospy.Duration(1)):
+            rospy.logerr("Emulator move_base simple failed because move_base action server is not available")
+            return
+        rospy.loginfo("send goal to %s", self.move_base_action_name)
+        ac_move_base.send_goal(goal)
+        # ac_move_base.wait_for_result() # no need to wait for the result as this is the topic interface to move_base without feedback
+
 if __name__ == '__main__':
     rospy.init_node('emulation_base')
     EmulationBase()