move_base.h和move_base.cpp

ROS

move_base_node.cpp
move_base::MoveBase **move_base( buffer );
创建了一个move_base( buffer )对象

move_base.h
boost::shared_ptr
[C++] Boost智能指针——boost::shared_ptr（使用及原理分析）https://blog.csdn.net/dddxxxx/article/details/87929470

move_base.cpp
1.MoveBase(tf2_ros::Buffer& tf)
MoveBase类的构造函数。构造函数后面的冒号起分割作用，是类给成员变量赋值的方法，初始化列表，更适用于成员变量的常量const型。

//关于goal实际中用这个
60： as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);//与成员函数executeCb的第一个参数move_base_goal相关

boost :: bind是标准函数std :: bind1st和std :: bind2nd的泛化。 它支持任意函数对象，函数，函数指针和成员函数指针，并且能够将任何参数绑定到特定值或将输入参数路由到任意位置。bind（f，_1，5）（x）等价于f（x，5）; 这里_1是一个占位符参数，意思是“用第一个输入参数替换”。
1 struct X
2 {
3 bool f(int a);
4 };
5
6 X x;
7 shared_ptr p(new X);
8 int i = 5;
9
10 bind(&X::f, ref(x), _1)(i); // x.f(i)
11 bind(&X::f, &x, _1)(i); // (&x)->f(i)
12 bind(&X::f, x, _1)(i); // x.f(i)
13 bind(&X::f, p, _1)(i); // p->f(i)

关于this：https://www.cnblogs.com/WindSun/p/11323143.html
this只能在成员函数中使用。this作用域是在类内部，当在类的非静态成员函数中访问类的非静态成员的时候，编译器会自动将对象本身的地址作为一个隐含参数传递给函数。也就是说，即使你没有写上this指针，编译器在编译的时候也是加上this的，它作为非静态成员函数的隐含形参，对各成员的访问均通过this进行。 设MovePoint为类的成员函数，其原型应该是
void MovePoint( Point *this, int a, int b);
第一个参数是指向该类对象的一个指针，我们在定义成员函数时没看见是因为这个参数在类中是隐含的。

//仿真时goal用这个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 nav_view and rviz
ros::NodeHandle simple_nh("move_base_simple");
goal_sub_ = simple_nh.subscribe("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

发布的消息：
//for commanding the base
vel_pub_ = nh.advertise("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise("current_goal", 0 );
ros::NodeHandle action_nh("move_base");
action_goal_pub_ = action_nh.advertise("goal", 1);

发布的服务：
//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);

//we're all set up now so we can start the action server？？？
as_->start();
dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~"));
dynamic_reconfigure::Server::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, 2);
dsrv->setCallback(cb);

2.void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level)
在MoveBase(tf2_ros::Buffer& tf)中使用，接收配置文件中的配置值
// 这里可能要变化，老师要求25m衔接，Clean up before initializing the new planner
planner_plan_->clear();
latest_plan_->clear();
controller_plan_->clear();
resetState();
planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_);

3.void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal)
在ROS目标回调中，将PoseStamped包装在操作消息中，然后重新发送到服务器。仿真时用这个发布目标，MoveBase(tf2_ros::Buffer& tf)中用，但会转换成action_goal，最后action_goal_pub_.publish(action_goal);

4.void MoveBase::clearCostmapWindows(double size_x, double size_y)
* @brief Clears obstacles within a window around the robot
* @param size_x The x size of the window
* @param size_y The y size of the window
//clear the planner's costmap
//clear the controller's costmap

服务请求和回答涉及的函数
5. bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
* @brief A service call that clears the costmaps of obstacles
* @param req The service request
* @param resp The service response
* @return True if the service call succeeds, false otherwise
6. bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
这个函数可以作为终点的生成改的地方，还有输入起点到底是机器人位置还是当前路线终点位置改的地方。 如果是当前路线的goal作为起点，则要加语句
* @brief A service call that can be made when the action is inactive that will return a plan
* @param req The goal request
* @param resp The plan request
* @return True if planning succeeded, false otherwise

**//first try to make a plan to the exact desired goal**
std::vector<geometry_msgs::PoseStamped> global_plan;
if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){
  ROS_DEBUG_NAMED("move_base","Failed to find a plan to exact goal of (%.2f, %.2f), **searching for a feasible goal within tolerance**", 
      req.goal.pose.position.x, req.goal.pose.position.y);

  ****//search outwards for a feasible goal within the specified tolerance****
  以下代码为指定的goal到不了怎么在可行范围内生成终点
  geometry_msgs::PoseStamped p;
  p = req.goal;
  bool found_legal = false;
  float resolution = planner_costmap_ros_->getCostmap()->getResolution();
  float search_increment = resolution*3.0;
  if(req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance;
  for(float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment) {
    for(float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment) {
      for(float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment) {

        //don't search again inside the current outer layer
        if(x_offset < max_offset-1e-9 && y_offset < max_offset-1e-9) continue;

        //search to both sides of the desired goal
        for(float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0) {

          //if one of the offsets is 0, -1*0 is still 0 (so get rid of one of the two)
          if(y_offset < 1e-9 && y_mult < -1.0 + 1e-9) continue;

          for(float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0) {
            if(x_offset < 1e-9 && x_mult < -1.0 + 1e-9) continue;

            p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult;
            p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult;

            if(planner_->makePlan(start, p, global_plan)){
              if(!global_plan.empty()){

                if (make_plan_add_unreachable_goal_) {
                  //adding the (unreachable) original goal to the end of the global plan, in case the local planner can get you there
                  //(the reachable goal should have been added by the global planner)
                  global_plan.push_back(req.goal);
                }

                found_legal = true;
                ROS_DEBUG_NAMED("move_base", "Found a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
                break;
              }
            }
            else{
              ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
            }
          }
        }
      }
    }
  }
}

//
    struct TimerEvent
00135 {
00136   Time last_expected;                     
00137   Time last_real;                         
00138 
00139   Time current_expected;                  
00140   Time current_real;                      
00141 
00142   struct
00143   {
00144     WallDuration last_duration;           
00145   } profile;
00146 };

7.MoveBase::~MoveBase()
MoveBase类的析构函数，释放各种。
我们25米续接的话，用不用 delete planner_plan_;??? delete latest_plan_;？？？

**8.

9.bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector& plan)
* @brief Make a new global plan
* @param goal The goal to plan to
* @param plan Will be filled in with the plan made by the planner
* @return True if planning succeeds, false otherwise
//make sure to set the plan to be empty initially
plan.clear();这个相当于我把route清空，但在这之前原先的route需要重新赋给一个容器，可以在这句话之前添加，为了25米续接
全局规划最终的输出是plan容器

10.void MoveBase::publishZeroVelocity()
发布Publishes a velocity command of zero to the base，即让车静止

11. bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q)
检查导航目标四元数是否有效，无效则抛弃目标？？？？？？怎么理解里面的代码

12.geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg)
转变goal坐标到全局坐标系下

13.void MoveBase::wakePlanner(const ros::TimerEvent& event);
* @brief This is used to wake the planner at periodic intervals.

        struct TimerEvent
    00135 {
    00136   Time last_expected;                     
    00137   Time last_real;                         
    00138 
    00139   Time current_expected;                  
    00140   Time current_real;                      
    00141 
    00142   struct
    00143   {
    00144     WallDuration last_duration;           
    00145   } profile;
    00146 };

收到goal的action后，通过current_real触发，看第15个函数的笔记

14.void MoveBase::planThread()
25米续接涉及这个函数

15.void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
这个是我们实际中要用的
//we have a goal so start the planner

boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();//wakePlanner()
lock.unlock();

current_goal_pub_.publish(goal);
std::vector<geometry_msgs::PoseStamped> global_plan;

此外，该函数还实现了如有新的目标到来怎么办

16. double MoveBase::distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2)
return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y);

17.bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector& global_plan)
这个函数是在上一个executeCb函数中被使用
* @brief Performs a control cycle
* @param goal A reference to the goal to pursue
* @param global_plan A reference to the global plan being used
* @return True if processing of the goal is done, false otherwise
以下获得车当前位置
//update feedback to correspond to our curent position
geometry_msgs::PoseStamped global_pose;
getRobotPose(global_pose, planner_costmap_ros_);
const geometry_msgs::PoseStamped& current_position = global_pose;
该函数实现了“the move_base state machine”, handles the control logic for navigation

18.bool MoveBase::loadRecoveryBehaviors(ros::NodeHandle node)
* @brief Load the recovery behaviors for the navigation stack from the parameter server
* @param node The ros::NodeHandle to be used for loading parameters
* @return True if the recovery behaviors were loaded successfully, false otherwise
RecoveryBehavior用来应对导航过程中各模块的故障，当全局规划故障、局部规划故障、震荡时都会进入到恢复行为中，它先清理周围一定范围以外的costmap（障碍层），接下来重新执行规划，若不奏效，则旋转180度，再执行规划。交替两次后，已转过360度，若还是没能排除故障，则恢复行为失败，关闭Movebase规划器。
在Movebase的全局规划和局部规划模块中，当规划失败时都会进入到恢复行为中，在loadDefaultRecoveryBehaviors函数中，按顺序加载了①“cons_clear”、②“rotate”、③“ags_clear”、④“rotate”。实际上，①/③都是ClearCostmapRecovery类，负责清理机器人一定范围外的costmap上的数据，区别在于③保留的范围更小，清理的更多。恢复行为是按照列表顺序调用的，当①、②无效后，才会执行③，清理更多区域。②/④都是RotateRecovery类，使机器人原地旋转。

19.void MoveBase::loadDefaultRecoveryBehaviors()
* @brief Loads the default recovery behaviors for the navigation stack
*这个函数和上个函数是在规划失败陷入困境时怎么办，这个是原地旋转

20.void MoveBase::resetState()
// Disable the planner thread，then Reset statemachine,可在bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector& global_plan) 中的状态机看有哪些状态

21.bool MoveBase::getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap)
该函数在bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector& global_plan) 中被使用

另外涉及的知识点：
c++并发编程之互斥锁（mutex）的使用方法https://www.cnblogs.com/zhanghu52030/p/9166737.html