ROS Navigation源代码剖析(2)-move_base启动过程

4 Source code

4.1 Main() function

navigation-kinetic\move_base\src\move_base_node.cpp

 

启动命令为: /opt/ros/kinetic/lib/move_base/move_base __name:=move_base __log=/tmp/ddd/move_bae-19.log

 

int main(int argc, char** argv){

  ros::init(argc, argv, "move_base_node");

  tf::TransformListener tf(ros::Duration(10));

  move_base::MoveBase move_base( tf );

  ros::spin();

  return(0);

}

 

所以，真正的启动过程是在MoveBase class的构造函数中。

 

4.2 MoveBase 启动过程

navigation-kinetic\move_base\src\move_base.cpp

启动过程在MoveBase的构造函数中MoveBase::MoveBase(tf::TransformListener& tf)

 

1 首先创建一个action server：MoveBaseActionServer。

   as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

  Action servrer接收外部的目标请求，驱动整个的路径规划和移动过程。

actionlib会启动一个线程，当外部请求到来时，调用MoveBase::executeCb回调函数处理

actionlib::SimpleActionServer>>::executeLoop()()from /opt/ros/kinetic/lib/libmove_base.so

 

此时action server还没有工作，等所有的初始化工作完成后，直到第15步才开始工作。

 

2 读取配置参数

变量名字	类型	参数名字	值
global_planner	std::string	base_global_planner	global_planner/GlobalPlanner
local_planner	std::string	base_local_planner	teb_local_planner/TebLocalPlannerROS
robot_base_frame_	std::string	global_costmap/robot_base_fram	/base_footprint
global_frame_	std::string	global_costmap/global_frame	/map
planner_frequency_	double	planner_frequency	1.0
controller_frequency_	double	controller_frequency	4.0
planner_patience_	double	planner_patience	3.0
controller_patience_	double	controller_patience	3.0
max_planning_retries_	int32_t	max_planning_retries	1
oscillation_timeout_	double	oscillation_timeout	5.0
oscillation_distance_	double	oscillation_distance	0.2
inscribed_radius_	double	local_costmap/inscribed_radius	0.325
circumscribed_radius_	double	local_costmap/circumscribed_radius	0.46
clearing_radius_	double	clearing_radius	0.46
conservative_reset_dist_	double	conservative_reset_dist	3.0
shutdown_costmaps_	bool	shutdown_costmaps	false
clearing_rotation_allowed_	bool	clearing_rotation_allowed	true
recovery_behavior_enabled_	bool	recovery_behavior_enabled	true

3 为global planner分配存放路径的内存，使用vector存储

planner_plan_ = new std::vector();

latest_plan_ = new std::vector();

controller_plan_ = new std::vector();

 

4 创建并启动global planner线程

planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

    此线程负责全局规划器的路径选择过程

5 订阅和发布相关的topic

vel_pub_= nh.advertise("cmd_vel", 1);  // 发布 /cmd_vel

current_goal_pub_= private_nh.advertise("current_goal", 0 );

//发布 /move_base/current_goal

ros::NodeHandle action_nh("move_base");

action_goal_pub_= action_nh.advertise("goal", 1);

//发布 /move_base/goal

ros::NodeHandle simple_nh("move_base_simple");

goal_sub_=simple_nh.subscribe("goal",1, boost::bind(&MoveBase::goalCB, this, _1));                                                   // 订阅 /move_base_simple/goal

 

6 创建和初始化global planner的costmap

planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);

planner_costmap_ros_->pause();  //先暂定运行

costmap的动态库位于 /opt/ros/kinetic/lib/libcostmap_2d.so

   

    会启动一个线程处理工作流程：

       costmap_2d::Costmap2DROS::mapUpdateLoop(double)

7 创建和初始化global planner

      boost::shared_ptr planner_;

planner_ = bgp_loader_.createInstance(global_planner);

//使用的planner为 global_planner/GlobalPlanner

      planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);

                   //加载和初始化相应的动态库/opt/ros/kinetic/lib/libglobal_planner.so

 

动态库的加载使用ros 的pluginlib规则

/opt/ros/kinetic/lib/libclass_loader.so

8 创建和初始化local planner的costmap

controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);

controller_costmap_ros_->pause(); //先暂定运行

   

与global costmap一样，也会启动一个线程处理工作流程：

       costmap_2d::Costmap2DROS::mapUpdateLoop(double)

 

9  创建和初始化local planner

      boost::shared_ptr tc_;

tc_ = blp_loader_.createInstance(local_planner);

      //使用的planner为teb_local_planner/TebLocalPlannerROS

      tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);

                  //加载和初始化相应的动态库/opt/ros/kinetic/lib/libteb_local_planner.so

 

还有一个库/opt/ros/kinetic/lib/libbase_local_planner.so

 

10 启动global和local costmap的处理

   planner_costmap_ros_->start();

   controller_costmap_ros_->start();

 

11 启动两个service

  //advertise a service for getting a plan   /move_base/make_plan

    make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);

    //advertise a service for clearing the costmaps  /move_base/clear_costmaps

clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

 

12 加载recovery behavior

//load any user specified recovery behaviors, and if that fails load the defaults

    if(!loadRecoveryBehaviors(private_nh)){

      loadDefaultRecoveryBehaviors();

    }

13 设置move base的初始状态为PLANNING

    //initially, we'll need to make a plan

    state_ = PLANNING;

14 执行recovery behavior

    //we'll start executing recovery behaviors at the beginning of our list

    recovery_index_ = 0;

15 启动执行action server

    //we're all set up now so we can start the action server

    as_->start();

    此时，movebase 可以处理外部的导航请求了。

16启动动态配置参数功能

Move base的一些参数支持动态修改，通过此功能实现。

dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~"));

dynamic_reconfigure::Server::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);

dsrv_->setCallback(cb);

 

至此，整个move base导航启动完成。

后面会对每一步再做详细剖析。