admin 管理员组文章数量: 1086019
参考博客：
MoveBace.cpp阅读笔记
贪心算法的一个典型案例——Dijkstra算法: 浅谈路径规划算法之Dijkstra算法
A*: A*寻路算法
关于寻路算法的一些思考（1）——A*算法介绍
ROS的全局规划代码介绍（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.subsc版权声明：本文标题：移动机器人gazebo仿真(5)—规划算法Ａ* 内容由网友自发贡献，该文观点仅代表作者本人， 转载请联系作者并注明出处：http://roclinux.cn/b/1738259889a1952463.html， 本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容，一经查实，本站将立刻删除。
发表评论