数字家庭影院
Navigation包Global_planner全局路径规划源码详细解析学习总结，如有错误欢迎指正！⼀⼂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";
bool success =makePlan(req.start, al, 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)
⾸先在构造函数中需要初始化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_ 作为⼀种旧式规划⾏为：
1. The start of the path does not match the actual start location.
2. The very end of the path moves along grid lines.
3. 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);
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议