参考博客:
贪心算法的一个典型案例——Dijkstra算法: 浅谈路径规划算法之Dijkstra算法
A*: A*寻路算法
重要:ROS Navigation的global_planner类继承关系与实现算法
导航实际流程为:
进行全局路径规划,在进行局部路径规划,然后发布速度
全局路径规划在makePlan函数中,该函数中调用了 planner_的 makePlan和 empty接口。planner_为继承于BaseGlobalPlanner的实例,由pluginlib通过具体类的名字进行装载。
之后,调用tc_的setPlan接口,对局部路径规划器进行全局路径设置,然后,调用tc_的isReached接口进行判断,然后调用tc_的computeVelocityCommands接口,进行速度计算,然后进行速度下发。
tc_为继承于BaseLocalPlanner的实例,也是由pluginlinb通过具体类的名字进行装载。
下面带来两个问题,planner_怎么进行路径规划,以及tc_如何计算速度。planner_在初始化时候,被塞入了planner_costmap_ros_
tc_在初始化时,被塞入了controller_costmap_ros_
在global planner的包中,注册了插件:global planner::GlobalPlanner
代码阅读:
global_planne
1、plan_node.cpp
plan_node.cpp是全局规划代码的入口(代码注释都是自己理解然后添加,也许会有错误)
#include <global_planner/planner_core.h> #include <navfn/MakeNavPlan.h> #include <boost/shared_ptr.hpp> #include <costmap_2d/costmap_2d_ros.h> namespace cm = costmap_2d; namespace rm = geometry_msgs; using std::vector; using rm::PoseStamped; using std::string; using cm::Costmap2D; using cm::Costmap2DROS; namespace global_planner { class PlannerWithCostmap : public GlobalPlanner { public: PlannerWithCostmap(string name, Costmap2DROS* cmap); bool makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp); private: void poseCallback(const rm::PoseStamped::ConstPtr& goal); Costmap2DROS* cmap_; ros::ServiceServer make_plan_service_; ros::Subscriber pose_sub_; }; //Publish a path for visualization purposes bool PlannerWithCostmap::makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp) { vector<PoseStamped> path; req.start.header.frame_id = "/map"; req.goal.header.frame_id = "/map"; bool success = makePlan(req.start, req.goal, path); resp.plan_found = success; if (success) { resp.path = path; } return true; } void PlannerWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) { tf::Stamped<tf::Pose> global_pose; cmap_->getRobotPose(global_pose);//获取机器人起始位姿 vector<PoseStamped> path; rm::PoseStamped start; start.header.stamp = global_pose.stamp_; start.header.frame_id = global_pose.frame_id_; start.pose.position.x = global_pose.getOrigin().x(); start.pose.position.y = global_pose.getOrigin().y(); start.pose.position.z = global_pose.getOrigin().z(); start.pose.orientation.x = global_pose.getRotation().x(); start.pose.orientation.y = global_pose.getRotation().y(); start.pose.orientation.z = global_pose.getRotation().z(); start.pose.orientation.w = global_pose.getRotation().w(); makePlan(start, *goal, path);//路径规划 } PlannerWithCostmap::PlannerWithCostmap(string name, Costmap2DROS* cmap) : GlobalPlanner(name, cmap->getCostmap(), cmap->getGlobalFrameID()) { ros::NodeHandle private_nh("~"); cmap_ = cmap; make_plan_service_ = private_nh.advertiseService("make_plan", &PlannerWithCostmap::makePlanService, this); pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &PlannerWithCostmap::poseCallback, this); } } // namespace int main(int argc, char** argv) { ros::init(argc, argv, "global_planner"); //设置tf监听时间间隔 tf::TransformListener tf(ros::Duration(10)); //costmap_2d::Costmap2D 类是存储和访问2D代价地图的的基本数据结构,下面代码作用是初始化 costmap_2d::Costmap2DROS lcr("costmap", tf); //两个线程:1、提供planservice 2、订阅goal,当得到goal则启动makeplan global_planner::PlannerWithCostmap pppp("planner", &lcr); ros::spin(); return 0; }
接下来分析makeplan函数
2、makeplan
GlobalPlanner::makePlan类的使用接口有多种,例如:
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); } bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan){.....}
但最终程序的主体是:
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.clear(); ros::NodeHandle n; std::string global_frame = frame_id_; //until tf can handle transforming things that are way in the past... we'll 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;//map double start_x, start_y, goal_x, goal_y; //下面将世界坐标系下的start和goal转化为map形式 if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) { ROS_WARN( "The robot's 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 can't be an obstacle tf::Stamped<tf::Pose> start_pose; tf::poseStampedMsgToTF(start, start_pose);//map下信息转化为tf类的数据 clearRobotCell(start_pose, start_x_i, start_y_i);//清除开始点,因为开始位置不可能是障碍 //计算costmap的xsize和ysize,赋值给nx ,ny int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY(); //make sure to resize the underlying array that Navfn uses,(分配空间,大小和costmap一样大) p_calc_->setSize(nx, ny); planner_->setSize(nx, ny); path_maker_->setSize(nx, ny); potential_array_ = new float[nx * ny]; //调用以下函数将costmap的四个边的全部cell都设置为LETHAL_OBSTACLE(占用) outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE); //计算potential bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y, nx * ny * 2, 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) { //extract the plan if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, 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 shouldn't 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(); }
值得注意的是,在GlobalPlanner::initialize()这个初始化函数中有一段代码,决定了使用A*还是D*亦或是其他算法计算:
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); bool use_dijkstra; private_nh.param("use_dijkstra", use_dijkstra, true); if (use_dijkstra) { DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy); if(!old_navfn_behavior_) de->setPreciseStart(true); planner_ = de; } else planner_ = new AStarExpansion(p_calc_, cx, cy);//决定使用的算法 bool use_grid_path; private_nh.param("use_grid_path", use_grid_path, false); if (use_grid_path) path_maker_ = new GridPath(p_calc_); else path_maker_ = new GradientPath(p_calc_);
从makeplan代码中分析,最关键的语句有两句:
1、计算potential
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
nx * ny * 2, potential_array_);
这里的planner_的定义由GlobalPlanner::initialize()中的参数决定(程序见上)
2、提取plan
if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, 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); }
astar.cpp文件
计算potential时,假设 参数文件中 use_dijkstra = fause ,那么使用的就是astar算法,即
planner_ = new AStarExpansion(p_calc_, cx, cy)
因此首先需要分析astar.cpp内的函数:这篇博客这部分写的不错点击打开链接
代码中用了堆,这两篇博客对堆讲的比较详细:点击打开链接 堆相关算法详解与C++编程实现
输入参数 为指向概率地图的指针 其实位置地坐标 目标坐标 一个指向大小为nx*ny的数组
bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles, float* potential) { queue_.clear(); int start_i = toIndex(start_x, start_y); queue_.push_back(Index(start_i, 0));//push the start point into OPEN queue_ std::fill(potential, potential + ns_, POT_HIGH); //initial all the potential as very large value 1e10 potential[start_i] = 0;//set start_i为0 int goal_i = toIndex(end_x, end_y); int cycle = 0; while (queue_.size() > 0 && cycle < cycles) { Index top = queue_[0];//get the Index with lowest cost (set to current) std::pop_heap(queue_.begin(), queue_.end(), greater1());//build the heap sort queue_.pop_back();//remove the Index with mini cost (remove from OPEN) int i = top.i;//target node the Index's i from (i,cost) if (i == goal_i) return true; //for each neighbour node (*) of the current node(0), // + * + i-nx // * 0 * i-1, 0 , i+1 // + * + i+nx add(costs, potential, potential[i], i + 1, end_x, end_y); add(costs, potential, potential[i], i - 1, end_x, end_y); add(costs, potential, potential[i], i + nx_, end_x, end_y); add(costs, potential, potential[i], i - nx_, end_x, end_y); cycle++; } return false; }
//接下来add函数的定义
/*f(n)=g(n)+h(n) 其中, f(n) 是从初始状态经由状态n到目标状态的代价估计,g(n) 是在状态空间中从初始状态到状态n的实际代价, h(n) 是从状态n到目标状态的最佳路径的估计代价。 (对于路径搜索问题,状态就是图中的节点,代价就是距离)*/
void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,
int end_y) {
if (next_i < 0 || next_i >= ns_)
return;
if (potential[next_i] < POT_HIGH)//it means the potential cell has been explored
return;
if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))//it means this cell is obstacle return;
return;
//计算next_i的potential值,calculatePotential函数返回next_i周围的节点到next_i的最小值
potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);
int x = next_i % nx_, y = next_i / nx_;//x mean column ,y means row
float distance = abs(end_x - x) + abs(end_y - y);//calculate h(n)
queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
std::push_heap(queue_.begin(), queue_.end(), greater1());
}
} //end namespace global_planner
3、A*算法代码总结:
1、bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles, float* potential)
输入参数:* costs 即: costmap_->getCharMap()
start_x,start_y 起始点
end_x,end_y 目标点
cycles 即:nx * ny * 2
*potential 用于存储代价,数组大小为nx * ny
首先 start_x,start_y,end_x,end_y转化为 start_i,end_i;
将start_i加入到open 中
queue_.push_back(Index(start_i, 0)); //Index包括 i 和 cost ,代码将cost清零
进入循环,循环条件:堆的size大于0 且 循环次数小于 2*nx*ny
循环中先定义 Index top = queue_[0]; 即取最小堆的根,包含序号i,代价 cost
然后通过 std::pop_heap(queue_.begin(), queue_.end(), greater1()); queue_.pop_back(); 将树根放到末端并删除
取i=top.i, 计算代价地图中要到达目标点 该点(i)的邻点 所消耗代价的最小值
接下来分析如何计算:
2、void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,
int end_y)代价计算:f(n)=g(n)+h(n)
调用形式是: add(costs, potential, potential[i], i + 1, end_x, end_y);
add(costs, potential, potential[i], i - 1, end_x, end_y);add(costs, potential, potential[i], i + nx_, end_x, end_y);
add(costs, potential, potential[i], i - nx_, end_x, end_y);
输入参数: costs地图 potential数组 当前i点的potential值 邻点的代号 目标点的行、列信息
先计算start_i到 邻点(i+1,i-1,i+nx,i-nx) 的最小代价g(n),使用函数:
potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);
然后计算 邻点(i+1,i-1,i+nx,i-nx) 到目标点的估值代价h (n),与前面的最小代价g(n)相加,并放到树根
queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));
std::push_heap(queue_.begin(), queue_.end(), greater1());
3、 calculatePotential函数
virtual float calculatePotential(float* potential, unsigned char cost, int n, float prev_potential=-1){ if(prev_potential < 0){ // get min of neighbors float min_h = std::min( potential[n - 1], potential[n + 1] ), min_v = std::min( potential[n - nx_], potential[n + nx_]); prev_potential = std::min(min_h, min_v); } return prev_potential + cost; }