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导航启动完成。
后面会对每一步再做详细剖析。
评论(0)
您还未登录,请登录后发表或查看评论