2025-08-27  7e401d86f36cb7229c0a471a0bf23327 99+   13 分钟   2.0 k0次访问

ROS2 实战

ROS2 实战

本文是我学习 ROS2（Robot Operating System 2）的 一些实操演练

任务要求

使用ROS2，实现pluginlib插件的使用，完成动态参数的设置以及用Composable Nodes多线程容器组合启动

1、创建工作区与功能包

1
2
3
4
5
mkdir -p ~/ros2_advanced_ws/src
cd ~/ros2_advanced_ws/src
ros2 pkg create --build-type ament_cmake adv_demo \
  --dependencies rclcpp rclcpp_components pluginlib nav_msgs geometry_msgs

其中，上述最后一段是创建adv_demo文件夹以及各种文件依赖

随后，我们补全文件目录

1
2
3
4
5
6
7
8
9
10
11
12
13
14
adv_demo/
  CMakeLists.txt
  package.xml
  include/adv_demo/
    planner_base.hpp
  src/
    planner_component.cpp
    plugins/astar_planner.cpp
    plugins/dwa_planner.cpp
  resource/
    planner_plugins.xml
  launch/
    composed.launch.py

2、定义插件接口(pluginlib 抽象基类)

include/adv_demo/planner_base.hpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
#pragma once
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <vector>

namespace adv_demo {

struct PlanOptions {
  double max_speed{5.0};
};

class PlannerBase {
public:
  virtual ~PlannerBase() = default;
  virtual std::string name() const = 0;

  // 从起点到终点生成 Path（演示接口，实际可加代价图/障碍物）
  virtual nav_msgs::msg::Path plan(const geometry_msgs::msg::PoseStamped& start,
                                   const geometry_msgs::msg::PoseStamped& goal,
                                   const PlanOptions& opts) = 0;
};

} // namespace adv_demo

用纯虚函数重写命名以及规划路线的函数

geometry_msgs::msg是 ROS 中用于表示基础几何信息的消息包，msg 表示这是消息类型的定义

常见消息类型：
PoseStamped： 带时间戳和坐标系的位姿（位置 + 姿态）
Point：三维点（x, y, z）
Quaternion：四元数（表示旋转姿态）
Twist：线速度和角速度（用于控制机器人运动）
Vector3：三维向量

nav_msgs 是 ROS 中用于导航相关功能的消息包，msg 同样表示消息类型。

常见消息类型：
Path：由一系列 PoseStamped 组成的路径（如规划器输出的轨迹）
Odometry：里程计信息（包含机器人的位置、速度等）
OccupancyGrid：占据栅格地图（用于导航中的环境表示）

两个插件的实现

src/plugins/astar_planner.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
#include "adv_demo/planner_base.hpp"
#include <rclcpp/rclcpp.hpp>
#include <pluginlib/class_list_macros.hpp>

namespace adv_demo {

class AStarPlanner : public PlannerBase {
public:
  std::string name() const override { return "AStarPlanner"; }

  nav_msgs::msg::Path plan(const geometry_msgs::msg::PoseStamped& start,
                           const geometry_msgs::msg::PoseStamped& goal,
                           const PlanOptions& opts) override {
    nav_msgs::msg::Path path;
    path.header = goal.header;
    // 演示：直线插值 20 段
    for (int i = 0; i <= 20; ++i) {
      geometry_msgs::msg::PoseStamped p;
      double t = static_cast<double>(i)/20.0;
      p.header = goal.header;
      p.pose.position.x = start.pose.position.x * (1 - t) + goal.pose.position.x * t;
      p.pose.position.y = start.pose.position.y * (1 - t) + goal.pose.position.y * t;
      path.poses.push_back(p);
    }
    (void)opts; // 未用到，但展示可传动态参数
    return path;
  }
};

} // namespace adv_demo

PLUGINLIB_EXPORT_CLASS(adv_demo::AStarPlanner, adv_demo::PlannerBase) //注册插件

src/plugins/dwa_planner.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
#include "adv_demo/planner_base.hpp"
#include <rclcpp/rclcpp.hpp>
#include <pluginlib/class_list_macros.hpp>

namespace adv_demo {

class DwaPlanner : public PlannerBase {
public:
  std::string name() const override { return "DwaPlanner"; }

  nav_msgs::msg::Path plan(const geometry_msgs::msg::PoseStamped& start,
                           const geometry_msgs::msg::PoseStamped& goal,
                           const PlanOptions& opts) override {
    nav_msgs::msg::Path path;
    path.header = goal.header;
    // 演示：做个“S形”路径，体现与 A* 不同
    for (int i = 0; i <= 40; ++i) {
      geometry_msgs::msg::PoseStamped p;
      double t = static_cast<double>(i)/40.0;
      p.header = goal.header;
      p.pose.position.x = start.pose.position.x*(1-t) + goal.pose.position.x*t;
      p.pose.position.y = start.pose.position.y*(1-t) + goal.pose.position.y*t
                          + 0.5*sin(6.28*t); // 轻微弯曲
      path.poses.push_back(p);
    }
    (void)opts;
    return path;
  }
};

} // namespace adv_demo

PLUGINLIB_EXPORT_CLASS(adv_demo::DwaPlanner, adv_demo::PlannerBase)

插件声明

resource/planner_plugins.xml

1
2
3
4
5
6
7
8
9
10
11
12
13
<library path="libadv_demo_planners">
  <class name="adv_demo/AStarPlanner"
         type="adv_demo::AStarPlanner"
         base_class_type="adv_demo::PlannerBase">
    <description>A* demo planner</description>
  </class>
  <class name="adv_demo/DwaPlanner"
         type="adv_demo::DwaPlanner"
         base_class_type="adv_demo::PlannerBase">
    <description>DWA demo planner</description>
  </class>
</library>

上面的插件声明，是为了告诉ROS系统插件在哪，去哪可以加载插件。

组件化规划节点

src/planner_component.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <pluginlib/class_loader.hpp>
#include "adv_demo/planner_base.hpp"
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>

namespace adv_demo {

class PlannerComponent : public rclcpp::Node {
public:
  explicit PlannerComponent(const rclcpp::NodeOptions& options)
  : Node("planner_component", options),
    loader_("adv_demo", "adv_demo::PlannerBase")
  {
    // 声明 & 读取动态参数
    this->declare_parameter<std::string>("planner_type", "adv_demo/AStarPlanner");
    this->declare_parameter<double>("max_speed", 5.0);

    // 参数回调（动态生效）
    param_cb_handle_ = this->add_on_set_parameters_callback(
      [this](const std::vector<rclcpp::Parameter>& params) {
        rcl_interfaces::msg::SetParametersResult res; res.successful = true;
        for (auto& p : params) {
          if (p.get_name() == "planner_type") {
            desired_plugin_ = p.as_string();
            need_reload_ = true;
          } else if (p.get_name() == "max_speed") {
            opts_.max_speed = p.as_double();
            RCLCPP_INFO(this->get_logger(), "max_speed updated: %.2f", opts_.max_speed);
          }
        }
        return res;
      });

    // 订阅起终点（真实项目里来自定位/导航）
    start_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
      "start", 1, [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg){ start_ = *msg; has_start_ = true; tryPlan(); });
    goal_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
      "goal", 1, [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg){ goal_ = *msg; has_goal_ = true; tryPlan(); });

    path_pub_ = this->create_publisher<nav_msgs::msg::Path>("planned_path", 1);

    // 初次加载插件
    desired_plugin_ = this->get_parameter("planner_type").as_string();
    loadPlanner(desired_plugin_);
  }

private:
  void loadPlanner(const std::string& type) {
    try {
      planner_ = loader_.createSharedInstance(type);
      RCLCPP_INFO(get_logger(), "Loaded planner plugin: %s", planner_->name().c_str());
    } catch (const std::exception& e) {
      RCLCPP_ERROR(get_logger(), "Failed to load plugin '%s': %s", type.c_str(), e.what());
      planner_.reset();
    }
  }

  void tryPlan() {
    if (need_reload_) {
      loadPlanner(desired_plugin_);
      need_reload_ = false;
    }
    if (!planner_ || !has_start_ || !has_goal_) return;
    auto path = planner_->plan(start_, goal_, opts_);
    path_pub_->publish(path);
    RCLCPP_INFO(get_logger(), "Published path with %zu poses using %s",
                path.poses.size(), planner_->name().c_str());
  }

  // ROS I/O
  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr start_sub_, goal_sub_;
  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
  geometry_msgs::msg::PoseStamped start_, goal_;
  bool has_start_{false}, has_goal_{false};

  // pluginlib
  pluginlib::ClassLoader<PlannerBase> loader_;
  std::shared_ptr<PlannerBase> planner_;

  // 动态参数
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_handle_;
  PlanOptions opts_;
  std::string desired_plugin_;
  bool need_reload_{false};
};

} // namespace adv_demo

RCLCPP_COMPONENTS_REGISTER_NODE(adv_demo::PlannerComponent)

整体功能:
该类创建了一个 ROS 2 节点，能够：动态加载不同的路径规划插件（如 A*、DWA 等，基于之前定义的PlannerBase接口）
接收起点（start）和终点（goal）位姿
根据加载的规划器插件计算路径，并发布规划结果（planned_path）
支持动态参数调整（如切换规划器类型、修改最大速度）

组件、插件以及依赖等设置

CMakeLists.txt

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
cmake_minimum_required(VERSION 3.5)
project(adv_demo)

# 默认 C++17
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(pluginlib REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)

include_directories(include)

# 组件库（Nodelet 等价）
add_library(adv_demo_planner_component SHARED src/planner_component.cpp)
target_link_libraries(adv_demo_planner_component)
ament_target_dependencies(adv_demo_planner_component
  rclcpp rclcpp_components pluginlib nav_msgs geometry_msgs)
rclcpp_components_register_nodes(adv_demo_planner_component "adv_demo::PlannerComponent")

# 插件库（A* / DWA）
add_library(adv_demo_planners SHARED
  src/plugins/astar_planner.cpp
  src/plugins/dwa_planner.cpp)
ament_target_dependencies(adv_demo_planners rclcpp pluginlib nav_msgs geometry_msgs)

# 安装插件描述
install(FILES resource/planner_plugins.xml
  DESTINATION share/${PROJECT_NAME})

# 导出 pluginlib 索引（让 loader 能发现）
pluginlib_export_plugin_description_file(adv_demo resource/planner_plugins.xml)

# 安装库
install(TARGETS adv_demo_planner_component adv_demo_planners
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)

# 安装头文件与启动文件
install(DIRECTORY include/ DESTINATION include)
install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch)

ament_package()

package.xml

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
<package format="3">
  <name>adv_demo</name>
  <version>0.0.1</version>
  <description>Advanced ROS2 demo with pluginlib, dynamic params, components</description>

  <maintainer email="you@example.com">you</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>rclcpp_components</depend>
  <depend>pluginlib</depend>
  <depend>nav_msgs</depend>
  <depend>geometry_msgs</depend>

  <export>
    <!-- 让 pluginlib 在索引中能找到本包 -->
    <pluginlib plugin="${prefix}/share/adv_demo/planner_plugins.xml"/>
    <rclcpp_components/>
  </export>
</package>

组合式启动（组件容器）

launch/composed.launch.py

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode

def generate_launch_description():
    planner = ComposableNode(
        package='adv_demo',
        plugin='adv_demo::PlannerComponent',
        name='planner_component',
        parameters=[{
            'planner_type': 'adv_demo/AStarPlanner',
            'max_speed': 5.0
        }]
    )

    container = ComposableNodeContainer(
        name='adv_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container_mt',  # 多线程容器
        composable_node_descriptions=[planner],
        output='screen'
    )
    return LaunchDescription([container])

这段代码是 ROS 2（机器人操作系统 2）的启动文件（Launch File），用于配置和启动一个包含路径规划器组件的节点容器。它使用了 ROS 2 的组件化（Composable Nodes）机制，实现更灵活的节点管理。

编译与运行

1
2
3
4
5
cd ~/ros2_advanced_ws
colcon build --symlink-install \
  --cmake-args -DPYTHON_EXECUTABLE=/usr/bin/python3
source install/setup.bash

启动组件容器：

1
2
ros2 launch adv_demo composed.launch.py

在新终端（同样要 source ~/ros2_advanced_ws/install/setup.bash）发布起点与终点：

1
2
3
4
5
# 起点
ros2 topic pub /start geometry_msgs/PoseStamped "{header: {frame_id: 'map'}, pose: {position: {x: 0.0, y: 0.0}}}" -1
# 终点
ros2 topic pub /goal  geometry_msgs/PoseStamped "{header: {frame_id: 'map'}, pose: {position: {x: 10.0, y: 0.0}}}" -1

观察输出的路径：

1
2
ros2 topic echo /planned_path --qos-durability transient_local

动态参数在线切换与调参

切换到 DWA 插件：

1
ros2 param set /planner_component planner_type adv_demo/DwaPlanner

调整最大速度（供你的算法使用）：

1
2
3

ros2 param set /planner_component max_speed 8.0

再次发一次 goal 就会触发新插件规划。

ROS2 实战

https://garyaacm.github.io/2025/08/27/ROS2实战1/

作者

Gary

发布于

2025-08-27

更新于

2025-08-27

许可协议

评论
关注我

:D 一言句子获取中...

链接

最新评论

加载中，最新评论有1分钟缓存...

最新文章

分类

归档

标签

Python4
C++3
ROS22
嵌入式开发2
机器学习2
API1
BP1
C++C1
CNN1
GPU1
Git1
HTTP1
Hexo1
I2C1
Linux1
MAX301021
MQTT1
POSIX1
QThread1
Qt1
查看全部>>

订阅更新

×