ROS move_base
摘要：
move_base包提供了一個動作的實作（請參見actionlib包），給定一個世界上的目標，該動作將嘗試通過移動基地來實作，move_base節點將全域計劃程式和本地計劃程式鏈接在一起，以完成其全域導航任務，它支持任何全球規劃師堅持在指定的nav_core :: BaseGlobalPlanner介面nav_core包和任何地方規劃師秉承nav_core :: BaseLocalPlanner介面指定nav_core包，move_base節點還維護兩個用于完成導航任務的成本圖，一個用于全域計劃程式，一個用于本地計劃程式（請參閱costmap_2d包），
參考：http://wiki.ros.org/move_base/
Movebase原始碼位于Navigation包中，大家可在Git官方代碼包中進行安裝：
git clone https://github.com/ros-planning/navigation.git
MoveBase::MoveBase(tf2_ros::Buffer& tf):
tf_(tf),
as_(NULL),
planner_costmap_ros_(NULL),controller_costmap_ros_(NULL),
bgp_loader_("nav_core","nav_core::BaseGlobalPlanner"),
blp_loader_("nav_core","nav_core::BaseLocalPlanner"),
recovery_loader_("nav_core","nav_core::RecoveryBehavior"),
planner_plan_(NULL),latest_plan_(NULL),controller_plan_(NULL),
runPlanner_(false),setup_(false),p_freq_change_(false),c_freq_change_(false),
new_global_plan_(false){
/*---------------------------------------------------------------
1.創建一個Action Server::MoveBaseActionServer,系結回呼函式,該服務器的Callback為MoveBase::executeCb
actionlib會啟動一個執行緒，當外部請求到來時，呼叫MoveBase::executeCb回呼函式處理，
---------------------------------------------------------------*/
as_ = new MoveBaseActionServer(ros::NodeHandle(),"move_base",boost::bind(&MoveBase::executeCb,this,_1),false);
//定義一個名為move_base的SimpleActionServer
ros::NodeHandle private_nh("~");
ros::NodeHandle nh;
recovery_trigger_ = PLANNING_R;
/*---------------------------------------------------------------
2.讀取配置:引數從引數服務器獲取一些引數，包括兩個規劃器名稱、代價地圖坐標系、規劃頻率、控制周期等
---------------------------------------------------------------*/
std::string global_planner,local_planner;
private_nh.param("base_global_planner",global_planner,std::string("navfn/NavfnROS"));
private_nh.param("base_local_planner",local_planner,std::string("base_local_planner/TrajectoryPlannerROS"));
private_nh.param("global_costmap/robot_base_frame",robot_base_frame_,std::string("base_link"));
private_nh.param("planner_frequency",planner_frequency_,0.0);
private_nh.param("controller_frequency",planner_frequency_,20.0);
private_nh.param("planner_patience",planner_patience_,5.0);
private_nh.param("controller_patience",controller_patience_,15.0);
private_nh.param("max_planning_retries",max_planning_retries_,-1);
private_nh.param("oscillation_timeout",oscillation_timeout_,0.0);
private_nh.param("oscillation_distance",oscillation_distance_,0.5);
private_nh.param("make_plan_clear_costmap", make_plan_clear_costmap_, true);
private_nh.param("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, 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
recovery_status_pub_= action_nh.advertise<move_base_msgs::RecoveryStatus>("recovery_status", 1);
//提供訊息型別為geometry_msgs::PoseStamped的發送goals的介面，比如cb為MoveBase::goalCB，在rviz中輸入的目標點就是通過這個函式來回應的
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
private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);
private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);
/*---------------------------------------------------------------
6.創建和初始化Global Planner的costmap
---------------------------------------------------------------*/
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause(); //先暫停運行
/*---------costmap的動態庫位于 /opt/ros/kenetic/lib/libcostmap_2d.so
會先啟動一個執行緒處理作業流程：costmap_2d::Costmap2DROS::UdateLoop(double)*/
/*---------------------------------------------------------------
7.創建和初始化Global Planner
---------------------------------------------------------------*/
try {
planner_ = bgp_loader_.createInstance(global_planner); //使用的planner為global_planner/GlobalPlanner
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);//加載和初始化相應的動態庫/opt/ros/kenetic/lib/libglobal_planner.so
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
exit(1);
}
/*---------------------------------------------------------------
8.創建和初始化Local Planner的costmap
---------------------------------------------------------------*/
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause(); //先暫停運行
/*---------------------------------------------------------------
9.創建和初始化Local Planner
---------------------------------------------------------------*/
//create a local planner
try {
tc_ = blp_loader_.createInstance(local_planner); //使用的Planner為teb_local_planner/TebLocalPlannerROS
ROS_INFO("Created local_planner %s", local_planner.c_str());
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_); //加載和初始化相應的動態庫/opt/ros/kenetic/lib/libteb_local_planner.so
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
exit(1);
}
/*---------------------------------------------------------------
10.啟動global和local costmap的處理
---------------------------------------------------------------*/
planner_costmap_ros_->start();
controller_costmap_ros_->start();
/*---------------------------------------------------------------
11.啟動兩個Service：創建地圖,清除地圖服務
---------------------------------------------------------------*/
make_plan_srv_ = private_nh.advertiseService("make_plan",&MoveBase::planService,this);
//定義一個名為clear_costmaps的服務，cb為MoveBase::clearCostmapsService
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps",&MoveBase::clearCostmapsService,this);
//如果不小心關閉了costmap
if(shutdown_costmaps_){
ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
planner_costmap_ros_->stop();
controller_costmap_ros_->stop();
}
/*---------------------------------------------------------------
12.加載Recovery Behavior恢復器
---------------------------------------------------------------*/
if(!loadRecoveryBehaviors(private_nh)){
loadDefaultRecoveryBehaviors();
}
/*---------------------------------------------------------------
13.設定MoveBase的初始狀態為Planning
---------------------------------------------------------------*/
state_ = PLANNING;
/*---------------------------------------------------------------
14.執行Recovery Behavior
---------------------------------------------------------------*/
recovery_index_ = 0;
/*---------------------------------------------------------------
15.啟動執行Action Server：此時，Move Base就可以處理外部導航請求了
---------------------------------------------------------------*/
as_->start();
/*---------------------------------------------------------------
16.啟動動態配置引數功能：Move Base的一些引數支持修改，通過此功能實作
---------------------------------------------------------------*/
dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
//啟動動態引數服務器，回呼函式為reconfigureCB
dynamic_reconfigure::Server<MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB,this,_1,_2);
dsrv_->setCallback(cb);
}
轉載請註明出處，本文鏈接：https://www.uj5u.com/ruanti/281763.html
標籤：其他
上一篇：ARM匯編指令集