学习总结,如有错误欢迎指正!
一丶plan_node.cpp
从程序入口开始,首先在plan_node.cpp
的main
函数中,初始化了全局路径规划器。
costmap_2d::Costmap2DROS lcr("costmap", buffer);
global_planner::PlannerWithCostmap pppp("planner", &lcr);
在函数PlannerWithCostmap
中设置了两种调用makePlan
的路径:
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);
}
- 1.通过
make_plan
服务
req.start.header.frame_id = "map";
req.goal.header.frame_id = "map";
bool success = makePlan(req.start, req.goal, path);
- 2.通过
goal
回调函数
//得到当前机器人在MAP中的位置
cmap_->getRobotPose(global_pose);
makePlan(global_pose, *goal, path);
在getRobotPose
函数中,通过tf_.transform(robot_pose, global_pose, global_frame_);
函数,默认将机器人pose
从base_link
转换到map
坐标系下,可通过参数设置。得到起始点和目标点传入到makePlan
中。
二丶 planner_core.cpp
//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)
global_planner 是基类nav_core :: BaseGlobalPlanner的一个插件子类
首先在构造函数中需要初始化GlobalPlanner
,在initialize
中对一些参数进行赋值。
GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) :
GlobalPlanner() {
//initialize the planner
initialize(name, costmap, frame_id);
}
当调用makePlan
时,首先就是判断是否已经被初始化:
// code line 221~225 makePlan()
if (!initialized_) {
ROS_ERROR(
"This planner has not been initialized yet, but it is being used, please call initialize() before use");
return false;
}m
初始化完成之后,清除之前规划的Plan
,以防万一。然后检查起点和终点是否在我们所需要的坐标系下,一般在map
系下。
//clear the plan, just in case , code line 227 makePlan()
plan.clear();
if (goal.header.frame_id != global_frame) {...}
if (start.header.frame_id != global_frame){...}
将世界坐标系的点(map 坐标系)转换成图像坐标系(图像左下角)下的点(以像素表示)
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;
}
在Costmap2D
和GlobalPlanner
中都有实现worldToMap
,其实都是一样的,在GlobalPlanner
中则需要通过调用Costmap2D
来获取局部地图的起始点和分辨率,而在Costmap2D
则可以直接使用全局变量。
bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
{
if (wx < origin_x_ || wy < origin_y_)
return false;
mx = (int)((wx - origin_x_) / resolution_);
my = (int)((wy - origin_y_) / resolution_);
if (mx < size_x_ && my < size_y_)
return true;
return false;
}
old_navfn_behavior_ 作为一种旧式规划行为:
- The start of the path does not match the actual start location.
- The very end of the path moves along grid lines.
- All of the coordinates are slightly shifted by half a grid cell
现在在
worldToMap
所使用的convert_offset_ = 0
接下来将机器人Robot
所在的位置,在costmap
中设置成free
,当前位置不可能是一个障碍物。
即在clearRobotCell()
函数中:mx
,my
即当前机器人位置。
costmap_->setCost(mx, my, costmap_2d::FREE_SPACE);
设置规划地图边框:outlineMap
,此函数由参数outline_map_
决定。
根据costmap
跟起始终止点计算网格的potential
,计算的算法有两种:Dijkstra
和A*
,具体算法便不再讨论,资料很多。
当提取到plan
之后,调用getPlanFromPotential
,把path
转换变成geometry_msgs::PoseStamped
数据类型。
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);
}
此时便得到所需要的路径plan
,最终调用OrientationFilter
平滑之后发布出去。
orientation_filter_->processPath(start, plan);