MoveBase::executeCycle分析

本地规划核心工作函数

1. boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);//信号机制
   boost::recursive_mutex configuration_mutex_;//构造这样的锁止机构

   boost::recursive_mutex::scoped_lock 

2.     geometry_msgs::Twist cmd_vel;
   
       //update feedback to correspond to our curent position
       tf::Stamped<tf::Pose> global_pose;
       planner_costmap_ros_->getRobotPose(global_pose);//全局代价地图获得全局位姿
       geometry_msgs::PoseStamped current_position;
       tf::poseStampedTFToMsg(global_pose, current_position);

    tf::poseStampedTFToMsg 将Stamped <Pose>转换为PoseStamped msg，将global_pose转换为current_position

3. move_base_msgs::MoveBaseFeedback feedback;
   feedback.base_position = current_position;
   as_->publishFeedback(feedback);

   move_base_msgs::MoveBaseFeedback反馈goal的服务状态；as_->publishFeedback Publishes feedback for a given goal.

4. hypot计算三角形斜边->MoveBaes::distance()

       if(distance(current_position, oscillation_pose_) >= oscillation_distance_)//判断移动是否足够
       {//private_nh.param("oscillation_distance", oscillation_distance_, 0.5);后出现一句配置语句
         last_oscillation_reset_ = ros::Time::now();
         oscillation_pose_ = current_position;//重置振荡时间
   
         //if our last recovery was caused by oscillation, we want to reset the recovery index 
         if(recovery_trigger_ == OSCILLATION_R)
           recovery_index_ = 0;
       }

   geometry_msgs::PoseStamped oscillation_pose_;只在此处出现这一全局变量，如果处于振荡状态，记录下当前振荡时间；如果上次的恢复是由振荡触发的，则重置恢复指标

5.     //check that the observation buffers for the costmap are current, we don't want to drive blind
       if(!controller_costmap_ros_->isCurrent()){//是否是最新的局部规划地图
         ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str());
         publishZeroVelocity();//速度置0
         return false;
       }

   然后检查controller_costmap_ros_ 是否是最新的，不是最新的就速度置0，并返回错误

6. //if we have a new plan then grab it and give it to the controller
       if(new_global_plan_){//判断是否全局规划得到可执行的全局路径,标志物判断是否有新的全局规划
         //make sure to set the new plan flag to false
         new_global_plan_ = false;
   
         ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers");
   
         //do a pointer swap under mutex
         std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;
   
         boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
         controller_plan_ = latest_plan_;
         latest_plan_ = temp_plan;
         lock.unlock();
         ROS_DEBUG_NAMED("move_base","pointers swapped!");
   
         if(!tc_->setPlan(*controller_plan_)){
           //ABORT and SHUTDOWN COSTMAPS
           ROS_ERROR("Failed to pass global plan to the controller, aborting.");
           resetState();
   
           //disable the planner thread
           lock.lock();
           runPlanner_ = false;
           lock.unlock();
   
           as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
           return true;
         }
   
         //make sure to reset recovery_index_ since we were able to find a valid plan
         if(recovery_trigger_ == PLANNING_R)
           recovery_index_ = 0;
       }

   判断是否全局规划得到可执行的全局路径,标志物判断是否有新的全局规划

7. 根据状态值进行判断

   
         case PLANNING:
           {
             boost::recursive_mutex::scoped_lock lock(planner_mutex_);
             runPlanner_ = true;
             planner_cond_.notify_one();
           }
           ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state.");
           break;

   全局路径规划状态

8.       //if we're controlling, we'll attempt to find valid velocity commands
         case CONTROLLING:
           ROS_DEBUG_NAMED("move_base","In controlling state.");
   
           //check to see if we've reached our goal
           if(tc_->isGoalReached()){//是否到达目标点
             ROS_DEBUG_NAMED("move_base","Goal reached!");
             resetState();
   
             //disable the planner thread
             boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
             runPlanner_ = false;
             lock.unlock();
   
             as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");//动作编程，返回到达目的地
             return true;
           }
   
           //check for an oscillation condition
           if(oscillation_timeout_ > 0.0 &&
               last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
           {
             publishZeroVelocity();
             state_ = CLEARING;
             recovery_trigger_ = OSCILLATION_R;
           }//是否满足振荡条件
           
           {
            boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex()));
           
           if(tc_->computeVelocityCommands(cmd_vel)){
             ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
                              cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );
             last_valid_control_ = ros::Time::now();
             //make sure that we send the velocity command to the base
             vel_pub_.publish(cmd_vel);
             if(recovery_trigger_ == CONTROLLING_R)
               recovery_index_ = 0;
           }
           else {
             ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan.");
             ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_);
   
             //check if we've tried to find a valid control for longer than our time limit
             if(ros::Time::now() > attempt_end){
               //we'll move into our obstacle clearing mode
               publishZeroVelocity();
               state_ = CLEARING;
               recovery_trigger_ = CONTROLLING_R;
             }
             else{
               //otherwise, if we can't find a valid control, we'll go back to planning
               last_valid_plan_ = ros::Time::now();
               planning_retries_ = 0;
               state_ = PLANNING;
               publishZeroVelocity();
   
               //enable the planner thread in case it isn't running on a clock
               boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
               runPlanner_ = true;
               planner_cond_.notify_one();
               lock.unlock();
             }
           }
           }
   
           break;

   局部控制状态

9. 
         //we'll try to clear out space with any user-provided recovery behaviors
         case CLEARING:
           ROS_DEBUG_NAMED("move_base","In clearing/recovery state");
           //we'll invoke whatever recovery behavior we're currently on if they're enabled
           if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){
             ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_, recovery_behaviors_.size());
             recovery_behaviors_[recovery_index_]->runBehavior();
   
             //we at least want to give the robot some time to stop oscillating after executing the behavior
             last_oscillation_reset_ = ros::Time::now();
   
             //we'll check if the recovery behavior actually worked
             ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");
             last_valid_plan_ = ros::Time::now();
             planning_retries_ = 0;
             state_ = PLANNING;
   
             //update the index of the next recovery behavior that we'll try
             recovery_index_++;
           }
           else{
             ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it.");
             //disable the planner thread
             boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
             runPlanner_ = false;
             lock.unlock();
   
             ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this.");
   
             if(recovery_trigger_ == CONTROLLING_R){
               ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");
               as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
             }
             else if(recovery_trigger_ == PLANNING_R){
               ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
               as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
             }
             else if(recovery_trigger_ == OSCILLATION_R){
               ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
               as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
             }
             resetState();
             return true;
           }
           break;

   CLEARING 故障恢复状态：故障恢复触发的方式也有三种 PLANNING_R（全局路径规划）, CONTROLLING_R（局部路径规划）, OSCILLATION_R（振荡摆动）