本文共 5436 字,大约阅读时间需要 18 分钟。
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 读取配置参数
变量名字 | 类型 | 参数名字 | 值 |
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 |
| |||
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<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导航启动完成。
后面会对每一步再做详细剖析。
转载地址:https://blog.csdn.net/lclfans1983/article/details/106692805 如侵犯您的版权,请留言回复原文章的地址,我们会给您删除此文章,给您带来不便请您谅解!