移動機器人導航技術筆記（2）-global_planner學習（2）

這個模塊負責使用計算好的potential值生成一條全局規劃路徑。演算法很好理解，首先將goal所在的點的(x,y)塞到path，然後搜索當前的點的四周的四個臨近點，選取這四個臨近點的potential的值最小的點min，然後把這點塞到path，然後再將當前點更新為min這點，由於start 點的potential的值是0，所以這是個梯度下降的方法。

> /*********************************************************************> *> * Software License Agreement (BSD License)> *> * Copyright (c) 2008, 2013, 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 Willow Garage, Inc. 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> * David V. Lu!!> *********************************************************************/#include <global_planner/planner_core.h>#include <pluginlib/class_list_macros.h>#include <tf/transform_listener.h>#include <costmap_2d/cost_values.h>#include <costmap_2d/costmap_2d.h>#include <global_planner/dijkstra.h>#include <global_planner/astar.h>#include <global_planner/grid_path.h>#include <global_planner/gradient_path.h>#include <global_planner/quadratic_calculator.h>//register this planner as a BaseGlobalPlanner pluginPLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)

namespace global_planner {

void GlobalPlanner::outlineMap(unsigned char* costarr, int nx, int ny, unsigned char value) { unsigned char* pc = costarr; for (int i = 0; i < nx; i++) *pc++ = value; pc = costarr + (ny - 1) * nx; for (int i = 0; i < nx; i++) *pc++ = value; pc = costarr; for (int i = 0; i < ny; i++, pc += nx) *pc = value; pc = costarr + nx - 1; for (int i = 0; i < ny; i++, pc += nx) *pc = value;}

構造函數:

GlobalPlanner::GlobalPlanner() : costmap_(NULL), initialized_(false), allow_unknown_(true) {}GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) : costmap_(NULL), initialized_(false), allow_unknown_(true) { //initialize the planner initialize(name, costmap, frame_id);}

析構函數：

GlobalPlanner::~GlobalPlanner() { if (p_calc_) delete p_calc_; if (planner_) delete planner_; if (path_maker_) delete path_maker_; if (dsrv_) delete dsrv_;}

介面1——initialize()：

void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) { initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());}//核心函數——演算法實例化，通過參數對global plan中各個演算法的實例進行選取void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) { if (!initialized_) { ros::NodeHandle private_nh("~/" + name); costmap_ = costmap; frame_id_ = frame_id; unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY(); private_nh.param("old_navfn_behavior", old_navfn_behavior_, false); if(!old_navfn_behavior_) convert_offset_ = 0.5; else convert_offset_ = 0.0; ///**1、use_quadratic********************************************* bool use_quadratic; private_nh.param("use_quadratic", use_quadratic, true); if (use_quadratic) p_calc_ = new QuadraticCalculator(cx, cy); else p_calc_ = new PotentialCalculator(cx, cy); //p_calc_實例：計算「一個點」的可行性 ///*********************************************** ///**2、use_dijkstra********************************************* bool use_dijkstra; private_nh.param("use_dijkstra", use_dijkstra, true); if (use_dijkstra)///*********use_dijkstra { DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy); if(!old_navfn_behavior_) de->setPreciseStart(true); planner_ = de; //=DijkstraExpansion(p_calc_, cx, cy); } else planner_ = new AStarExpansion(p_calc_, cx, cy); //planner_實例：計算「所有」的可行點potential array ///*********************************************** ///**3、use_grid_path********************************************* bool use_grid_path; private_nh.param("use_grid_path", use_grid_path, false); if (use_grid_path)///*********use_grid_path path_maker_ = new GridPath(p_calc_); else path_maker_ = new GradientPath(p_calc_); //path_maker_實例：從可行點中「提取路徑」plan ///*********************************************** orientation_filter_ = new OrientationFilter(); //定義兩個發布器 plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1); potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1); private_nh.param("allow_unknown", allow_unknown_, true); planner_->setHasUnknown(allow_unknown_); private_nh.param("planner_window_x", planner_window_x_, 0.0); private_nh.param("planner_window_y", planner_window_y_, 0.0); private_nh.param("default_tolerance", default_tolerance_, 0.0); private_nh.param("publish_scale", publish_scale_, 100); double costmap_pub_freq; private_nh.param("planner_costmap_publish_frequency", costmap_pub_freq, 0.0); //get the tf prefix ros::NodeHandle prefix_nh; tf_prefix_ = tf::getPrefixParam(prefix_nh); //發布一個make_plan的service，make_plan_srv_ make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this); //動態調參 dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name)); dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb = boost::bind( &GlobalPlanner::reconfigureCB, this, _1, _2); dsrv_->setCallback(cb); initialized_ = true; } else ROS_WARN("This planner has already been initialized, you cant call it twice, doing nothing");}

initialize()函數調用，動態調參:

void GlobalPlanner::reconfigureCB(global_planner::GlobalPlannerConfig& config, uint32_t level) { planner_->setLethalCost(config.lethal_cost); path_maker_->setLethalCost(config.lethal_cost); planner_->setNeutralCost(config.neutral_cost); planner_->setFactor(config.cost_factor); publish_potential_ = config.publish_potential; orientation_filter_->setMode(config.orientation_mode);}

將某個costmap柵格設置為free:

void GlobalPlanner::clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my) { if (!initialized_) { ROS_ERROR( "This planner has not been initialized yet, but it is being used, please call initialize() before use"); return; } //set the associated costs in the cost map to be free costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);}bool GlobalPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp) { makePlan(req.start, req.goal, resp.plan.poses); resp.plan.header.stamp = ros::Time::now(); resp.plan.header.frame_id = frame_id_; return true;}

將map上的坐標（mx,my）轉化成world上的坐標（wx，wy）:

void GlobalPlanner::mapToWorld(double mx, double my, double& wx, double& wy) { wx = costmap_->getOriginX() + (mx+convert_offset_) * costmap_->getResolution(); wy = costmap_->getOriginY() + (my+convert_offset_) * costmap_->getResolution();}

將world上的坐標（wx，wy）轉化成map上的坐標（mx，my）:

bool GlobalPlanner::worldToMap(double wx, double wy, double& mx, double& my) { double origin_x = costmap_->getOriginX(), origin_y = costmap_->getOriginY(); double resolution = costmap_->getResolution(); if (wx < origin_x || wy < origin_y) return false; mx = (wx - origin_x) / resolution - convert_offset_; my = (wy - origin_y) / resolution - convert_offset_; if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY()) return true; return false;}

介面2：makePlan():

這裡調用了兩個核心程序：計算potential array——planner_->calculatePotentials()提取plan——getPlanFromPotential()

涉及到四種演算法：A*, Dijkstra；gradient_path, grid_path

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) { return makePlan(start, goal, default_tolerance_, plan); //調用下面的makePlan()函數}//核心函數——生成全局路徑 bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) { boost::mutex::scoped_lock lock(mutex_); if (!initialized_) { ROS_ERROR( "This planner has not been initialized yet, but it is being used, please call initialize() before use"); return false; } //clear the plan, just in case 清除plan plan.clear(); ros::NodeHandle n; std::string global_frame = frame_id_; //until tf can handle transforming things that are way in the past... well require the goal to be in our global frame if (tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) { ROS_ERROR( "The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str()); return false; } if (tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) { ROS_ERROR( "The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str()); return false; } ///********************************************************* double wx = start.pose.position.x; double wy = start.pose.position.y; unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i; double start_x, start_y, goal_x, goal_y; if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {//將起始點坐標轉換到地圖坐標 ROS_WARN( "The robots start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?"); return false; } if(old_navfn_behavior_){ start_x = start_x_i; start_y = start_y_i; }else{ worldToMap(wx, wy, start_x, start_y); } wx = goal.pose.position.x; wy = goal.pose.position.y; if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {//將目標點轉換到地圖坐標 ROS_WARN_THROTTLE(1.0, "The goal sent to the global planner is off the global costmap. Planning will always fail to this goal."); return false; } if(old_navfn_behavior_){ goal_x = goal_x_i; goal_y = goal_y_i; }else{ worldToMap(wx, wy, goal_x, goal_y); } ///********************************************************* //clear the starting cell within the costmap because we know it cant be an obstacle tf::Stamped<tf::Pose> start_pose; tf::poseStampedMsgToTF(start, start_pose); clearRobotCell(start_pose, start_x_i, start_y_i);//設置起點為FREE_SPACE int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY(); //分配空間，大小和costmap一樣大 //make sure to resize the underlying array that Navfn uses p_calc_->setSize(nx, ny); planner_->setSize(nx, ny); path_maker_->setSize(nx, ny); potential_array_ = new float[nx * ny]; ///potential_array_二維數組，存儲potential可行點*************************************** //將costmap的四周(邊界)變為LETHAL_OBSTACLE outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE); ///************************************************************************ //makePlan()的主要步驟1： //計算potential_array，尋找所有的可行點。調用的是：Expander::virtual bool calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,int cycles, float* potential) = 0; //calculatePotentials的方法有兩種（A*、Dijkstra）,由use_dijkstra參數決定：class AStarExpansion : public Expander 、class DijkstraExpansion : public Expander bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y, nx * ny * 2, potential_array_); //輸出potential_array_數組 ///************************************************************************ if(!old_navfn_behavior_) planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2); if(publish_potential_) publishPotential(potential_array_); if (found_legal) { //判斷calculatePotentials()函數是否成功找到可行點矩陣potential_array_ ///************************************************************************ //makePlan()的主要步驟2： //提取plan，調用path_maker_->getPath()實例，從所有的可行點中獲取路徑plan。調用的是：Traceback::virtual bool getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) = 0; //getPath的方法有兩種（GradientPath、GridPath），由use_grid_path參數決定：class GradientPath : public Traceback、class GridPath : public Traceback //extract the plan if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) { //調用path_maker_ -> getPath()，路徑存儲在plan中 ///************************************************************************ //make sure the goal we push on has the same timestamp as the rest of the plan geometry_msgs::PoseStamped goal_copy = goal; goal_copy.header.stamp = ros::Time::now(); plan.push_back(goal_copy); } else { ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldnt happen."); } }else{ ROS_ERROR("Failed to get a plan."); } // add orientations if needed，增加方向信息 orientation_filter_->processPath(start, plan); //publish the plan for visualization purposes publishPlan(plan);//發布可視化路徑 delete potential_array_; return !plan.empty();}

發布可視化路徑:

void GlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) { if (!initialized_) { ROS_ERROR( "This planner has not been initialized yet, but it is being used, please call initialize() before use"); return; } //create a message for the plan nav_msgs::Path gui_path; gui_path.poses.resize(path.size()); if (!path.empty()) { gui_path.header.frame_id = path[0].header.frame_id; gui_path.header.stamp = path[0].header.stamp; } // Extract the plan in world co-ordinates, we assume the path is all in the same frame for (unsigned int i = 0; i < path.size(); i++) { gui_path.poses[i] = path[i]; } plan_pub_.publish(gui_path);}

從Potential中提取路徑plan，調用path_maker_->getPath()實例:

bool GlobalPlanner::getPlanFromPotential(double start_x, double start_y, double goal_x, double goal_y, const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) { if (!initialized_) { ROS_ERROR( "This planner has not been initialized yet, but it is being used, please call initialize() before use"); return false; } std::string global_frame = frame_id_; //clear the plan, just in case plan.clear(); std::vector<std::pair<float, float> > path; ///**************************************************************************** //use_grid_path 調用path_maker_->getPath()實例，提取路徑 if (!path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)) { ///**************************************************************************** ROS_ERROR("NO PATH!"); return false; } ros::Time plan_time = ros::Time::now(); for (int i = path.size() -1; i>=0; i--) { std::pair<float, float> point = path[i]; //convert the plan to world coordinates double world_x, world_y; mapToWorld(point.first, point.second, world_x, world_y); geometry_msgs::PoseStamped pose; pose.header.stamp = plan_time; pose.header.frame_id = global_frame; pose.pose.position.x = world_x; pose.pose.position.y = world_y; pose.pose.position.z = 0.0; pose.pose.orientation.x = 0.0; pose.pose.orientation.y = 0.0; pose.pose.orientation.z = 0.0; pose.pose.orientation.w = 1.0; plan.push_back(pose); } if(old_navfn_behavior_){ plan.push_back(goal); } return !plan.empty();}void GlobalPlanner::publishPotential(float* potential){ int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY(); double resolution = costmap_->getResolution(); nav_msgs::OccupancyGrid grid; // Publish Whole Grid grid.header.frame_id = frame_id_; grid.header.stamp = ros::Time::now(); grid.info.resolution = resolution; grid.info.width = nx; grid.info.height = ny; double wx, wy; costmap_->mapToWorld(0, 0, wx, wy); grid.info.origin.position.x = wx - resolution / 2; grid.info.origin.position.y = wy - resolution / 2; grid.info.origin.position.z = 0.0; grid.info.origin.orientation.w = 1.0; grid.data.resize(nx * ny); float max = 0.0; for (unsigned int i = 0; i < grid.data.size(); i++) { float potential = potential_array_[i]; if (potential < POT_HIGH) { if (potential > max) { max = potential; } } } for (unsigned int i = 0; i < grid.data.size(); i++) { if (potential_array_[i] >= POT_HIGH) { grid.data[i] = -1; } else grid.data[i] = potential_array_[i] * publish_scale_ / max; } potential_pub_.publish(grid);}

} //end namespace global_planner

推薦閱讀：

相關文章