ROS Navigation Stack之dwa_local_planner源码分析

2024-04-27 01:58

本文主要是介绍ROS Navigation Stack之dwa_local_planner源码分析,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

DWA和base_local_planner的关系

在base_local_planner包中有两个文件叫trajectory_planner.cpp 以及对应的ros实现,其和DWA是同一层的。
由于nav_core提供了统一的接口,因此我们可以先看看统一的接口有哪些,那我们便知道每一个算法里比较重要的函数有哪些。

nav_core包里的base_local_planner.h文件
//最为关键的地方,计算机器人下一刻的速度
virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
//判断是否到达目标点
virtual bool isGoalReached() = 0;
//加载全局路径
virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
//初始化
virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;

下面我们就先看看base_local_planner的computeVelocityCommands的主要实现框架

bool TrajectoryPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{//检查初始化、检查是否已经到达目标点...略transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, transformed_plan);//如果已经到达目标点,姿态还没到if (xy_tolerance_latch_ || (getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance_)) {tc_->updatePlan(transformed_plan);//所以这个函数里最关键的子函数是findBestPathTrajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);return true;}tc_->updatePlan(transformed_plan);Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);//然后又是转换,然后就发布出速度了...
}

接下来我们看一下TrajectoryPlanner的findBestPath的实现框架,Come on~

Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,tf::Stamped<tf::Pose>& drive_velocities)
{//...Trajectory best = createTrajectories(pos[0], pos[1], pos[2], vel[0], vel[1], vel[2],acc_lim_x_, acc_lim_y_, acc_lim_theta_);//...
}

顺藤摸瓜,一睹createTrajectories的内部实现,这个函数是轨迹采样算法,可以说是一个非常关键的函数。

Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta,double vx, double vy, double vtheta,double acc_x, double acc_y, double acc_theta) 
{//检查最终点是否是有效的,判断变量在updatePlan中被赋值if( final_goal_position_valid_ ){double final_goal_dist = hypot( final_goal_x_ - x, final_goal_y_ - y );max_vel_x = min( max_vel_x, final_goal_dist / sim_time_ );}//是否使用dwa算法, sim_peroid_是1/controller_frequency_,暂时不清楚sim_period_和sim_time_的区别if (dwa_){max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_);min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_);max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_);min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_);}else{max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_);min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_);max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_);min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_);}//...先忽略其中的逻辑,只要知道按照不同的规则生成路径,调用的子函数是generateTrajectory
}

这个子函数的作用就是生成路径,并且评分

void TrajectoryPlanner::generateTrajectory
{//主要有两大作用://生成路径和速度vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);//计算位置x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);//对路径进行评分if (!heading_scoring_) {//cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;} else {cost = occdist_scale_ * occ_cost + pdist_scale_ * path_dist + 0.3 * heading_diff + goal_dist * gdist_scale_;}//这里的顺序与源码不同,我觉得总分来看更有组织性//该轨迹与全局路径的相对距离path_dist = path_map_(cell_x, cell_y).target_dist;//距离目标点距离goal_dist = goal_map_(cell_x, cell_y).target_dist;//离障碍物距离double footprint_cost = footprintCost(x_i, y_i, theta_i);occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));
}

综上所述,其整一个逻辑顺序就是computeVelocityCommands->findBestTrajectory --> createTrajectories --> generateTrajectory

最终,选择分数最低的轨迹,发布出去。这便是整个局部规划器的实现思路和逻辑。下一篇,谈谈Costmap2D。

这篇关于ROS Navigation Stack之dwa_local_planner源码分析的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/939236

相关文章

Java程序进程起来了但是不打印日志的原因分析

《Java程序进程起来了但是不打印日志的原因分析》:本文主要介绍Java程序进程起来了但是不打印日志的原因分析,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录Java程序进程起来了但是不打印日志的原因1、日志配置问题2、日志文件权限问题3、日志文件路径问题4、程序

Java 正则表达式URL 匹配与源码全解析

《Java正则表达式URL匹配与源码全解析》在Web应用开发中,我们经常需要对URL进行格式验证,今天我们结合Java的Pattern和Matcher类,深入理解正则表达式在实际应用中... 目录1.正则表达式分解:2. 添加域名匹配 (2)3. 添加路径和查询参数匹配 (3) 4. 最终优化版本5.设计思

Java字符串操作技巧之语法、示例与应用场景分析

《Java字符串操作技巧之语法、示例与应用场景分析》在Java算法题和日常开发中,字符串处理是必备的核心技能,本文全面梳理Java中字符串的常用操作语法,结合代码示例、应用场景和避坑指南,可快速掌握字... 目录引言1. 基础操作1.1 创建字符串1.2 获取长度1.3 访问字符2. 字符串处理2.1 子字

Python 迭代器和生成器概念及场景分析

《Python迭代器和生成器概念及场景分析》yield是Python中实现惰性计算和协程的核心工具,结合send()、throw()、close()等方法,能够构建高效、灵活的数据流和控制流模型,这... 目录迭代器的介绍自定义迭代器省略的迭代器生产器的介绍yield的普通用法yield的高级用法yidle

C++ Sort函数使用场景分析

《C++Sort函数使用场景分析》sort函数是algorithm库下的一个函数,sort函数是不稳定的,即大小相同的元素在排序后相对顺序可能发生改变,如果某些场景需要保持相同元素间的相对顺序,可使... 目录C++ Sort函数详解一、sort函数调用的两种方式二、sort函数使用场景三、sort函数排序

Java调用C++动态库超详细步骤讲解(附源码)

《Java调用C++动态库超详细步骤讲解(附源码)》C语言因其高效和接近硬件的特性,时常会被用在性能要求较高或者需要直接操作硬件的场合,:本文主要介绍Java调用C++动态库的相关资料,文中通过代... 目录一、直接调用C++库第一步:动态库生成(vs2017+qt5.12.10)第二步:Java调用C++

kotlin中const 和val的区别及使用场景分析

《kotlin中const和val的区别及使用场景分析》在Kotlin中,const和val都是用来声明常量的,但它们的使用场景和功能有所不同,下面给大家介绍kotlin中const和val的区别,... 目录kotlin中const 和val的区别1. val:2. const:二 代码示例1 Java

Go标准库常见错误分析和解决办法

《Go标准库常见错误分析和解决办法》Go语言的标准库为开发者提供了丰富且高效的工具,涵盖了从网络编程到文件操作等各个方面,然而,标准库虽好,使用不当却可能适得其反,正所谓工欲善其事,必先利其器,本文将... 目录1. 使用了错误的time.Duration2. time.After导致的内存泄漏3. jsO

Python实现无痛修改第三方库源码的方法详解

《Python实现无痛修改第三方库源码的方法详解》很多时候,我们下载的第三方库是不会有需求不满足的情况,但也有极少的情况,第三方库没有兼顾到需求,本文将介绍几个修改源码的操作,大家可以根据需求进行选择... 目录需求不符合模拟示例 1. 修改源文件2. 继承修改3. 猴子补丁4. 追踪局部变量需求不符合很

Spring事务中@Transactional注解不生效的原因分析与解决

《Spring事务中@Transactional注解不生效的原因分析与解决》在Spring框架中,@Transactional注解是管理数据库事务的核心方式,本文将深入分析事务自调用的底层原理,解释为... 目录1. 引言2. 事务自调用问题重现2.1 示例代码2.2 问题现象3. 为什么事务自调用会失效3