Go to the documentation of this file.00001
00002
00003
00004
00005
00006 import actionlib
00007 import copy
00008
00009 import rospy
00010 import nav_msgs.srv as nav_srvs
00011 import mbf_msgs.msg as mbf_msgs
00012 import move_base_msgs.msg as mb_msgs
00013 from dynamic_reconfigure.client import Client
00014 from dynamic_reconfigure.server import Server
00015 from geometry_msgs.msg import PoseStamped
00016 from move_base.cfg import MoveBaseConfig
00017
00018
00019 """
00020 move_base legacy relay node:
00021 Relays old move_base actions to the new mbf move_base action, similar but with richer result and feedback.
00022 We also relay the simple goal topic published by RViz, the make_plan service and dynamic reconfiguration
00023 calls (note that some parameters have changed names; see http://wiki.ros.org/move_base_flex for details)
00024 """
00025
00026
00027 def simple_goal_cb(msg):
00028 mbf_mb_ac.send_goal(mbf_msgs.MoveBaseGoal(target_pose=msg))
00029 rospy.logdebug("Relaying move_base_simple/goal pose to mbf")
00030
00031
00032 def mb_execute_cb(msg):
00033 mbf_mb_ac.send_goal(mbf_msgs.MoveBaseGoal(target_pose=msg.target_pose), feedback_cb=mbf_feedback_cb)
00034 rospy.logdebug("Relaying legacy move_base goal to mbf")
00035 mbf_mb_ac.wait_for_result()
00036
00037 status = mbf_mb_ac.get_state()
00038 result = mbf_mb_ac.get_result()
00039
00040 rospy.logdebug("MBF execution completed with result [%d]: %s", result.outcome, result.message)
00041 if result.outcome == mbf_msgs.MoveBaseResult.SUCCESS:
00042 mb_as.set_succeeded(mb_msgs.MoveBaseResult(), "Goal reached.")
00043 else:
00044 mb_as.set_aborted(mb_msgs.MoveBaseResult(), result.message)
00045
00046
00047 def make_plan_cb(request):
00048 mbf_gp_ac.send_goal(mbf_msgs.GetPathGoal(start_pose=request.start, target_pose=request.goal,
00049 use_start_pose = bool(request.start.header.frame_id),
00050 tolerance=request.tolerance))
00051 rospy.logdebug("Relaying legacy make_plan service to mbf get_path action server")
00052 mbf_gp_ac.wait_for_result()
00053
00054 status = mbf_gp_ac.get_state()
00055 result = mbf_gp_ac.get_result()
00056
00057 rospy.logdebug("MBF get_path execution completed with result [%d]: %s", result.outcome, result.message)
00058 if result.outcome == mbf_msgs.GetPathResult.SUCCESS:
00059 return nav_srvs.GetPlanResponse(plan=result.path)
00060
00061
00062 def mbf_feedback_cb(feedback):
00063 mb_as.publish_feedback(mb_msgs.MoveBaseFeedback(base_position=feedback.current_pose))
00064
00065
00066 def mb_reconf_cb(config, level):
00067 rospy.logdebug("Relaying legacy move_base reconfigure request to mbf")
00068
00069 if not hasattr(mb_reconf_cb, "default_config"):
00070 mb_reconf_cb.default_config = copy.deepcopy(config)
00071
00072 if config.get('restore_defaults'):
00073 config = mb_reconf_cb.default_config
00074
00075 mbf_config = copy.deepcopy(config)
00076
00077
00078 if 'base_local_planner' in mbf_config:
00079 mbf_config.pop('base_local_planner')
00080 if 'controller_frequency' in mbf_config:
00081 mbf_config['controller_frequency'] = mbf_config.pop('controller_frequency')
00082 if 'controller_patience' in mbf_config:
00083 mbf_config['controller_patience'] = mbf_config.pop('controller_patience')
00084 if 'max_controller_retries' in mbf_config:
00085 mbf_config['controller_max_retries'] = mbf_config.pop('max_controller_retries')
00086 if 'base_global_planner' in mbf_config:
00087 mbf_config.pop('base_global_planner')
00088 if 'planner_frequency' in mbf_config:
00089 mbf_config['planner_frequency'] = mbf_config.pop('planner_frequency')
00090 if 'planner_patience' in mbf_config:
00091 mbf_config['planner_patience'] = mbf_config.pop('planner_patience')
00092 if 'max_planning_retries' in mbf_config:
00093 mbf_config['planner_max_retries'] = mbf_config.pop('max_planning_retries')
00094 if 'recovery_behavior_enabled' in mbf_config:
00095 mbf_config['recovery_enabled'] = mbf_config.pop('recovery_behavior_enabled')
00096 if 'conservative_reset_dist' in mbf_config:
00097 mbf_config.pop('conservative_reset_dist')
00098 if 'clearing_rotation_allowed' in mbf_config:
00099 mbf_config.pop('clearing_rotation_allowed')
00100
00101 mbf_drc.update_configuration(mbf_config)
00102 return config
00103
00104
00105 if __name__ == '__main__':
00106 rospy.init_node("move_base")
00107
00108
00109
00110
00111
00112 mbf_mb_ac = actionlib.SimpleActionClient("move_base_flex/move_base", mbf_msgs.MoveBaseAction)
00113 mbf_gp_ac = actionlib.SimpleActionClient("move_base_flex/get_path", mbf_msgs.GetPathAction)
00114 mbf_mb_ac.wait_for_server(rospy.Duration(20))
00115 mbf_gp_ac.wait_for_server(rospy.Duration(10))
00116
00117
00118 mbf_drc = Client("move_base_flex", timeout=10)
00119
00120
00121 mb_sg = rospy.Subscriber('move_base_simple/goal', PoseStamped, simple_goal_cb)
00122 mb_as = actionlib.SimpleActionServer('move_base', mb_msgs.MoveBaseAction, mb_execute_cb, auto_start=False)
00123 mb_as.start()
00124
00125
00126 mb_mps = rospy.Service('~make_plan', nav_srvs.GetPlan, make_plan_cb)
00127
00128
00129 mb_drs = Server(MoveBaseConfig, mb_reconf_cb)
00130
00131 rospy.spin()