目录
- 0 专栏介绍
- 1 ROS2路径平滑器介绍
- 2 平滑器插件编写模板
-
- 2.1 构造平滑器插件类
- 2.2 注册并导出插件
- 2.3 编译与使用插件
- 3 基于B样条曲线的路径平滑
0 专栏介绍
本专栏旨在通过对ROS2的系统学习,掌握ROS2底层基本分布式原理,并具有机器人建模和应用ROS2进行实际项目的开发和调试的工程能力。
🚀详情:《ROS2从入门到精通》
1 ROS2路径平滑器介绍
**路径平滑器(smoother)**是Nav2
中的一个任务服务器,它实现了nav2_behavior_tree::SmoothPath
接口,主要负责改善路径的平滑度或质量,Nav2
中默认的平滑器包括
名称 | 作者 | 介绍 |
---|---|---|
Simple Smoother |
Steve Macenski |
为不可行的2D规划器提供更平滑的简单路径 |
Constrained Smoother |
Matej Vargovcik 与Steve Macenski |
使用约束问题求解器优化各种标准(如平滑度或与障碍物的距离)的路径平滑器,保持最小转弯半径 |
Savitzky-Golay Smoother |
Steve Macenski |
使用Savitzky-Golay滤波器通过数字信号处理平滑路径,以去除路径中的噪声。 |
本文介绍路径平滑器插件的编写范式,并提供一个全新的基于B样条曲线平滑的插件
2 平滑器插件编写模板
2.1 构造平滑器插件类
所有路径平滑插件的基类是nav2_core::Smoother
,该基类提供了7个纯虚方法来实现控制器插件,一个合法的平滑器插件必须覆盖这7个基本方法:
configure()
:在平滑器服务器进入on_configure
状态时会调用此方法,此方法执行ROS2参数声明和平滑器成员变量的初始化;activate()
:在平滑器服务器进入on_activate
状态时会调用此方法,此方法实现平滑器进入活动状态前的必要操作;deactivate()
:在平滑器服务器进入on_deactivate
状态时会调用此方法,此方法实现平滑器进入非活动状态前的必要操作;cleanup()
:在平滑器服务器进入on_cleanup
状态时会调用此方法,此方法清理为平滑器创建的各种资源;smooth()
:接受一个原始路径,并提供具体的平滑实现
按照上述标准,本文案例中B样条平滑器的基本成员函数和变量如下所示
cpp
namespace nav2_smoother
{
class BSplineSmoother : public nav2_core::Smoother
{
public:
/**
* @brief A constructor for nav2_smoother::BSplineSmoother
*/
BSplineSmoother() = default;
/**
* @brief A destructor for nav2_smoother::BSplineSmoother
*/
~BSplineSmoother() override = default;
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr &,
std::string name, std::shared_ptr<tf2_ros::Buffer>,
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>,
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>) override;
void cleanup() override { costmap_sub_.reset(); }
void activate() override { RCLCPP_WARN(logger_, "Using: B-spline smoother"); }
void deactivate() override {}
/**
* @brief Method to smooth given path
*
* @param path In-out path to be smoothed
* @param max_time Maximum duration smoothing should take
* @return If smoothing was completed (true) or interrupted by time limit (false)
*/
bool smooth(
nav_msgs::msg::Path &path,
const rclcpp::Duration &max_time) override;
std::shared_ptr<trajectory_generation::BSpline> bspline_gen_;
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
rclcpp::Logger logger_{rclcpp::get_logger("BSplineSmoother")};
};
}
2.2 注册并导出插件
在创建了自定义平滑器的前提下,需要导出该平滑器插件以便平滑器服务器可以在运行时正确地加载。在ROS2中,插件的导出和加载由pluginlib
处理。
-
源文件配置导出宏
cpp#include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(nav2_smoother::BSplineSmoother, nav2_core::Smoother)
-
配置插件描述文件
xxx_smoother.xml
,例如本案例为bspline_smoother.xml
文件。此XML文件包含以下信息:library path
:插件库名称及其位置;class name
:平滑算法类的名称;class type
:平滑算法类的类型;base class
:平滑基类的名称,统一为nav2_core::Smoother
description
:插件的描述。
实例如下
xml<class_libraries> <library path="bspline_smoother"> <class type="nav2_smoother::BSplineSmoother" name="bspline_smoother/BSplineSmoother" base_class_type="nav2_core::Smoother"> <description>B-Spline smoother</description> </class> </library> </class_libraries>
-
配置
CMakeLists.txt
文件使用
cmake
函数pluginlib_export_plugin_description_file()
来导出插件。这个函数会将插件描述文件安装到install/share
目录中,并设置ament
索引以使其可被发现,实例如下cpluginlib_export_plugin_description_file(nav2_core bspline_smoother.xml)
-
配置
package.xml
描述文件,实例如下:xml<export> <build_type>ament_cmake</build_type> <nav2_core plugin="${prefix}/bspline_smoother.xml" /> </export>
2.3 编译与使用插件
编译该插件软件包,接着通过配置文件使用插件。
参数的传递链如下:首先在simulation.launch.py
中引用配置文件navigation.yaml
python
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(simulation_dir, 'config', 'navigation.yaml'),
description='Full path to the ROS2 parameters file to use for all launched nodes')
接着在navigation.yaml
中修改插件配置,默认如下,用的是SimpleSmoother
插件:
yaml
smoother_server:
ros__parameters:
use_sim_time: True
smoother_plugins: ["simple_smoother"]
simple_smoother:
plugin: "nav2_smoother::SimpleSmoother"
tolerance: 1.0e-10
max_its: 1000
do_refinement: True
将上述替换为自己的插件,本案例为:
yaml
smoother_server:
ros__parameters:
use_sim_time: True
smoother_plugins: ["simple_smoother"]
simple_smoother:
plugin: "bspline_smoother/BSplineSmoother"
接着运行即可看到平滑算法被替换
3 基于B样条曲线的路径平滑
B样条曲线是一种用于表示和描绘曲线的数学工具,它在计算机图形学、计算机辅助设计、计算机动画和数值分析等领域得到广泛应用。其名称中的B
代表了基本(basis)
,而样条
则是在各个领域中广泛应用的一种绘制曲线的技术,例如计算机图形学、物理学模拟、金融和经济分析等。在计算机图形学中,样条通常用于创建平滑的曲线和曲面,以便在三维场景中呈现出更真实的效果。在物理学模拟中,样条可用于描述物体的运动轨迹和变形过程。
具体算法原理请参考:
平滑器的效果如下所示,其中红色是原始A*算法的规划路径,绿色是B样条曲线平滑后的路径
完整代码通过下方博主名片联系获取
🔥 更多精彩专栏:
👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇