mped temp_goal = planner_goal_;
//路径规划global
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
if(gotPlan){
//成功规划路径
ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");
}
//if we didn't get a plan and we are in the planning state (the robot isn't moving)
else if(state_==PLANNING){
//check if we've tried to make a plan for over our time limit
if(ros::Time::now() > attempt_end && runPlanner_){
//we'll move into our obstacle clearing mode
state_ = CLEARING;
publishZeroVelocity();
recovery_trigger_ = PLANNING_R;
}
}
//定时器,多久没有规划路径,就通知一次规划路径
timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
}
}
//这个是在global costmap下做的global planner
bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
// global costmap
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex()));
//得到机器人现在的位置信息
if(!planner_costmap_ros_->getRobotPose(global_pose)) {
ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
return false;
}
//global planner
if(!planner_->makePlan(start, goal, plan) || plan.empty()){
ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y);
return false;
}
}
bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){
//发布速度topic
geometry_msgs::Twist cmd_vel;
//push the feedback out
//发布一些反馈信息
move_base_msgs::MoveBaseFeedback feedback;
feedback.base_position = current_position;
as_->publishFeedback(feedback);
//check to see if we've moved far enough to reset our oscillation timeout
if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
{
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;
}
//if we have a new plan then grab it and give it to the controller
if(new_global_plan_){
std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;
latest_plan_ = temp_plan;
//这是local planner的规划
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;
}
//the move_base state machine, handles the control logic for navigation
switch(state_){
//if we are in a planning state, then we'll attempt to make a plan
case PLANNING:
runPlanner_ = true;
planner_cond_.notify_one();
ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state.");
break;
//if we're controlling, we'll attempt to find valid velocity c |