Gerona学习笔记 path_planner

planner_node

定义了类planner，是其他规划算法的基础

函数	功能
void updateMapCallback	当使用地图时的回调函数
void updateGoalCallback	当有新目标时的回调函数 调用的是findPath函数
virtual void updateMap	当地图改变时更新地图
virtual void execute	"plan_path"服务的回调函数(path plan的入口)，会调用findPath，并判断是否规划成功 server_(nh, “plan_path”, boost::bind(&Planner::execute, this, _1), false)
path_msgs::PathSequence planImpl	传入goal，在路径规划子线程中调用,实际做规划的函数，调用plan函数，是在继承类中定义的,根据不用的运动模型实现了不同规划方法的重载
path_msgs::PathSequence optimizePathCost	对路径优化，与smoothPath和interpolatePath一起构成优化部分
path_msgs::PathSequence smoothPath	对路径光滑处理
path_msgs::PathSequence interpolatePath	当路径超过最大长度时，对其分段处理
virtual path_msgs::PathSequence plan	for pose mode
virtual path_msgs::PathSequence planWithoutTargetPose	for non pose mode

函数	功能
transformPose	将geometry_msgs::PoseStamped类型的pose转换为需要的类型，在Planner::planImpl()中调用
preprocess	是对地图进行预处理，并发布"/planer/map"与"/planer/cost"的topic，预处理主要包括是否融合pointcloud和laserscan，还有膨胀处理
postprocess	对原始路径进行后期处理，处理过程包括:使用当前代价地图进行梯度下降，优化路径optimizePathCost，再插值interpolatePath，后平滑smoothPath，再插值平滑一次得到最终路径，并返回
preempt	是server绑定的函数，如果抢占成功则输出preempting
feedback	发布是否规划成功的status
laserCallback	将scan传入给front或者back
integrateLaserScan	把scan数据融合到地图中
cloudCallback	点云的回调函数
integratePointCloud	融合点云
growObstacles	对地图进行膨胀，使用的是opencv的膨胀dilate方法
calculateGradient	计算梯度信息
publishGradient	根据梯度信息优化路径
findPath	execute调用，使用接收的地图开始规划，并发布，过程包括处理地图，真正规划是doPlan，调用为path_raw = doPlan(request)，可能需要调用postprocess，对原始路径raw path进行后期处理，并publish(path, path_raw)，最后返回path
doPlan	规划的实现，创建了子线程boost::thread worker(boost::bind(&Planner::planThreaded, this, request))，子线程中调用planImpl函数
planThreaded	被创建的路径规划子线程，调用了planImpl函数
subdividePath	递归函数，当路径超过设定的最大距离时，将其分段
simplifyPath	化简路径，判断点是否可以被移除，用的是连线法
smoothPath	调用的是smoothPathSegment
smoothPathSegment	实际的平滑路径实现

path_planner_node

定义了类 path_planner，继承了planner

plan

该函数中根据不同的模型选择具体的规划算法，在Planner::planImpl()函数中调用
和planWithoutTargetPose类似，根据algorithm的不同，来调不同的planInstance函数

planInstance

定义了模板类函数planInstance，其中Algorithm为模板，实际使用时为AStarSteeringDynamic(AStarDynamicSearch<DynamicSteeringNeighborhood,NoExpansion,Pose2d,GridMap2d,100>)
该函数中:

• 首先initSearch初始化搜索算法，配置地图和超时的时间
• 实际有用的是findPath，传入的是from_map, to_map, search_options，如果使用render_open_cells_，还需要boost::bind(&PathPlanner::renderCells, this, algo_id)，render_open_cells_应该是是否将开集可视化
• path = algo.findPath(from_map, to_map, search_options) 这句话调用的是cslibs_path_planning/include/cslibs_path_planning/generic/SearchAlgorithm.hpp中的findPath函数
  重点说SearchAlgorithm

SearchAlgorithm

namespace为lib_path，下面有三个模板类

• GenericParameter
• GenericSearchAlgorithm
• GenericDynSearchAlgorithm 是上一个类的继承

GenericParameter

定义了一些结构，包括PointT MapT等结构

GenericSearchAlgorithm

• findPath virtual PathT findPath(const PointT& from, const PointT& to, SearchOptions so = SearchOptions()) 调用的是findPathPose函数

• findPathPose函数调用的是findPathImp函数

• findPathImp函数是算法真正开始的地方

• processNeighbor函数是看是否更新cost

• backtrack从终点开始往回迭代搜寻到起点的最优路径

planMapInstance

也是定义模板函数，但与planInstance不同，只有起点没有终点，重点也是findPath函数

planWithoutTargetPose

是来实现planMapInstance的，根据algorithm的不同，来调不同的planMapInstance函数