void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)//接收目标点并进行坐标变换并发布
-
typedef boost :: shared_ptr <:: move_base_msgs :: MoveBaseGoal > move_base_msgs :: MoveBaseGoalPtr typedef :: move_base_msgs :: MoveBaseGoal_ <std :: allocator <void>> move_base_msgs :: MoveBaseGoal
-
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
-
geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
将goal转换成全局地图下的goal
-
//we have a goal so start the planner boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);//给该线程上锁 planner_goal_ = goal;//传入目标并开启线程 runPlanner_ = true; planner_cond_.notify_one(); lock.unlock();//打开互斥锁
开启planner
boost::condition_variable_any planner_cond_;//用来进行多线程同步的
调用notify_one的时候,启用一个线程,解除阻止当前正在等待此条件的线程之一。如果没有线程在等待,则该函数不执行任何操作。如果超过一个,则未指定选择哪个线程。
-
current_goal_pub_.publish(goal); std::vector<geometry_msgs::PoseStamped> global_plan; ros::Rate r(controller_frequency_);//private_nh.param("controller_frequency", controller_frequency_, 20.0);
发布目标,controller_frequency_向底盘控制移动话题cmd_vel发送命令的频率.
-
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.
-
进入循环
-
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);
-
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;
唤醒规划师线程,以便它可以干净地退出。