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<move_base_msgs::MoveBaseAction_<std::allocator<void>>>::executeLoop()()from /opt/ros/kinetic/lib/libmove_base.so

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

2 读取配置参数

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

planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();

latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();

controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();

4 创建并启动global planner线程

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

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

5 订阅和发布相关的topic

vel_pub_= nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);  // 发布 /cmd_vel

current_goal_pub_= private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );

//发布 /move_base/current_goal

ros::NodeHandle action_nh("move_base");

action_goal_pub_= action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);

//发布 /move_base/goal

ros::NodeHandle simple_nh("move_base_simple");

goal_sub_=simple_nh.subscribe<geometry_msgs::PoseStamped>("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<nav_core::BaseGlobalPlanner> 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<nav_core::BaseLocalPlanner> 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<move_base::MoveBaseConfig>(ros::NodeHandle("~"));

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

dsrv_->setCallback(cb);

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

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