move_base 源码阅读

https://blog.csdn.net/u013834525/article/list/1?

https://www.cnblogs.com/shhu1993/p/6323699.html#%E6%BA%90%E7%A0%81

http://www.pianshen.com/article/3044120500/

  void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
  {
    //判断goal有效性
    //统一转换到全局坐标系
    geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);

    //启动2.1.3中所说的新线程来获取规划路径
    runPlanner_ = true;
    //唤醒等待条件变量的一个线程：即调用planner_cond_.wait()的MoveBase::planThread()
    planner_cond_.notify_one();

    current_goal_pub_.publish(goal);//这个话题貌似只有一些rviz上用来显示的

    ros::NodeHandle n;
    while(n.ok())
    {
      if(c_freq_change_)
      {
        //更改控制周期
      }

      if(as_->isPreemptRequested()){//是否有抢占请求，根据参考1第8点的说法，SimpleActionServer的政策是，新的goal都会抢占旧的goal，这里应该只是为了清除新goal的一些状态。（那些待定的goal也有可能抢占，或者可以直接cancel抢占Current？）
        if(as_->isNewGoalAvailable()){//如果是新的goal这个函数会将其他goal设置为被抢占状态
          //if we're active and a new goal is available, we'll accept it, but we won't shut anything down
          move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();//接收new goal

          if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){
            return;//无效退出
          }

          goal = goalToGlobalFrame(new_goal.target_pose);//统一转换到全局坐标系

          //唤醒规划线程
          lock.lock();
          planner_cond_.notify_one();
          lock.unlock();
        }
        else {//如果是cancel了
          //if we've been preempted explicitly we need to shut things down
          resetState();//停止规划线程、停车等
          as_->setPreempted();//设置current goal被抢占

          return;
        }
      }

      //we also want to check if we've changed global frames because we need to transform our goal pose
      if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
        goal = goalToGlobalFrame(goal);//判断这段时间是否改了坐标系
      }

      //the real work on pursuing a goal is done here
      bool done = executeCycle(goal, global_plan);//这是控制机器人跟踪的主要函数，下面详细讲

      //if we're done, then we'll return from execute
      if(done)
        return;
      r.sleep();//控制周期睡眠
    }

    //ros不ok时清除退出
    return;
  }

  void MoveBase::planThread(){
    bool wait_for_wake = false;
    boost::unique_lock<boost::mutex> lock(planner_mutex_);
    while(n.ok()){
      //check if we should run the planner (the mutex is locked)
      while(wait_for_wake || !runPlanner_){
        planner_cond_.wait(lock);//使线程进入睡眠，等待MoveBase::executeCb，以及规划周期的唤醒
        wait_for_wake = false;
      }

      //planner_goal_是在MoveBase::executeCb中得到的目标位姿，需要上锁保证线程安全
      geometry_msgs::PoseStamped temp_goal = planner_goal_;
      lock.unlock();

      //run planner
      planner_plan_->clear();//清除原来规划出的路径向量
      //MoveBase::makePlan作用是获取机器人的位姿作为起点，然后调用全局规划器的makePlan返回规划路径，存储在planner_plan_
      bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);

      if(gotPlan){//如果规划出路径则更新相应路径，并将state_转换为CONTROLLING状态
        std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;

        lock.lock();
        planner_plan_ = latest_plan_;
        latest_plan_ = temp_plan;//将最新的全局路径放到latest_plan_中，其在MoveBase::executeCycle中被传递到controller_plan_中，利用锁来进行同步

        //make sure we only start the controller if we still haven't reached the goal
        if(runPlanner_)
          state_ = CONTROLLING;
        lock.unlock();
      }
      //如果没有规划出路径，并且处于PLANNING状态，则判断是否超过最大规划周期或者规划次数
      //如果是则进入自转模式，否则应该会等待MoveBase::executeCycle的唤醒再次规划
      else if(state_==PLANNING){//仅在MoveBase::executeCb及其调用的MoveBase::executeCycle
        //或者重置状态时会被设置为PLANNING，一般是刚获得新目标，或者得到路径但计算不出下一步控制时重新进行路径规划
        ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);

        lock.lock();
        if(runPlanner_ &&
           (ros::Time::now() > attempt_end || ++planning_retries_ > uint32_t(max_planning_retries_))){
          //we'll move into our obstacle clearing mode
          state_ = CLEARING;
          publishZeroVelocity();//直接向cmd_vel话题发布000的速度信息
          recovery_trigger_ = PLANNING_R;
        }
        lock.unlock();
      }

      //take the mutex for the next iteration
      lock.lock();

      //如果还没到规划周期则定时器睡眠，在定时器中断中通过planner_cond_唤醒，这里规划周期为0
      if(planner_frequency_ > 0){
      }
    }
  }