在move base中使用给定的全局路径，实现局部避障_拟合一条曲线发送给move_base的全局路劲

个人理解，有问题请指正，谢谢！

在move base中global planner效果并不理想，很大程度上也影响局部规划的效果，因此尝试给出人工采取的全局路径，再用teb local planner局部避障。
一、move base 源码部分

1. move base 框架图
   
   图出自：Estrategias de diseño basadas en patrones de un subsistema de movimiento para un robot pulverizador

目前ROS中global planner主要包括：A*和Dijkstra，local planner主要包括：dwa、teb等。

2. 构造函数初始过程

首先列出move base中订阅的topic：

ros::NodeHandle simple_nh("move_base_simple");
    goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
1
2

此处geometry_msgs::PoseStamped类型可通过rviz给出目标点，在回调函数goalCB中，将geometry_msgs::PoseStamped类型转换成move_base_msgs::MoveBaseActionGoal类型通过topic"goal"发送出去。
发布的topic有：

    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);
1
2
3
4
5

创建服务：a.开始创建地图（详情见5.部分） b.清除地图

	//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);
1
2
3
4

c.动态参数服务（详情见4.部分）

dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);
1

d.move base 服务（详情见6.部分）

as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
1

然后比较重要的参数是确定全局与局部规划器，参数设置完成后，创建全局和局部的规划器。

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"));
1
2

启动开始更新 costmap，和创建地图相关服务。

costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
    // Start actively updating costmaps based on sensor data
    planner_costmap_ros_->start();
    controller_costmap_ros_->start();
1
2
3
4

加载指定的恢复器，如果失败的话使用默认恢复（具体流程在3.部分）。

    if(!loadRecoveryBehaviors(private_nh)){
      loadDefaultRecoveryBehaviors();
    }
1
2
3

确定初始状态为 PLANNING，生成一个路径。
此时我们将从列表的开头开始执行恢复行为。

  //initially, we'll need to make a plan
    state_ = PLANNING;
    recovery_index_ = 0;
1
2
3

所有参数设置完成后，便可以启动 move base 服务。

3. 恢复的规划器

同全局规划器和局部规划器一样，不过其实在局部代价地图或全局代价地图中找不到路径时被调用，
比如在默认规划器中： rotate_recovery 原地转360度，clear_costmap_recovery 将地图恢复到静态地图的样子。

if(node.getParam("recovery_behaviors", behavior_list)){...}
else{
//if no recovery_behaviors are specified, we'll just load the defaults
   return false;
}
1
2
3
4
5

在loadRecoveryBehaviors中，如果没有定义规划器类型，则直接使用默认的规划器loadDefaultRecoveryBehaviors。

 void MoveBase::loadDefaultRecoveryBehaviors(){
    recovery_behaviors_.clear();
    try{
      //we need to set some parameters based on what's been passed in to us to maintain backwards compatibility
      //first, we'll load a recovery behavior to clear the costmap
      //next, we'll load a recovery behavior to rotate in place
      //next, we'll load a recovery behavior that will do an aggressive reset of the costmap
      //we'll rotate in-place one more time
    }
    catch(pluginlib::PluginlibException& ex){
    }
    return;
  }

1
2
3
4
5
6
7
8
9
10
11
12
13
14

4. 动态参数服务回调函数 reconfigureCB

首先设置默认值

    if(!setup_)
    {
      last_config_ = config;
      default_config_ = config;
      setup_ = true;
      return;
    }

1
2
3
4
5
6
7
8

如果当前设置的全局或者局部规划器与之前不同，则创建新的规划器，在当前规划器结束之后，清空掉旧的规划路径，并重新规划，以全局规划器代码为例：

 if(config.base_global_planner != last_config_.base_global_planner) {
      boost::shared_ptr<nav_core::BaseGlobalPlanner> old_planner = planner_;
      //initialize the global planner
      ROS_INFO("Loading global planner %s", config.base_global_planner.c_str());
      try {
        planner_ = bgp_loader_.createInstance(config.base_global_planner);
        // wait for the current planner to finish planning
        boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
        // 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_);
        lock.unlock();
      } catch (const pluginlib::PluginlibException& ex) {
        planner_ = old_planner;
        config.base_global_planner = last_config_.base_global_planner;
      }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

5. 创建地图服务回调函数 planService

确定起始点，如果服务msg中给出的起始点为空，则设置全局变量global_pose为起始点。

    if(req.start.header.frame_id.empty()) {
        geometry_msgs::PoseStamped global_pose;
        start = global_pose;
    }
    else {
        start = req.start;
    }
1
2
3
4
5
6
7

然后尝试制定出准确的目标计划
使用 move base 在公差范围内寻找可行的路径。

boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
planner_->makePlan(start, req.goal, global_plan)
1
2

如果失败则在规定公差范围内，向外寻找可行目标点，然后再使用 move base 规划。

if(planner_->makePlan(start, p, global_plan)){
	if(!global_plan.empty()){
		if (make_plan_add_unreachable_goal_) {
			global_plan.push_back(req.goal);
        }
		found_legal = true;
		break;
     }
}
else{
 ROS_DEBUG_NAMED("move_base","Failed");
 }
1
2
3
4
5
6
7
8
9
10
11
12

如果此时成功规划全局路径，则把原始的目标点也添加到全局路径中去，万一在局部规划中可达呢。如果失败的话就继续寻找合适的目标点。
最终把找到的全局路径，通过服务的msg返回。

    //copy the plan into a message to send out
    resp.plan.poses.resize(global_plan.size());
    for(unsigned int i = 0; i < global_plan.size(); ++i){
      resp.plan.poses[i] = global_plan[i];
    }

1
2
3
4
5
6

6. move base 服务回调函数
   在源码中注释非常详细，首先判断目标点是否为一个有效的四元数，然后将目标点转换为全局坐标系下。

 if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation));
 
 geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
1
2
3

此时，已经有了目标点，便开始路径规划，唤醒相关线程。同时开始不断循环，判断是否有新的目标点抢断。
如果被抢段，则执行与初始相似的步骤，1.判断是否有效 2.转换坐标系 3.唤醒相关线程等。

7. MoveBase::planThread()路径规划线程

首先确定了目标点goal：

geometry_msgs::PoseStamped temp_goal = planner_goal_;
1

其中planner_goal_是通过"MoveBase::executeCb"赋值得到，

geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
planner_goal_ = goal;
1
2

在得到目标点goal后，便开始尝试规划路径，得到全局路径planner_plan_

planner_plan_->clear();
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);

new_global_plan_ = true;
1
2
3
4

同样在"MoveBase::executeCb"中，调用了函数MoveBase::executeCycle，在这里规划了局部路径。

bool done = executeCycle(goal);
1

首先，在MoveBase::executeCycle中，判断是否有全局路径，即设置的new_global_plan_，然后将全局路径传局部路径中，用于初始状态。

tc_->setPlan(*controller_plan_)
1

二、设定全局路径
源码中，move base经过全局路径规划器规划出全局路径，然后通过setPlan()传到局部路径规划器中，即在planService中寻找一个合适的目标点，并通过makePlan确定全局路径global_plan。

1. 设定目标点goal，在源码中通过goalCB确定goal，现在将全局路径中最后一个路径点设置成目标点goal。此处在发送全局路径时实现，同时发送目标点。

  global_path_pub_ = nh_.advertise<nav_msgs::Path>("globalPlan", 10, true);
  goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10, true);
1
2

原来的话题依然保留，然后加上接收全局路径的topic。

    //global plan
    path_sub_ = nh.subscribe<nav_msgs::Path>("globalPlan", 1, boost::bind(&MoveBase::pathCB, this, _1));
1
2

2. 在回调函数pathCB中设定全局路径。
   需要将nav_msgs::Path赋值给planner_plan_，

	geometry_msgs/PoseStamped[] poses
std::vector<geometry_msgs::PoseStamped>* planner_plan_;
1
2

即将用数组对容器赋值：

  void MoveBase::pathCB(const nav_msgs::Path::ConstPtr& path){
    ROS_INFO("move_base accept global plan");
    gotPlan = true;
    std::vector<geometry_msgs::PoseStamped>* planner_plan_temp; 
    for(int i = 0;i<path->poses.size();i++){
        planner_plan_temp->push_back(path->poses[i]);
    }
    globa_planner_plan_ = planner_plan_temp;
  }
1
2
3
4
5
6
7
8
9

3. 在 executeCb执行后才会执行planThread线程，在线程中设置全局路径：

      //run planner
      planner_plan_->clear();
      //bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
	  planner_plan_ = globa_planner_plan_;
1
2
3
4

此时不使用makePlan得到的全局路径，因此注释掉该代码，将gotPlan设置成全局变量，在pathCB中赋值。
将planService函数注释，直接返回，防止生成新的全局路径。

// A service call that can be made when the action is inactive that will return a plan
bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
1
2

（未完，有待测试）
补充：也可以自己编写全局路径规划器插件实现！