//move_base.cpp
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* Mike Phillips (put the planner in its own thread)
*********************************************************************/
#include <move_base/move_base.h>
#include <cmath>
#include <boost/algorithm/string.hpp>
#include <boost/thread.hpp>
#include <geometry_msgs/Twist.h>
namespace move_base {
//构造函数
MoveBase::MoveBase(tf::TransformListener& tf) :
tf_(tf),
as_(NULL),
planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
//初始化全局路径规划算法
bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
//初始化局部路径规划算法
blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
//初始化recovery的方法
recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) {
//分配内存actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction>,当执行as_->start()时调用MoveBase::executeCb函数,这个函数是move_base的核心
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
ros::NodeHandle private_nh("~");
ros::NodeHandle nh;
recovery_trigger_ = PLANNING_R;
//get some parameters that will be global to the move base node
std::string global_planner, local_planner;
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
private_nh.param("global_costmap/global_frame", global_frame_, std::string("/map"));
private_nh.param("planner_frequency", planner_frequency_, 0.0);
private_nh.param("controller_frequency", controller_frequency_, 20.0);
private_nh.param("planner_patience", planner_patience_, 5.0);
private_nh.param("controller_patience", controller_patience_, 15.0);
private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
private_nh.param("oscillation_distance", oscillation_distance_, 0.5);
//set up plan triple buffer
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
//set up the planner's thread
//为路径规划算法分配线程,执行MoveBase::planThread函数
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
//for comanding the base
//vel_pub_负责广播cmd_vel主题,这就是move_base的输出
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );
ros::NodeHandle action_nh("move_base");
action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
//we'll provide a mechanism for some people to send goals as PoseStamped messages over a topic
//they won't get any useful information back about its status, but this is useful for tools
//like nav_view and rviz
//接受goal的topic,调用MoveBase::goalCB函数,将goal封装成move_base_msgs::MoveBaseActionGoal类型,然后由action_goal_pub_发布
ros::NodeHandle simple_nh("move_base_simple");
goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
//we'll assume the radius of the robot to be consistent with what's specified for the costmaps
private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);
private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);
//create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();
//initialize the global planner
try {
planner_ = bgp_loader_.createInstance(global_planner);
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
exit(1);
}
//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();
//create a local planner
try {
tc_ = blp_loader_.createInstance(local_planner);
ROS_INFO("Created local_planner %s", local_planner.c_str());
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
exit(1);
}
// Start actively updating costmaps based on sensor data
planner_costmap_ros_->start();
controller_costmap_ros_->start();
//advertise a service for getting a planplanService
//make_plan_srv_接受Service的消息后,开始路径规划,得到global_plan的路径,跟as_得到的action一样,都会make_plan,但是不做局部路径规划(只是提供了个全局路径规划的接口)
make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
//advertise a service for clearing the costmaps
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
//if we shutdown our costmaps when we're deactivated... we'll do that now
if(shutdown_costmaps_){
ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
planner_costmap_ros_->stop();
controller_costmap_ros_->stop();
}
//load any user specified recovery behaviors, and if that fails load the defaults
if(!loadRecoveryBehaviors(private_nh)){
loadDefaultRecoveryBehaviors();
}
//initially, we'll need to make a plan
state_ = PLANNING;
//we'll start executing recovery behaviors at the beginning of our list
recovery_index_ = 0;
//we're all set up now so we can start the action server
//开始执行路径规划和导航算法
as_->start();
//动态调参
dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
}
void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level){
boost::recursive_mutex::scoped_lock l(configuration_mutex_);
//The first time we're called, we just want to make sure we have the
//original configuration
if(!setup_)
{
last_config_ = config;
default_config_ = config;
setup_ = true;
return;
}
if(config.restore_defaults) {
config = default_config_;
//if someone sets restore defaults on the parameter server, prevent looping
config.restore_defaults = false;
}
if(planner_frequency_ != config.planner_frequency)
{
planner_frequency_ = config.planner_frequency;
p_freq_change_ = true;
}
if(controller_frequency_ != config.controller_frequency)
{
controller_frequency_ = config.controller_frequency;
c_freq_change_ = true;
}
planner_patience_ = config.planner_patience;
controller_patience_ = config.controller_patience;
conservative_reset_dist_ = config.conservative_reset_dist;
recovery_behavior_enabled_ = config.recovery_behavior_enabled;
clearing_rotation_allowed_ = config.clearing_rotation_allowed;
shutdown_costmaps_ = config.shutdown_costmaps;
oscillation_timeout_ = config.oscillation_timeout;
oscillation_distance_ = config.oscillation_distance;
if(config.base_global_planner != last_config_.base_global_planner) {
boost::shared_ptr<nav_core::BaseGlobalPlanner> old_planner = planner_;
//initialize the global planner
ROS_INFO("Loading global planner %s", config.base_global_planner.c_str());
try {
planner_ = bgp_loader_.createInstance(config.base_global_planner);
// wait for the current planner to finish planning
boost::unique_lock<boost::mutex> lock(planner_mutex_);
// Clean up before initializing the new planner
planner_plan_->clear();
latest_plan_->clear();
controller_plan_->clear();
resetState();
planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_);
lock.unlock();
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
containing library is built? Exception: %s", config.base_global_planner.c_str(), ex.what());
planner_ = old_planner;
config.base_global_planner = last_config_.base_global_planner;
}
}
if(config.base_local_planner != last_config_.base_local_planner){
boost::shared_ptr<nav_core::BaseLocalPlanner> old_planner = tc_;
//create a local planner
try {
tc_ = blp_loader_.createInstance(config.base_local_planner);
// Clean up before initializing the new planner
planner_plan_->clear();
latest_plan_->clear();
controller_plan_->clear();
resetState();
tc_->initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
containing library is built? Exception: %s", config.base_local_planner.c_str(), ex.what());
tc_ = old_planner;
config.base_local_planner = last_config_.base_local_planner;
}
}
last_config_ = config;
}
//将goal转换为move_base_msgs::MoveBaseActionGoal类型
void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
ROS_DEBUG_NAMED("move_base","In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
move_base_msgs::MoveBaseActionGoal action_goal;
action_goal.header.stamp = ros::Time::now();
action_goal.goal.target_pose = *goal;
action_goal_pub_.publish(action_goal);
}
//清空机器人周围的正方形空间
void MoveBase::clearCostmapWindows(double size_x, double size_y){
tf::Stamped<tf::Pose> global_pose;
//clear the planner's costmap
planner_costmap_ros_->getRobotPose(global_pose);
std::vector<geometry_msgs::Point> clear_poly;
double x = global_pose.getOrigin().x();
double y = global_pose.getOrigin().y();
geometry_msgs::Point pt;
pt.x = x - size_x / 2;
pt.y = y - size_y / 2;
clear_poly.push_back(pt);
pt.x = x + size_x / 2;
pt.y = y - size_y / 2;
clear_poly.push_back(pt);
pt.x = x + size_x / 2;
pt.y = y + size_y / 2;
clear_poly.push_back(pt);
pt.x = x - size_x / 2;
pt.y = y + size_y / 2;
clear_poly.push_back(pt);
planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
//clear the controller's costmap
controller_costmap_ros_->getRobotPose(global_pose);
clear_poly.clear();
x = global_pose.getOrigin().x();
y = global_pose.getOrigin().y();
pt.x = x - size_x / 2;
pt.y = y - size_y / 2;
clear_poly.push_back(pt);
pt.x = x + size_x / 2;
pt.y = y - size_y / 2;
clear_poly.push_back(pt);
pt.x = x + size_x / 2;
pt.y = y + size_y / 2;
clear_poly.push_back(pt);
pt.x = x - size_x / 2;
pt.y = y + size_y / 2;
clear_poly.push_back(pt);
controller_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
}
//将costmap重置为unknow(不确定是否正确)
bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){
//clear the costmaps
planner_costmap_ros_->resetLayers();
controller_costmap_ros_->resetLayers();
return true;
}
//没啥用
bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp){
if(as_->isActive()){
ROS_ERROR("move_base must be in an inactive state to make a plan for an external user");
return false;
}
//make sure we have a costmap for our planner
if(planner_costmap_ros_ == NULL){
ROS_ERROR("move_base cannot make a plan for you because it doesn't have a costmap");
return false;
}
tf::Stamped<tf::Pose> global_pose;
if(!planner_costmap_ros_->getRobotPose(global_pose)){
ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
return false;
}
geometry_msgs::PoseStamped start;
//if the user does not specify a start pose, identified by an empty frame id, then use the robot's pose
if(req.start.header.frame_id == "")
tf::poseStampedTFToMsg(global_pose, start);
else
start = req.start;
//update the copy of the costmap the planner uses
//清空机器人周围的costmap
clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
//first try to make a plan to the exact desired goal
std::vector<geometry_msgs::PoseStamped> global_plan;
//如果可以规划成功就直接把global_plan赋值给resp发送出去,如果规划不成功,就在req.goal的req.tolerance附近找一个值来重新路径规划
//如果规划成功就退出,并把req.goal放到global_plan的最后,然后将global_plan发送出去
if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){
ROS_DEBUG_NAMED("move_base","Failed to find a plan to exact goal of (%.2f, %.2f), searching for a feasible goal within tolerance",
req.goal.pose.position.x, req.goal.pose.position.y);
//search outwards for a feasible goal within the specified tolerance
geometry_msgs::PoseStamped p;
p = req.goal;
bool found_legal = false;
float resolution = planner_costmap_ros_->getCostmap()->getResolution();
float search_increment = resolution*3.0;
if(req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance;
for(float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment) {
for(float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment) {
for(float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment) {
//don't search again inside the current outer layer
if(x_offset < max_offset-1e-9 && y_offset < max_offset-1e-9) continue;
//search to both sides of the desired goal
for(float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0) {
//if one of the offsets is 0, -1*0 is still 0 (so get rid of one of the two)
if(y_offset < 1e-9 && y_mult < -1.0 + 1e-9) continue;
for(float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0) {
if(x_offset < 1e-9 && x_mult < -1.0 + 1e-9) continue;
p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult;
p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult;
if(planner_->makePlan(start, p, global_plan)){
if(!global_plan.empty()){
//adding the (unreachable) original goal to the end of the global plan, in case the local planner can get you there
//(the reachable goal should have been added by the global planner)
global_plan.push_back(req.goal);
found_legal = true;
ROS_DEBUG_NAMED("move_base", "Found a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
break;
}
}
else{
ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
}
}
}
}
}
}
}
//copy the plan into a message to send out
//将全局路径规划得到的值
resp.plan.poses.resize(global_plan.size());
for(unsigned int i = 0; i < global_plan.size(); ++i){
resp.plan.poses[i] = global_plan[i];
}
return true;
}
//析构函数
MoveBase::~MoveBase(){
recovery_behaviors_.clear();
delete dsrv_;
if(as_ != NULL)
delete as_;
if(planner_costmap_ros_ != NULL)
delete planner_costmap_ros_;
if(controller_costmap_ros_ != NULL)
delete controller_costmap_ros_;
planner_thread_->interrupt();
planner_thread_->join();
delete planner_thread_;
delete planner_plan_;
delete latest_plan_;
delete controller_plan_;
planner_.reset();
tc_.reset();
}
//全局路径规划
bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex()));
//make sure to set the plan to be empty initially
plan.clear();
//since this gets called on handle activate
if(planner_costmap_ros_ == NULL) {
ROS_ERROR("Planner costmap ROS is NULL, unable to create global plan");
return false;
}
//get the starting pose of the robot
tf::Stamped<tf::Pose> global_pose;
if(!planner_costmap_ros_->getRobotPose(global_pose)) {
ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
return false;
}
geometry_msgs::PoseStamped start;
tf::poseStampedTFToMsg(global_pose, start);
//if the planner fails or returns a zero length plan, planning failed
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;
}
return true;
}
//发布速度为0的cmd_vel
void MoveBase::publishZeroVelocity(){
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = 0.0;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;
vel_pub_.publish(cmd_vel);
}
//验证是否是四元数
bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){
//first we need to check if the quaternion has nan's or infs
if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
ROS_ERROR("Quaternion has nans or infs... discarding as a navigation goal");
return false;
}
tf::Quaternion tf_q(q.x, q.y, q.z, q.w);
//next, we need to check if the length of the quaternion is close to zero
if(tf_q.length2() < 1e-6){
ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal");
return false;
}
//next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
tf_q.normalize();
tf::Vector3 up(0, 0, 1);
double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
if(fabs(dot - 1) > 1e-3){
ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
return false;
}
return true;
}
//将goal转化为geometry_msgs::PoseStamped格式的goal_pose_msg
geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg){
std::string global_frame = planner_costmap_ros_->getGlobalFrameID();
tf::Stamped<tf::Pose> goal_pose, global_pose;
poseStampedMsgToTF(goal_pose_msg, goal_pose);
//just get the latest available transform... for accuracy they should send
//goals in the frame of the planner
goal_pose.stamp_ = ros::Time();
try{
tf_.transformPose(global_frame, goal_pose, global_pose);
}
catch(tf::TransformException& ex){
ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
goal_pose.frame_id_.c_str(), global_frame.c_str(), ex.what());
return goal_pose_msg;
}
geometry_msgs::PoseStamped global_pose_msg;
tf::poseStampedTFToMsg(global_pose, global_pose_msg);
return global_pose_msg;
}
//唤醒条件变量planner_cond_
void MoveBase::wakePlanner(const ros::TimerEvent& event)
{
// we have slept long enough for rate
planner_cond_.notify_one();
}
//全局路径规划的线程
void MoveBase::planThread(){
ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread...");
ros::NodeHandle n;
ros::Timer timer;
bool wait_for_wake = false;
boost::unique_lock<boost::mutex> lock(planner_mutex_);
while(n.ok()){
//check if we should run the planner (the mutex is locked)
while(wait_for_wake || !runPlanner_){
//if we should not be running the planner then suspend this thread
ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");
//线程阻塞,只有等planner_cond_.notify_one()出现,线程才会继续进行
planner_cond_.wait(lock);
wait_for_wake = false;
}
ros::Time start_time = ros::Time::now();
//time to plan! get a copy of the goal and unlock the mutex
geometry_msgs::PoseStamped temp_goal = planner_goal_;
lock.unlock();
ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");
//run planner
planner_plan_->clear();
//调用makeplan函数
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());
//pointer swap the plans under mutex (the controller will pull from latest_plan_)
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();
new_global_plan_ = true;
ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");
//make sure we only start the controller if we still haven't reached the goal
if(runPlanner_)
state_ = CONTROLLING;
if(planner_frequency_ <= 0)
runPlanner_ = false;
lock.unlock();
}
//if we didn't get a plan and we are in the planning state (the robot isn't moving)
else if(state_==PLANNING){
ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
//check if we've tried to make a plan for over our time limit
lock.lock();
if(ros::Time::now() > attempt_end && runPlanner_){
//we'll move into our obstacle clearing mode
state_ = CLEARING;
publishZeroVelocity();
recovery_trigger_ = PLANNING_R;
}
lock.unlock();
}
//take the mutex for the next iteration
lock.lock();
//setup sleep interface if needed
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;
//每隔sleep_time的时间执行一次全局路径规划
timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
}
}
}
}
//执行局部路径规划和导航
//action产生的消息类型见http://wiki.ros.org/actionlib
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
{
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;
}
geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
//we have a goal so start the planner
boost::unique_lock<boost::mutex> lock(planner_mutex_);
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
current_goal_pub_.publish(goal);
std::vector<geometry_msgs::PoseStamped> global_plan;
ros::Rate r(controller_frequency_);
if(shutdown_costmaps_){
ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");
planner_costmap_ros_->start();
controller_costmap_ros_->start();
}
//we want to make sure that we reset the last time we had a valid plan and control
last_valid_control_ = ros::Time::now();
last_valid_plan_ = ros::Time::now();
last_oscillation_reset_ = ros::Time::now();
ros::NodeHandle n;
//如果控制频率改变了,就改变循环频率
while(n.ok())
{
if(c_freq_change_)
{
ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
r = ros::Rate(controller_frequency_);
c_freq_change_ = false;
}
//判断goal是否被抢先,可能被新的goal超前,也可能被cancel超前(说的不恰当,详见wiki)
if(as_->isPreemptRequested()){
//判断是否设置可以接受新的goal
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;
//we have a new goal so make sure the planner is awake
//新的goal将旧的goal抢先后,开始重新规划路径
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
last_valid_control_ = ros::Time::now();
last_valid_plan_ = ros::Time::now();
last_oscillation_reset_ = ros::Time::now();
}
//取消也算超前……
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;
}
}
//we also want to check if we've changed global frames because we need to transform our goal pose
//判断tf是否有变化
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
last_valid_control_ = ros::Time::now();
last_valid_plan_ = ros::Time::now();
last_oscillation_reset_ = ros::Time::now();
}
//for timing that gives real time even in simulation
ros::WallTime start = ros::WallTime::now();
//the real work on pursuing a goal is done here
//局部路径规划的核心
bool done = executeCycle(goal, global_plan);
//if we're done, then we'll return from execute
//判断是否已经到达goal,如果已经到达就返回
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());
}
//wake up the planner thread so that it can exit cleanly
//如果n.ok() == false,说明node已经被终结,那么应该执行下面的语句,使得条件变量planner_cond_被唤醒、被阻塞的进程继续向下执行
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;
}
//计算p1和p2两个点之间的距离
double MoveBase::distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2)
{
return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y);
}
//局部路径规划的核心函数
bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
//we need to be able to publish velocity commands
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);
//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;
}
//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();
return false;
}
//if we have a new plan then grab it and give it to the controller
//当调用了makeplan并成功返回后,会在planThread中将new_global_plan_置为true
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::mutex> lock(planner_mutex_);
//将全局的路径赋值给controller_plan_
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;
}
//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:
{
boost::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;
//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::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();
state_ = PLANNING;
publishZeroVelocity();
//enable the planner thread in case it isn't running on a clock
boost::unique_lock<boost::mutex> lock(planner_mutex_);
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
}
}
}
break;
//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");
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::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;
default:
ROS_ERROR("This case should never be reached, something is wrong, aborting");
resetState();
//disable the planner thread
boost::unique_lock<boost::mutex> lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
return true;
}
//we aren't done yet
return false;
}
//加载Recovery的行为
bool MoveBase::loadRecoveryBehaviors(ros::NodeHandle node){
XmlRpc::XmlRpcValue behavior_list;
if(node.getParam("recovery_behaviors", behavior_list)){
if(behavior_list.getType() == XmlRpc::XmlRpcValue::TypeArray){
for(int i = 0; i < behavior_list.size(); ++i){
if(behavior_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct){
if(behavior_list[i].hasMember("name") && behavior_list[i].hasMember("type")){
//check for recovery behaviors with the same name
for(int j = i + 1; j < behavior_list.size(); j++){
if(behavior_list[j].getType() == XmlRpc::XmlRpcValue::TypeStruct){
if(behavior_list[j].hasMember("name") && behavior_list[j].hasMember("type")){
std::string name_i = behavior_list[i]["name"];
std::string name_j = behavior_list[j]["name"];
if(name_i == name_j){
ROS_ERROR("A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.",
name_i.c_str());
return false;
}
}
}
}
}
else{
ROS_ERROR("Recovery behaviors must have a name and a type and this does not. Using the default recovery behaviors instead.");
return false;
}
}
else{
ROS_ERROR("Recovery behaviors must be specified as maps, but they are XmlRpcType %d. We'll use the default recovery behaviors instead.",
behavior_list[i].getType());
return false;
}
}
//if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors
for(int i = 0; i < behavior_list.size(); ++i){
try{
//check if a non fully qualified name has potentially been passed in
if(!recovery_loader_.isClassAvailable(behavior_list[i]["type"])){
std::vector<std::string> classes = recovery_loader_.getDeclaredClasses();
//感觉这行代码有bug,两个嵌套for循环都是i变量
for(unsigned int i = 0; i < classes.size(); ++i){
if(behavior_list[i]["type"] == recovery_loader_.getName(classes[i])){
//if we've found a match... we'll get the fully qualified name and break out of the loop
ROS_WARN("Recovery behavior specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
std::string(behavior_list[i]["type"]).c_str(), classes[i].c_str());
behavior_list[i]["type"] = classes[i];
break;
}
}
}
//加载各个behavior
boost::shared_ptr<nav_core::RecoveryBehavior> behavior(recovery_loader_.createInstance(behavior_list[i]["type"]));
//shouldn't be possible, but it won't hurt to check
if(behavior.get() == NULL){
ROS_ERROR("The ClassLoader returned a null pointer without throwing an exception. This should not happen");
return false;
}
//initialize the recovery behavior with its name
behavior->initialize(behavior_list[i]["name"], &tf_, planner_costmap_ros_, controller_costmap_ros_);
recovery_behaviors_.push_back(behavior);
}
catch(pluginlib::PluginlibException& ex){
ROS_ERROR("Failed to load a plugin. Using default recovery behaviors. Error: %s", ex.what());
return false;
}
}
}
else{
ROS_ERROR("The recovery behavior specification must be a list, but is of XmlRpcType %d. We'll use the default recovery behaviors instead.",
behavior_list.getType());
return false;
}
}
else{
//if no recovery_behaviors are specified, we'll just load the defaults
return false;
}
//if we've made it here... we've constructed a recovery behavior list successfully
return true;
}
//we'll load our default recovery behaviors here
//加载默认的Behaviors
void MoveBase::loadDefaultRecoveryBehaviors(){
recovery_behaviors_.clear();
try{
//we need to set some parameters based on what's been passed in to us to maintain backwards compatibility
ros::NodeHandle n("~");
n.setParam("conservative_reset/reset_distance", conservative_reset_dist_);
n.setParam("aggressive_reset/reset_distance", circumscribed_radius_ * 4);
//first, we'll load a recovery behavior to clear the costmap
boost::shared_ptr<nav_core::RecoveryBehavior> cons_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
recovery_behaviors_.push_back(cons_clear);
//next, we'll load a recovery behavior to rotate in place
boost::shared_ptr<nav_core::RecoveryBehavior> rotate(recovery_loader_.createInstance("rotate_recovery/RotateRecovery"));
if(clearing_rotation_allowed_){
rotate->initialize("rotate_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_);
recovery_behaviors_.push_back(rotate);
}
//next, we'll load a recovery behavior that will do an aggressive reset of the costmap
boost::shared_ptr<nav_core::RecoveryBehavior> ags_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
ags_clear->initialize("aggressive_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
recovery_behaviors_.push_back(ags_clear);
//we'll rotate in-place one more time
if(clearing_rotation_allowed_)
recovery_behaviors_.push_back(rotate);
}
catch(pluginlib::PluginlibException& ex){
ROS_FATAL("Failed to load a plugin. This should not happen on default recovery behaviors. Error: %s", ex.what());
}
return;
}
void MoveBase::resetState(){
// Disable the planner thread
boost::unique_lock<boost::mutex> lock(planner_mutex_);
runPlanner_ = false;
lock.unlock();
// Reset statemachine
state_ = PLANNING;
recovery_index_ = 0;
recovery_trigger_ = PLANNING_R;
publishZeroVelocity();
//if we shutdown our costmaps when we're deactivated... we'll do that now
if(shutdown_costmaps_){
ROS_DEBUG_NAMED("move_base","Stopping costmaps");
planner_costmap_ros_->stop();
controller_costmap_ros_->stop();
}
}
};
move_base
最新推荐文章于 2024-03-25 21:45:39 发布