具体需要看看一下localplanner如何循环控制底座移动。
Globalplanner会使用dijkstra算法找到一条最短路径plan叫做globalplan,因为planthread一直在运行,所以这个plan一直在更新,所以只要globalcostmap一直在变化,那么这个globalplan也会一直变化。有了globalplan,就可以把他设置给localplanner.
然后localplanner把globalplan转换成基于localplanner坐标系的坐标点,将一些超出了localcostmap范围的坐标点去掉。将转换后的Plan数据设置给localplanner.然后让Localplanner算出多条可选轨迹,通过criticfunction给每个轨迹打分,找到最优的轨迹,然后计算出速度控制命令。在localplanner里面,给各个方向的轨迹打分时,会考虑localcostmap里面的障碍物数据,从而将和障碍物冲突的轨迹抛弃。还会考虑到local的目标点,和localplan.
障碍物的规避主要有两个阶段:
1.global planner使用global costmap数据会规避已知的障碍物,所以globalplan里面已经将该障碍物规避了。
2. local planner为达到local目标点,会使用localcostmap里面新遇到的障碍物数据,使用criticfunction打分机制找出一条最佳的local轨迹规划,并产生速度控制命令。
movebase里下面的函数循环来计算速度并发送给底座。
MoveBase::executeCycle函数。
→boolDWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist&cmd_vel) {
实际使用的是DWAPlanner。
详细分析DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist&cmd_vel) {
1. 得到机器人当前位置
2.调用LocalPlannerUtil::getLocalPlan函数先调用base_local_planner::transformGlobalPlan函数,遍历globalplan中的点,只保留到机器人距离在localcostmap范围之内的点。再将这些plan中的坐标点转换成基于localplanner使用的frame坐标系的数据。由于localcostmap是4米宽,所以半径是2米,所以将大于2米的坐标点会被去掉。从而确保我们得到一个局部的plan数据,减少后期的计算量。
3.调用DWAPlanner::updatePlanAndLocalCosts函数,将当前机器人位置,和上面过滤转换后的plan数据设置给loaclaplanner. planner里面的path_costs,goal_costs是criticcost function,他们由此知道了更新后的Plan数据和目标坐标,注意这个更新后的plan的最有一个点就是localplanner的目标。这些criticfunction在后面的路径规划中用来给路径打分。
4.然后调用LatchedStopRotateController::isPositionReached判断是否已经到达了目标地,如果到达,下面就可以旋转到目标方向,如果没有到,继续下面。
4. 如果没有到达目的地,调用boolDWAPlannerROS::dwaComputeVelocityCommands函数计算最终要使用的速度。首先调用base_local_planner::TrajectoryDWAPlanner::findBestPath