ros Navigation DWA学习

参考博客
ROS 源码阅读——局部路径规划之DWAPlannerROS分析

dwa_planner_ros.cpp中的computeVelocityCommands()函数是计算最后的速度指令的。在函数中，首先得到机器人位置，然后得到全局路径映射后的局部路径，然后由dp_.updatePlanAndLocalCosts()更新打分，然后判断是否到达目标附近，是的话调用停止的函数发布速度，否则调用dwaComputeVelocityCommands计算速度。

dp_.updatePlanAndLocalCosts()函数更新打分应该是更新局部路径产生的路径点那些内容。

在dwaComouteVelocityCommands函数中，主要的是findBestPath()函数。

findBestPath()函数在dwa_planner.cpp文件中定义。
首先利用generator_.initialise()产生轨迹。该函数为base_local_planner功能包中的simpleTrajectorygenerator.cpp中的类。

generator_.initialise(pos,vel,goal,&limits,vsamples_);result_traj_.cost_ = -7;

然后就是findBestTrajectory()函数

std::vector<base_local_planner::Trajectory> all_explored;
scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);

该函数在base_local_planner功能包中的simpleScoredSamplingPlanner.cpp中定义.。
首先进行打分项的prepare。打分项那部分，每个都是critics,这个在dwa_planner.cpp中构造函数中会把不同的costfunction压入。分别是

 // see constructor body for explanationsbase_local_planner::SimpleTrajectoryGenerator generator_;base_local_planner::OscillationCostFunction oscillation_costs_;base_local_planner::ObstacleCostFunction obstacle_costs_;base_local_planner::MapGridCostFunction path_costs_;base_local_planner::MapGridCostFunction goal_costs_;base_local_planner::MapGridCostFunction goal_front_costs_;base_local_planner::MapGridCostFunction alignment_costs_;base_local_planner::TwirlingCostFunction twirling_costs_;

打分项prepare后，就是simple_scored_sampling_planner中的scoreTrajectory函数，而在其中，cost计算如下

double cost = score_function_p->scoreTrajectory(traj);

这一部分在base_local_planner功能包中的Trajectory_cost_function.h中声明，是一个虚函数，其定义根据之前score_function_p的不同，即critics中类型的不同，分别到不同的costfunction中。

以obstacle_cost_function为例。
其cost为f_cost和cost的最大值，f_cost计算如下：

double f_cost = footprintCost(px, py, pth,scale, footprint_spec_,costmap_, world_model_);

footprintCost在obstacle_cost_function.cpp中。
其计算如下：

 double footprint_cost = world_model->footprintCost(x, y, th, footprint_spec);

world_model.h文件中footprintCost函数也是虚函数，因为用的是costmap,所以为costmap_model.cpp的footprintCost函数。

在其中会返回足迹点之间线段上所有的点中的最大代价，或者求和。
点的代价参考博客 costmap_2D包的理解

应该是就不包含距离障碍的代价，就是碰撞或不碰撞。

ros Navigation DWA学习相关推荐

【硬核】 ROS Navigation 局部路径规划常见算法
简介最近,作者参加了关于RMUS 高校 SimReal挑战赛,首次接触到了机器人导航领域,这里记录一下这段时间的收货.sim2real的全称是simulation to reality,是强化学 ...
ROS运动规划学习六---dwa_local_planner
文章目录前言一.dwa_local_planner结构二.setPlan.initialize.isGoalReached 三.computeVelocityCommands() 总结前言在 ...
ROS Navigation插件注册自定义导航避障算法
前言最近开组会的时候,导师催促我寻找创新点,着实让我头疼.因为说实话,我真的不想找什么创新点,我只想学习一些招聘简历上的技能类的东西,比如熟悉A*.Dijkstra和DWA导航避障算法,熟悉ROS, ...
理解ROS Navigation Stack，看完这篇你就知道啦！
前言 ROS Navigation Stack是ROS提供的一个二维的导航功能包集合,通过输入里程计.传感器信息和目标位姿,输出控制机器人到达目标状态的安全速度指令.ROS Navigation St ...
ROS Navigation Stack安装
ROS Navigation Stack安装 Navigation导航包是做导航几乎都要用的,大家可以先去ROS Wiki上学习下我们先Git下对应版本的软件包,我是Kinetic的,所以是Kine ...
ROS Navigation Tuning Guide（导航调试指南）
ROS Navigation Tuning Guide 导航调试指南准备工作距离传感器里程计定位速度与加速度的设置获得最大速度获得最大加速度设置最小值 XY方向的速度 Global P ...
ROS运动规划学习三---move_base
文章目录前言一.move_base工程结构二.move_base简要分析 1.MoveBase类的声明---move_base.h 2.MoveBase类的实现---move_base.cpp ...
ROS与Arduino学习（三）订阅与发布
ROS与Arduino学习(三)订阅与发布 Tutorial Level:HelloWorld Next Tutorial:用ROS的Cmake编译程序本节介绍如何在arduino中发布与订阅消息. ...
ROS与Arduino学习（六）Logging日志
ROS与Arduino学习(六)Logging日志 Tutorial Level:客户端与服务器 Next Tutorial:小案例节点通信本节较为简单告诉大家如何向系统发布日志信息. T ...

ros Navigation DWA学习

ros Navigation DWA学习相关推荐

最新文章

热门文章