注释代码：void MoveBaseplanThread ROS_DEBUG_NAMEDmove_base_plan_threadStarting planner thread; rosNodeHandle n; rosTimer timer; bool wait_for_wake = false; boostunique_lockboostrecursive_mut

// 定义MoveBase类的planThread()函数 void MoveBase::planThread(){ // 输出ROS_DEBUG信息 ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread..."); // 初始化ROS节点 ros::NodeHandle n; // 定义ROS定时器 ros::Timer timer; // 定义是否等待唤醒 bool wait_for_wake = false; // 加锁 boost::unique_lockboost::recursive_mutex lock(planner_mutex_); // 循环 while(n.ok()){ // 检查是否应该运行规划器（锁定了互斥量） while(wait_for_wake || !runPlanner_){ // 如果不应该运行规划器，则暂停此线程 ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending"); planner_cond_.wait(lock); wait_for_wake = false; } // 获取当前时间 ros::Time start_time = ros::Time::now(); // 时间到了，要规划了！先获取目标点的拷贝，然后解锁互斥量 geometry_msgs::PoseStamped temp_goal = planner_goal_; lock.unlock(); // 输出ROS_DEBUG信息 ROS_DEBUG_NAMED("move_base_plan_thread","Planning..."); // 运行全局规划器，得到规划路径 planner_plan_->clear(); bool gotPlan = n.ok() && makePlan(temp_goal, planner_plan_); // 如果得到规划路径 if(gotPlan){ ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size()); // 交换规划路径指针（互斥量下） std::vector<geometry_msgs::PoseStamped> temp_plan = planner_plan_; lock.lock(); planner_plan_ = latest_plan_; latest_plan_ = temp_plan; last_valid_plan_ = ros::Time::now(); planning_retries_ = 0; new_global_plan_ = true; // 输出ROS_DEBUG信息 ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner"); // 如果仍未到达目标点，启动控制器 if(runPlanner_) state_ = CONTROLLING; if(planner_frequency_ <= 0) runPlanner_ = false; lock.unlock(); } // 如果未得到规划路径且机器人处于规划状态（未移动） else if(state_==PLANNING){ ROS_DEBUG_NAMED("move_base_plan_thread","No Plan..."); ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_); // 检查是否超时或者重试次数已达到最大值 lock.lock(); planning_retries_++; if(runPlanner_ && (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){ // 进入障碍物清除模式 state_ = CLEARING; runPlanner_ = false; publishZeroVelocity(); recovery_trigger_ = PLANNING_R; } lock.unlock(); } // 加锁 lock.lock(); // 如果设置了规划频率，则设置休眠时间 if(planner_frequency_ > 0){ ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now(); if (sleep_time > ros::Duration(0.0)){ wait_for_wake = true; timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this); } } }