Fix: Converts move_base_simple/goal into a move_base action.

At least MIR software version 2.8 did not react properly to goal
messages. This implements a workaround.
Closes DFKI-NI#60.

mir_driver/nodes/mir_bridge.py

@@ -8,13 +8,14 @@
 from mir_driver import rosbridge
 from rospy_message_converter import message_converter
 
+from actionlib import SimpleActionClient
 from actionlib_msgs.msg import GoalID, GoalStatusArray
 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
 from dynamic_reconfigure.msg import Config, ConfigDescription
 from geometry_msgs.msg import PolygonStamped, Pose, PoseArray, PoseStamped, PoseWithCovarianceStamped, Twist
 from mir_actions.msg import *
 from mir_msgs.msg import *
-from move_base_msgs.msg import MoveBaseActionFeedback, MoveBaseActionGoal, MoveBaseActionResult, MoveBaseFeedback, MoveBaseResult
+from move_base_msgs.msg import MoveBaseAction, MoveBaseActionFeedback, MoveBaseActionGoal, MoveBaseActionResult, MoveBaseFeedback, MoveBaseGoal, MoveBaseResult
 from nav_msgs.msg import GridCells, MapMetaData, OccupancyGrid, Odometry, Path
 from rosgraph_msgs.msg import Log
 from sdc21x0.msg import MotorCurrents
@@ -228,8 +229,8 @@ def _remove_tf_prefix_dict_filter(msg_dict):
               TopicConfig('mir_cmd', String),
               TopicConfig('move_base/cancel', GoalID),
 ##              TopicConfig('move_base/goal', MirMoveBaseActionGoal),
-              TopicConfig('move_base/goal', MoveBaseActionGoal, dict_filter=_move_base_goal_dict_filter),  # really mir_actions/MirMoveBaseActionGoal
-              TopicConfig('move_base_simple/goal', PoseStamped)]
+              TopicConfig('move_base/goal', MoveBaseActionGoal, dict_filter=_move_base_goal_dict_filter)]  # really mir_actions/MirMoveBaseActionGoal
+#              TopicConfig('move_base_simple/goal', PoseStamped)]
 #              TopicConfig('relative_move_action/cancel', GoalID),
 #              TopicConfig('relative_move_action/goal', RelativeMoveActionGoal)]
 
@@ -331,6 +332,11 @@ def __init__(self):
             if ('/' + sub_topic.topic) not in subscribed_topics:
                 rospy.logwarn("[%s] topic '%s' is not yet subscribed to by the MiR!", rospy.get_name(), sub_topic.topic)
 
+        # At least with software version 2.8 there were issues when forwarding a simple goal to the robot
+        # This workaround converts it into an action. Check https://github.com/dfki-ric/mir_robot/issues/60 for details.
+        self._move_base_client = SimpleActionClient('move_base', MoveBaseAction)
+        rospy.Subscriber("/move_base_simple/goal",  PoseStamped, self._move_base_simple_goal_callback)
+
     def get_topics(self):
         srv_response = self.robot.callService('/rosapi/topics', msg={})
         topic_names = sorted(srv_response['topics'])
@@ -360,6 +366,16 @@ def get_topics(self):
 
         return topics
 
+    def _move_base_simple_goal_callback(self, msg):
+        if not self._move_base_client.wait_for_server(rospy.Duration(2)):
+            rospy.logwarn("Could not connect to 'move_base' server after two seconds. Dropping goal.")
+            rospy.logwarn("Did you activate 'planner' in the MIR web interface?")
+            return
+        goal = MoveBaseGoal()
+        goal.target_pose.header = copy.deepcopy(msg.header)
+        goal.target_pose.pose = copy.deepcopy(msg.pose)
+        self._move_base_client.send_goal(goal)
+
 
 def main():
     rospy.init_node('mir_bridge')