ros::NodeHandle  nh;
nh.getParam("robot_pose", robot_pose_ );

// 在 move_base 节点内
ros::NodeHandle n("~/");
double control_frequency;
n.param("controller_frequency", control_frequency, 15.0);

// 在 move_base 节点内
ros::NodeHandle nh("~/" + string("Planner") );
nh.param("linear_vel", linear_vel_, 0.3);

ros::init(argc, argv, "my_node_name");
ros::NodeHandle nh1("~");
ros::NodeHandle nh2("~foo");
ros::Subscriber sub1 = nh1.subscribe("topic1", ...);
ros::Subscriber sub2 = nh2.subscribe("topic2", ...);

ros::NodeHandle n;
ros::Timer timer;
bool wait_for_wake = false;   // 先不唤醒线程
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
while(n.ok() )
{
    // check if we should run the planner (the mutex is locked)
    while(wait_for_wake || !runPlanner_){
    //if we should not be running the planner then suspend this thread
        log.debug("[move_base_plan_thread] %s","Planner thread is suspending");
        planner_cond_.wait(lock);
        wait_for_wake = false;
    }
    // 开启线程
    ros::Time start_time = ros::Time::now();
    //time to plan! get a copy of the goal and unlock the mutex
    geometry_msgs::PoseStamped temp_goal = planner_goal_;
    lock.unlock();
    ROS_DEBUG_NAMED("move_base_plan_thread",  "Planning...");
    // run planner
    planner_plan_->clear();
    bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
    ......
}

while(wait_for_wake || !runPlanner_ )
{
}

if(gotPlan)
{
    ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
    //pointer swap the plans under mutex (the controller will pull from latest_plan_)
    std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;
    
    lock.lock();
    // 交换变量，赋值给 latest_plan_
    planner_plan_ = latest_plan_;
    latest_plan_ = temp_plan;
    last_valid_plan_ = ros::Time::now();
    planning_retries_ = 0;
    new_global_plan_ = true;
    
    //make sure we only start the controller if we still haven't reached the goal
    if(runPlanner_)
        state_ = CONTROLLING;
    if(planner_frequency_ <= 0)
        runPlanner_ = false;
    lock.unlock();
}
// 如果没找到路径，而且还在 PLANNING 状态(机器人不动)
else if(state_==PLANNING)
{
    ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
    //check if make a plan for over time limit or maximum number of retries
    lock.lock();
    // lock是用于这个非原子操作
    planning_retries_++;
    // 如果参数 max_planning_retries还是默认值 -1，那转为uint32_t就是一个很大的数，就是不考虑重试次数
    if(runPlanner_ &&
            (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_)) )
    {
        // 进入 CLEARING 状态
        state_ = CLEARING;
        runPlanner_ = false;  // proper solution for issue #523
        publishZeroVelocity();      // 车不准动
        recovery_trigger_ = PLANNING_R;
    }
    lock.unlock();
}
//if can't get new plan, robot will move with last global plan
// 其实跟上面的情况一样
else if(state_ == CONTROLLING)
{
    movebase_log.warn("Robot in controlling, but can't get new global plan ");
    publishZeroVelocity();
    state_ = CLEARING;
    recovery_trigger_ = PLANNING_R;
}
//take the mutex for the next iteration
lock.lock();
if(planner_frequency_ > 0)
{
    ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now();
    if (sleep_time > ros::Duration(0.0)  )
    {
        wait_for_wake = true;
        // wakePlanner的函数只有一句: planner_cond_.notify_one();
        timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
    }
}

ros::init(argc, argv, "move_base_node");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,ros::console::levels::Debug);

tf::TransformListener tf(ros::Duration(10));
move_base::MoveBase move_base( tf );
ros::spin();

return(0);

//  枚举变量RecoveryTrigger，可取值有PLANNING_R, CONTROLLING_R, OSCILLATION_R
recovery_trigger_ = PLANNING_R;	

// as_维护movebase的actionServer状态机，并且新建了一个executeCb线程
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

ros::NodeHandle private_nh("~");
ros::NodeHandle nh;
// 给一系列参数设置默认值
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("global_costmap/global_frame", global_frame_, std::string("/map"));
private_nh.param("planner_frequency", planner_frequency_, 0.0);
private_nh.param("controller_frequency", controller_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);

// set up plan triple buffer, 声明3个vector指针
// 三个plan都是global_plan，最终由controller_plan_ 传给local planner: if(!tc_->setPlan(*controller_plan_))
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();

//建立planner线程
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

//用于控制机器人底座
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );

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

// we'll provide a mechanism for some people to send goals as //PoseStamped messages over a topic, they won't get any useful //information back about its status, but this is useful for tools like rviz
ros::NodeHandle simple_nh("move_base_simple");
goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

// /机器人几何尺寸有关参数  we'll assume the radius of the robot to be consistent with what's specified for the costmaps
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);

//创建planner's costmap的ROS封装类， 初始化一个指针，用于underlying map
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();

//初始化global planner
try {
	// boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
	// 模板类ClassLoader，实例化 BaseGlobalPlanner
    planner_ = bgp_loader_.createInstance(global_planner);
    planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
    log.info("Created global_planner %s", global_planner.c_str());
} catch (const pluginlib::PluginlibException& ex) {
    log.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);
}

//创建controller's costmap的ROS封装类， 初始化一个指针，用于underlying map
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();

//初始化local planner
try {
    tc_ = blp_loader_.createInstance(local_planner);
    log.info("Created local_planner %s", local_planner.c_str());
    tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
    log.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);
}
// 开启costmap基于传感器数据的更新
planner_costmap_ros_->start();
controller_costmap_ros_->start();

//advertise a service for getting a plan
make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);

//advertise a service for clearing the costmaps
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
// if we shutdown our costmaps when we're deactivated... we'll do that now
if(shutdown_costmaps_){
    log.debug("[move_base] %s","Stopping costmaps initially");
    planner_costmap_ros_->stop();
    controller_costmap_ros_->stop();
}

// 载入recovery behavior, 这部分是状态机的设计问题，优先加载用户的设置,如果没有则加载默认设置
if(!loadRecoveryBehaviors(private_nh)){
    loadDefaultRecoveryBehaviors();
}
//initially, we'll need to make a plan
state_ = PLANNING;

//we'll start executing recovery behaviors at the beginning of our list
recovery_index_ = 0;

// start the action server
as_->start();

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);

private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));

void resize(size_type _Newsize)
void resize(size_type _Newsize, _Ty _Val)

vector<int> vec;
vec.push_back(1);
vec.push_back(2);
vec.push_back(3);
vec.push_back(4);
vec.push_back(5);

cout<< vec.size() <<"  "<<vec.capacity()<<"  "<<endl;   // 5   8
vec.resize(10);     // 改变大小
cout<< vec.size() <<"  "<<vec.capacity()<<"  "<<endl;   // 10   10

vector<int> v{1,2,3,4,5};
qDebug()<< *(v.data() ) ;  // 1

// 先清除vector之前的内容,再将区间[first, last)的元素赋到当前vector
void assign(const_iterator first, const_iterator last);
// 先清除vector之前的内容,再将Count个Val插入到vector
void assign( size_type _Count, const Type& _Val );

vector<int> vec;
vec.assign(5,9);

cout<< vec.size() <<"  "<<vec.capacity()<<"  "<<endl;

vector<int> vec;
vec.push_back(1);
vec.push_back(2);
vec.push_back(3);
vec.push_back(4);
vec.push_back(5);
cout<< vec.size() <<"  "<<vec.capacity()<<"  "<<endl;

iterator erase(iterator it); // 删除指定元素，并返回删除元素后一个元素的位置（如果无元素，返回end()）
iterator erase(iterator first, iterator last); // 注意：删除元素后，删除点之后的元素对应的迭代器不再有效。

void clear() const; // 函数删除当前vector中的所有元素，相当于调用erase( begin(), end())

int v[6] = {1,2,3,4,5,6};
std::vector<int*> ps;
int* p1 = v;
int* p2 = v+1;
int* p3 = v+2;
int* p4 = v+3;

ps.push_back( p1 );
ps.push_back( p2 );
ps.push_back( p3 );
ps.push_back( p4 );

// delete ps.back(), ps.pop_back();

for (auto it = ps.begin(); it != ps.end(); it++)
{
  if (*it != NULL)
  {
    delete *it;
    *it = NULL;
  }
}

ps.clear();
ps.shrink_to_fit();

using Type = std::vector<int>;
int f = 12;
Type vec={f};

struct timeval tv;
gettimeofday(&tv,nullptr);
time_t curTime = tv.tv_sec;   // 获取当前UTC时间戳，单位为秒

#include <chrono>
using namespace chrono;

auto start = system_clock::now();
/*  do something  */
auto end   = system_clock::now();
auto duration = duration_cast<microseconds>(end - start);
cout <<  "花费了"
     << double(duration.count()) * microseconds::period::num / microseconds::period::den
     << "秒" << endl;

#include <chrono>

using std::chrono::steady_clock;
// 这个类型是 steady_clock::time_point
auto t1 = std::chrono::steady_clock::now();

// 	your code

auto t2 = std::chrono::steady_clock::now();

//秒
double dr_s=std::chrono::duration<double>(t2-t1).count();
//毫秒
double dr_ms=std::chrono::duration<double,std::milli>(t2-t1).count();
//微秒
double dr_us=std::chrono::duration<double,std::micro>(t2-t1).count();
//纳秒
double dr_us=std::chrono::duration<double,std::nano>(t2-t1).count();

std::chrono::milliseconds 
std::chrono::seconds
std::chrono::minutes 
std::chrono::hours

std::this_thread::sleep_for(std::chrono::milliseconds(100));
std::this_thread::sleep_for(std::chrono::seconds(1));

// 将栅格地图原点设置在整张图的(-20, -20)位置
grid_map.info.origin.x = -20;
grid_map.info.origin.y = -20;

index = (int)( (x - origin_x) / resolution) + ((int)((y - origin_y) / resolution)) * width;