MoveBase::executeCb分析

 void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)//接收目标点并进行坐标变换并发布

1. typedef boost :: shared_ptr <:: move_base_msgs :: MoveBaseGoal > move_base_msgs :: MoveBaseGoalPtr
   
   typedef :: move_base_msgs :: MoveBaseGoal_ > move_base_msgs :: MoveBaseGoal

2.  if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){//目标输入合法性检查
         as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");//通知目标在处理过程中遇到错误必须终止
         return;
       }

   检查传入参数的合法性，主要是pose.orientation

3. geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);

   将goal转换成全局地图下的goal

4.     //we have a goal so start the planner
       boost::unique_lock lock(planner_mutex_);//给该线程上锁
       planner_goal_ = goal;//传入目标并开启线程
       runPlanner_ = true;
       planner_cond_.notify_one();
       lock.unlock();//打开互斥锁

   开启planner

   boost::condition_variable_any planner_cond_;//用来进行多线程同步的

   调用notify_one的时候，启用一个线程，解除阻止当前正在等待此条件的线程之一。如果没有线程在等待，则该函数不执行任何操作。如果超过一个，则未指定选择哪个线程。

5. current_goal_pub_.publish(goal);
   std::vector global_plan;
   ros::Rate r(controller_frequency_);//private_nh.param("controller_frequency", controller_frequency_, 20.0);

   发布目标，controller_frequency_向底盘控制移动话题cmd_vel发送命令的频率.

6. if(shutdown_costmaps_){
       ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");
       planner_costmap_ros_->start();
       controller_costmap_ros_->start();
   }

   shutdown_costmaps:当move_base在不活动状态时,是否关掉costmap.

7. 进入循环

•       if(c_freq_change_)//是否改变控制频率
        {
          ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
          r = ros::Rate(controller_frequency_);
          c_freq_change_ = false;
        }

  改变控制频率

• if(as_->isPreemptRequested()){//是否是可以抢占的
          if(as_->isNewGoalAvailable()){//是否有新的目标点
            //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();
  
            if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){
              as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
              return;
            }//检查参数是否合法
  
            goal = goalToGlobalFrame(new_goal.target_pose);//转换坐标
  
            //we'll make sure that we reset our state for the next execution cycle
            recovery_index_ = 0;
            state_ = PLANNING;//状态改变为PLANNING全局路径规划状态
  
            //we have a new goal so make sure the planner is awake
            lock.lock();
            planner_goal_ = goal;
            runPlanner_ = true;
            planner_cond_.notify_one();//启用线程
            lock.unlock();
  
            //publish the goal point to the visualizer
            ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
            current_goal_pub_.publish(goal);//发布目标
  
            //make sure to reset our timeouts and counters
            last_valid_control_ = ros::Time::now();
            last_valid_plan_ = ros::Time::now();
            last_oscillation_reset_ = ros::Time::now();
            planning_retries_ = 0;
          }
          else {//没有发现新的目标点
            //if we've been preempted explicitly we need to shut things down
            resetState();
  
            //notify the ActionServer that we've successfully preempted
            ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
            as_->setPreempted();//将活动目标的状态设置为抢占
  
            //we'll actually return from execute after preempting
            return;
          }
        }

  每次goal到来都被调用,如果有新目标到来而被抢占则唤醒planThread线程处理，否则为取消目标并挂起处理线程

• resetState()

•       if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
          goal = goalToGlobalFrame(goal);
  
          //we want to go back to the planning state for the next execution cycle
          recovery_index_ = 0;
          state_ = PLANNING;
  
          //we have a new goal so make sure the planner is awake
          lock.lock();
          planner_goal_ = goal;
          runPlanner_ = true;
          planner_cond_.notify_one();
          lock.unlock();
  
          //publish the goal point to the visualizer
          ROS_DEBUG_NAMED("move_base","The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
          current_goal_pub_.publish(goal);
  
          //make sure to reset our timeouts and counters
          last_valid_control_ = ros::Time::now();
          last_valid_plan_ = ros::Time::now();
          last_oscillation_reset_ = ros::Time::now();
          planning_retries_ = 0;
        }

  判断是否改变了成为全局frameID，如果没有重新改变然后再发布

• bool done = executeCycle(goal, global_plan);

  executeCycle分析

•       if(done)
          return;//如果完成则退出循环
  
        //check if execution of the goal has completed in some way
  
        ros::WallDuration t_diff = ros::WallTime::now() - start;
        ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec());
  
        r.sleep();
        //make sure to sleep for the remainder of our cycle time
        if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
          ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());

  cycleTime() 获取从开始睡眠的一个周期里面实际运行的时间在死循环最后sleep足够时间，以满足frequency的要求。如果死循环被退出，则在本函数末尾开启planThread，似的线程能正常执行到末尾，当线程再次在死循环中运行时，检查while(n.ok()){}会失败，然后线程正常清理退出。

• while(n.ok())循环结束

8.

    //wake up the planner thread so that it can exit cleanly
    lock.lock();
    runPlanner_ = true;
    planner_cond_.notify_one();
    lock.unlock();

    //if the node is killed then we'll abort and return
    as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
    return;

唤醒规划师线程，以便它可以干净地退出。