模板

VScode配置模板

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
{
  "ROS2 C++ Node Template": {
    "prefix": "ros2cpp",
    "body": [
      "/*",
      "  需求:",
      "  步骤:",
      "    1.包含头文件；",
      "    2.初始化 ROS2 客户端；",
      "    3.定义节点类；",
      "    ",
      "    4.调用spin函数,并传入节点对象指针;",
      "    5.释放资源。",
      "*/",
      "",
      "// 1.包含头文件；",
      "#include \"rclcpp/rclcpp.hpp\"",
      "",
      "// 3.自定义节点类",
      "class ${1:MyNode}: public rclcpp::Node{ ",
      "public:",
      "    $1():Node(\"mynode_node_cpp\"){  // 节点名称一般小写",
      "        RCLCPP_INFO(this->get_logger(),\"创建成功!\");",
      "",
      "    }",
      "};",
      "",
      "int main(int argc, char ** argv)",
      "{",
      "  // 2.初始化 ROS2 客户端；",
      "  rclcpp::init(argc,argv);",
      "  // 4.调用spin函数,并传入节点类对象;",
      "  rclcpp::spin(std::make_shared<$1>()); ",
      "  // 5.释放资源。",
      "  rclcpp::shutdown();",
      "  return 0;",
      "}"
    ],
    "description": "A cpp file template"
  }
}

launch.py：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
{
    "ROS2 Launch Template": {
        "prefix": "ros2_launch",
        "body": [
            "from launch import LaunchDescription",
            "from launch_ros.actions import Node",
            "",
            "def generate_launch_description():",
            "",
            "    return LaunchDescription([])",
        ],
        "description": "A template for ROS2 launch files"
    }
}

在ROS2中：

  • 节点创建方法设为public
  • 内部组件(publisher/subscriber等)设为private

ROS 组件通常加下划线后缀表示成员变量(client_)，节点实例常用简单名称表示主要对象(client)

常见函数：

std::bind(&MyNode::Fuc,this,_[i])绑定成员函数

  • 第一个参数表示对象的成员函数的指针
  • 第二个参数表示对象的地址
  • 后面为输入参数留空 using std::placeholders::_[i]

std::make_shared<MyNode>()创建节点指针

  • <>内为节点类型
  • 返回值为std::shared_ptr类型对象

注意，创建函数的时候SharedPtr 已经代表指针，不要带&

using namespace std::placeholders 实现函数占位符

using namespace std::chrono_literals 可以直接输入时间

C++ lambda函数：匿名函数

1
[capture list] (parameter list) -> return type { function body }

ROS2概述与环境搭建

ROS2简介

整个ROS生态由通信（Plumbing）、工具（Tools）、功能（Capabilities）与社区（Community）四大部分组成

ROS组成体系

ROS2是全新一代机器人操作系统，不只是功能增强的ROS1

本文档采用的是ROS2的humble版本，运行在Linux（Ubuntu 22.04）操作系统上

ROS2对比ROS

ROS 1：围绕一个集中式的 ROS Master 节点构建，它负责节点发现、注册、连接建立（主题、服务、动作服务器/客户端）和参数服务器。节点间通信主要使用 TCPROS/UDPROS 等自定义协议

ROS 2：采用完全去中心化的设计，它基于数据分发服务 (DDS) 标准作为其默认的中间件。DDS 本身就是一个成熟、标准化、去中心化的发布/订阅框架，内置了丰富的服务质量和发现机制

ROS2移除 Master 消除了单点故障，构建系统从Catkin→Ament / Colcon

ROS2安装

虚拟机部分转ROS文档

安装Ubuntu后需要把当前用户设为管理员，否则sudo会失败

大致步骤如下：

  1. 准备1：设置语言环境；
  2. 准备2：启动Ubuntu universe存储库；
  3. 设置软件源；
  4. 安装ROS2；
  5. 配置环境。

准备1：设置语言环境

请先检查本地语言环境是否支持UTF-8编码，可调用如下指令检查并设置UTF-8编码：

1
2
3
4
5
6
locale  # 检查是否支持 UTF-8
sudo apt update && sudo apt install locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8

注意：语言环境可以不同，但必须支持UTF-8编码

准备2：启动Ubuntu universe存储库

打开软件与更新(Software & Updates)窗口，确保启动了universe存储库，以保证可以下载”社区维护的免费和开源软件“

下载自可以改为清华源

设置软件源

先将ROS 2 apt存储库添加到系统，用apt授权的GPG密钥：

1
2
sudo apt update && sudo apt install curl gnupg lsb-release
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg

然后将存储库添加到源列表：

1
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(source /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null

安装ROS2

首先更新apt存储库缓存：

1
sudo apt update

然后升级已安装的软件(ROS2软件包建立在经常更新的Ubuntu系统上，在安装新软件包之前请确保系统是最新的)：

1
sudo apt upgrade

安装桌面版ROS2(建议)，包含ROS、RViz、示例与教程：

1
sudo apt install ros-humble-desktop

过程中可能会报错，安装工具

1
2
sudo apt install aptitude 
sudo aptitude install xxx  # 根据方案来解决安装报错问题

配置环境

终端下，执行ROS2程序时，需要调用如下命令配置环境：

1
source /opt/ros/humble/setup.bash

每次新开终端时，都得执行上述命令，或者将配置环境指令写入 ”~/.bashrc“ 文件，则以后新开终端不再需要手动配置环境：

1
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc

安装测试：

1
2
ros2 run turtlesim turtlesim_node
ros2 run turtlesim turtle_teleop_key

安装colcon构建工具

colcon是一个命令行工具，用于改进编译，测试和使用多个软件包的工作流程

安装命令如下：

1
sudo apt-get install python3-colcon-common-extensions

安装完colcon之后，就可以在ROS2中编写应用程序了

运行优化

每次终端中执行工作空间下的节点时，都需要调用. install/setup.bash指令，使用不便，可以将该指令的调用添加进~/setup.bash，操作格式如下：

1
echo "source /{工作空间路径}/install/setup.bash" >> ~/.bashrc

ROS2集成开发环境

在刚刚的过程会发现ubuntu频繁需要密码，打开terminal，输入seahorse

右键Login更改密码，将新密码设为空即可

终端

在 ROS 中，需要频繁的使用到终端，且可能需要同时开启多个窗口，推荐使用Terminator

安装：

1
sudo apt install terminator

常用快捷键：

1
2
3
4
5
6
Alt+Up                          //移动到上面的终端
Alt+Down                        //移动到下面的终端
Alt+Left                        //移动到左边的终端
Alt+Right                       //移动到右边的终端
Ctrl+Shift+O                    //水平分割终端
Ctrl+Shift+E                    //垂直分割终端

VScode

不要下Ubuntu software里的，可能无法输入中文

去官网下deb文件，下载后在终端中打开，输入命令安装

1
sudo dpkg -i xxxx.deb

vscode 下载插件：Chinese，C++，CMake，Cmake Tools，Python，Msg Language Support，URDF，vscode-pdf，YAML，XML

快捷键配置：向上向下复制行；向上向下移动

VS Code设置打开新窗口不覆盖前窗口

VSCode设置自动保存

在VSCode中，cpp文件中的#include "rclcpp/rclcpp.hpp"包含语句会抛出异常，这是因为没有设置VSCode配置文件中 includepath属性，可以按照如下步骤解决此问题：

  1. 将鼠标移到错误提示语句，此时会出现弹窗；
  2. 点击弹窗中的快速修复，会有新的弹窗，再点击编辑"includePath"设置；
  3. 在新页面中，包含路径属性对应的文本域中，换行输入被包含的路径/opt/ros/humble/include/**

git

安装命令：

1
sudo apt install git

在本教程中会经常使用git clone 仓库地址的方式来将Git仓库拷贝到本地

操作命令

可以通过编译指令colcon和ROS2内置的工具指令ros2来实现功能包的创建、编译、查找与执行等相关操作

colcon 是官方推荐的构建工具，CMake 是底层的构建系统

创建

新建功能包语法如下：

1
ros2 pkg create [包名] --build-type [构建类型] --dependencies [依赖列表] --node-name [可执行程序名称]

格式解释：

  • –build-type：是指功能包的构建类型，有cmake、ament_cmake(c++)、ament_python(python)三种类型可选；
  • –dependencies：所依赖的功能包列表，空格分割多个功能包；
  • –node-name：可执行程序的名称，会自动生成对应的源文件并生成配置文件

编译

编译功能包语法如下：

1
2
colcon build
colcon build --packages-select [功能包列表]

前者会构建工作空间下的所有功能包，后者可以构建指定功能包

编译后刷新环境变量才可以执行（如果在.bashrc文件中已经添加则不需要）

1
. install/setup.bash

从ROS1的source ./devel/setup.bash -> . install/setup.bash

查找

在ros2 pkg命令下包含了多个查询功能包相关信息的参数。

1
2
3
4
ros2 pkg executables ([包名]) # 输出所有功能包或指定功能包下的可执行程序。
ros2 pkg list # 列出所有功能包
ros2 pkg prefix [包名] # 列出功能包路径
ros2 pkg xml # 输出功能包的package.xml内容

执行

执行命令语法如下：

1
ros2 run [功能包] [可执行程序] [参数]

ROS2体系框架

ROS2文件系统

立足系统架构，ROS2可以划分为三层

操作系统层（OS Layer）

ROS虽然称之为机器人操作系统，但实质只是构建机器人应用程序的软件开发工具包，ROS必须依赖于传统意义的操作系统

中间层（Middleware Layer）

主要由数据分发服务DDS与ROS2封装的关于机器人开发的中间件组成。DDS是一种去中心化的数据通讯方式，ROS2还引入了服务质量管理 （Quality of Service）机制，借助该机制可以保证在某些较差网络环境下也可以具备良好的通讯效果。ROS2中间件则主要由客户端库、DDS抽象层与进程内通讯API构成

应用层（Application Layer）

指开发者构建的应用程序，在应用程序中是以功能包为核心的，在功能包中可以包含源码、数据定义、接口等内容

1.5.1文件系统

对于一般开发者而言，工作内容主要集中在应用层，开发者一般通过实现具有某一特定功能的功能包来构建机器人应用程序

功能包是ROS2应用程序的核心，但是功能包不能直接构建，必须依赖于工作空间，一个ROS2工作空间的目录结构如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
WorkSpace --- 自定义的工作空间。
    |--- build：存储中间文件的目录，该目录下会为每一个功能包创建一个单独子目录。
    |--- install：安装目录，该目录下会为每一个功能包创建一个单独子目录。
    |--- log：日志目录，用于存储日志文件。
    |--- src：用于存储功能包源码的目录。
        |-- C++功能包
            |-- package.xml：包信息，比如:包名、版本、作者、依赖项。
            |-- CMakeLists.txt：配置编译规则，比如源文件、依赖项、目标文件。
            |-- src：C++源文件目录。
            |-- include：头文件目录。
            |-- msg：消息接口文件目录。
            |-- srv：服务接口文件目录。
            |-- action：动作接口文件目录。
        |-- Python功能包
            |-- package.xml：包信息，比如:包名、版本、作者、依赖项。
            |-- setup.py：与C++功能包的CMakeLists.txt类似。
            |-- setup.cfg：功能包基本配置文件。
            |-- resource：资源目录。
            |-- test：存储测试相关文件。
            |-- 功能包同名目录：Python源文件目录。

无论是Python功能包还是C++功能包，都可以自定义一些配置文件相关的目录

1
2
3
4
5
6
7
8
|-- C++或Python功能包
    |-- launch：存储launch文件。
    |-- rviz：存储rviz2配置相关文件。
    |-- urdf：存储机器人建模文件。
    |-- params：存储参数文件。
    |-- world：存储仿真环境相关文件。
    |-- map：存储导航所需地图文件。
    |-- ......
源文件说明

无论是C++实现还是Python实现，都是直接实例化的Node对象

更推荐以继承Node的方式来创建节点对象

C++继承Node实现示例如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
#include "rclcpp/rclcpp.hpp"

class newNode: public rclcpp::Node{ // newNode继承ROS2节点基类
public: // 公共访问区域
    newNode():Node("node_name"){ // 调用父类Node构造函数newNode()
        RCLCPP_INFO(this->get_logger(),"hello world!"); // this 是指向当前对象实例的指针
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc,argv); // ROS2初始化
    auto node = std::make_shared<newNode>(); // 创建类实例,()内传参
    rclcpp::shutdown(); // 资源清理
    return 0;
}

Python继承Node实现示例如下：

1
2
3
4
5
6
7
8
9
10
11
import rclpy
from rclpy.node import Node

class newNode(Node):
    def __init__(self):
        super().__init__("node_name_py")
        self.get_logger().info("hello world!")
def main():
    rclpy.init()
    node = newNode() 
    rclpy.shutdown()

之所以继承比直接实例化Node更被推荐，是因为继承方式可以在一个进程内组织多个节点，这对于提高节点间的通信效率是很有帮助的，但是直接实例化则与该功能不兼容

初始化与资源释放在程序中起什么作用？

  1. 前提：构建的程序可能由若干步骤或阶段组成

    初始化–>节点对象–日志输出–>数据的发布—>数据订阅–>…–>资源释放

  2. 不同步骤或阶段之间涉及到数据的传递

  3. 使用context(上下文)对象进行数据传递，这是一个容器，可以存储数据，也可以从中读取数据

  4. 初始化其实就是要创建context对象，资源释放就是要销毁context对象

配置文件说明

C++功能包的构建信息主要包含在package.xml与CMakeLists.txt中

Python功能包的构建信息则主要包含在package.xml和setup.py中

package.xml

不管是何种类型的功能包，package.xml的格式都是类似的，在该文件中包含了包名、版本、作者、依赖项的信息，可以为colcon构建工具确定功能包的编译顺序

一个简单的package.xml示例如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>pkg01_helloworld_cpp</name> 包名
  <version>0.0.0</version> 包的版本号
  <description>TODO: Package description</description> 包的描述信息
  <maintainer email="[联系者邮箱]">ros2</maintainer> 维护者信息
  <license>TODO: License declaration</license> 软件协议

  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

package：根标签，format属性用来声明文件的格式版本

依赖项：

  • buildtool_depend：声明编译工具依赖；

  • build_depend：声明编译依赖；

  • build_export_depend：声明根据此包构建库所需依赖；

  • exec_depend：声明执行时依赖；

    build_depend，build_export_depend，exec_depend很多时候是一样的

  • depend：相当于、、三者的集成；

  • test_depend：声明测试依赖；

  • doc_depend：声明构建文档依赖

CMakeLists.txt

C++功能包中需要配置CMakeLists.txt文件，该文件描述了如何构建C++功能包

一个简单的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
# 声明cmake的最低版本
cmake_minimum_required(VERSION 3.8)
# 包名，需要与package.xml中的包名一致
project(pkg01_helloworld_cpp)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# 引入外部依赖包
find_package(rclcpp REQUIRED)

# 映射源文件与可执行文件
add_executable(helloworld src/helloworld.cpp)
# 设置目标依赖库
ament_target_dependencies(
  helloworld
  "rclcpp"
)
# 定义安装规则
install(TARGETS helloworld
  DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

setup.py

Python功能包中需要配置setup.py文件，该文件描述了如何构建Python功能包

一个简单的setup.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
26
27
from setuptools import setup

package_name = 'pkg02_helloworld_py'

setup(
    name=package_name, # 包名
    version='0.0.0',   # 版本
    packages=[package_name], # 功能包列表
    data_files=[ #需要被安装的文件以及安装路径
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'], # 安装依赖
    zip_safe=True,
    maintainer='ros2', # 维护者
    maintainer_email='ros2@todo.todo', # 维护者 email
    description='TODO: Package description', # 包描述
    license='TODO: License declaration', # 软件协议
    tests_require=['pytest'], # 测试依赖
    entry_points={
        'console_scripts': [
            # 映射源文件与可执行文件
            'helloworld = pkg02_helloworld_py.helloworld:main'
        ],
    },
)

所以不建议修改包名

ROS2核心模块

通信模块

功能包

安装方式：

  • 二进制安装

    ROS官方或社区提供的功能包可以很方便的通过二进制方式安装

    1
    sudo apt install ros-[ROS2版本代号]-[功能包名称]

    可以调用apt search ros-ROS2版本代号-* | grep -i [关键字]格式的命令，根据关键字查找所需的功能包

  • 源码安装

    一般从github获取源码，下载命令如下：

    1
    git clone [仓库地址]

    源码下载后，需要自行编译

分布式

ROS2是一个分布式架构，不同的ROS2设备之间可以方便的实现通信，这在多机器人设备协同中是极其重要的

终端命令与rqt

rqt是一个图形化工具，它的功能与命令行工具类似，但是图形化的交互方式更为友好

launch文件

通过launch文件，可以批量的启动ROS2节点，这是在构建大型项目时启动多节点的常用方式

TF坐标变换

TF坐标变换可以实现机器人不同部件或不同机器人之间的相对位置关系的转换

可视化

ROS2内置了三维可视化工具rviz2，它可以图形化的方式显示机器人模型或显示机器人系统中的一些抽象数据

ROS2技术支持

ROS 2 文档 — ROS 2 文档：Humble 文档

ROS2应用方向

NAV2

Nav2项目继承自ROS Navigation Stack。该项目旨在可以让移动机器人从A点安全的移动到B点。它也可以应用于涉及机器人导航的其他应用，例如跟随动态点。Nav2将用于实现路径规划、运动控制、动态避障和恢复行为等一系列功能

OpenCV

OpenCV（Open Source Computer Vision Library）是一个开源的计算机视觉和机器学习软件库。OpenCV旨在为计算机视觉应用程序提供通用基础架构，并加速机器感知在商业产品中的使用。OpenCV允许企业轻松地使用和修改代码

MoveIt

MoveIt是一组ROS软件包， 主要包含运动规划、碰撞检测、运动学、3D感知、操作控制等功能。它可以用于构建机械臂的高级行为。MoveIt现在可以用于市面上的大多数机械臂，并被许多大公司使用

The Autoware Foundation

Autoware Foundation是ROS下属的非营利组织，支持实现自动驾驶的开源项目。Autoware基金会在企业发展和学术研究之间创造协同效应，为每个人提供自动驾驶技术

F1 Tenth

F1 Tenth是将模型车改为无人车的竞速赛事，是一个由研究人员、工程师和自主系统爱好者组成的国际社区。它最初于 2016 年在宾夕法尼亚大学成立，但后来扩展到全球许多其他机构

microROS

在基于ROS的机器人应用中，micro-ROS正在弥合性能有限的微控制器和一般处理器之间的差距。micro-ROS在各种嵌入式硬件上运行，使ROS能直接应用于机器人硬件

Open Robotics

Open Robotics与全球ROS社区合作，为机器人创建开放的软件和硬件平台，包括 ROS1、ROS2、Gazebo模拟器和Ignition模拟器。Open Robotics使用这些平台解决一些重要问题，并通过为各种客户组织提供软件和硬件开发服务来帮助其他人做同样的事情。

PX4

PX4是一款用于无人机和其他无人驾驶车辆的开源飞行控制软件。该项目为无人机开发人员提供了一套灵活的工具，用于共享技术并为无人机应用程序创建量身定制解决方案

ROS2通信机制核心

通信机制简介

节点

在通信时，不论采用何种方式，通信对象的构建都依赖于节点(Node)，在ROS2中，一般情况下每个节点都对应某一单一的功能模块

一个完整的机器人系统可能由许多协同工作的节点组成，ROS2中的单个可执行文件(C++程序或Python程序)可以包含一个或多个节点

话题

话题(Topic)是一个纽带，具有相同话题的节点可以关联在一起，这正是通信的前提。ROS2是跨语言的，只要二者使用了相同的话题，就可以实现数据的交互。

通信模型

在ROS2中，常用的通信模型有四种：

  1. 话题通信：是一种单向通信模型，在通信双方中，发布方发布数据，订阅方订阅数据，数据流单向的由发布方传输到订阅方
  2. 服务通信：是一种基于请求响应的通信模型，在通信双方中，客户端发送请求数据到服务端，服务端响应结果给客户端
  3. 动作通信：是一种带有连续反馈的通信模型，在通信双方中，客户端发送请求数据到服务端，服务端响应结果给客户端，但是在服务端接收到请求到产生最终响应的过程中，会发送连续的反馈信息到客户端
  4. 参数服务：是一种基于共享的通信模型，在通信双方中，服务端可以设置数据，而客户端可以连接服务端并操作服务端数据

Snipaste_2025-06-30_22-06-00

接口

在通信过程中，需要传输数据，就必然涉及到数据载体，即要以特定格式传输数据

在ROS2中，数据载体称之为接口(interfaces)，通信时使用的数据载体一般需要使用接口文件定义

常用的接口文件有三种：msg文件、srv文件与action文件，每种文件都可以按照一定格式定义特定数据类型的“变量”

  • msg文件

    用于定义话题通信中数据载体的接口文件，示例如下：

    1
    2
    int64 num1
    int64 num2

    在文件中声明了一些被传输的变量数据

  • srv文件

    用于定义服务通信中数据载体的接口文件，示例如下：

    1
    2
    3
    4
    int64 num1
    int64 num2
    ---
    int64 sum

    文件中声明的数据被---分割为两部分，上半部分用于声明请求数据，下半部分用于声明响应数据

  • action文件

    用于定义动作通信中数据载体的接口文件，示例如下：

    1
    2
    3
    4
    5
    int64 num
    ---
    int64 sum
    ---
    float64 progress

    文件中声明的数据被---分割为三部分，上半部分用于声明请求数据，中间部分用于声明响应数据，下半部分用于声明连续反馈数据

可以使用的字段类型有：

  • int8, int16, int32, int64 (或者无符号类型: uint*)
  • float32, float64
  • string
  • time, duration
  • 其他msg文件
  • 变长数组和定长数组

ROS中还有一种特殊类型：Header，标头包含时间戳和ROS2中常用的坐标帧信息，许多接口文件的第一行包含Header标头

参数通信的数据无需定义接口文件，参数通信时数据会被封装为参数对象，参数客户端和服务端操作的都是参数对象

准备工作

  1. 请先创建工作空间ws01_plumbing，本章以及第3章代码部分内容存储在该工作空间下

    1
    2
    3
    mkdir -p ws01_plumbing/src
    cd ws01_plumbing/src
    colcon build

  2. 实际应用中一般建议创建专门的接口功能包定义接口文件，当前教程也遵循这一建议，预先创建教程所需使用的接口功能包

    需要注意的是，目前为止无法在Python功能包中定义接口文件

    终端下进入工作空间的src目录，执行如下命令：

    1
    ros2 pkg create --build-type ament_cmake [base_interfaces]

    该功能包将用于保存本章教程中自定义的接口文件

注意：功能包可以在vscode里创建，但是工作空间建议在终端先创建好，再用vscode打开

话题通信

话题通信是ROS中使用频率最高的一种通信模式，话题通信是基于发布订阅模式

一个节点发布消息，另一个节点订阅该消息

像雷达、摄像头、GPS…. 等等传感器数据的采集都是使用话题通信

数据发布对象称为发布方，数据订阅对象称之为订阅方，发布方和订阅方通过话题相关联，消息的流向是单向的

话题通信的发布方与订阅方是一种多对多的关系，这意味着数据会出现交叉传输的情况

可以通过rqt命令查看话题之间的关系

2.2话题通信模型2 (1)

与ROS1不同的是，现在不需要master了，不存在master挂了整个系统崩溃的情况

话题通信一般应用于不断更新的、少逻辑处理的数据传输场景

关于消息接口的使用有多种方式：

  1. 在ROS2中通过std_msgs包封装了一些原生的数据类型，这些原生数据类型也可以作为话题通信的载体，不过这些数据一般只包含一个 data 字段，而std_msgs包中其他的接口文件也比较简单，结构的单一意味着功能上的局限性，当传输一些结构复杂的数据时，就显得力不从心了；
  2. 在ROS2中还预定义了许多标准话题消息接口，这在实际工作中有着广泛的应用，比如：sensor_msgs包中定义了许多关于传感器消息的接口（雷达、摄像头、点云……），geometry_msgs包中则定义了许多几何消息相关的接口（坐标点、坐标系、速度指令……）；
  3. 如果上述接口文件都不能满足需求，那么就可以自定义接口消息；

案例需求

需求1：编写话题通信实现，发布方以某个频率发布一段文本，订阅方订阅消息，并输出在终端

需求2：编写话题通信实现，发布方以某个频率发布自定义接口消息，订阅方订阅消息，并输出在终端

需求1和需求2的主要区别在于消息载体，前者可以使用原生的数据类型，后者需要自定义接口消息

终端下进入工作空间的src目录，调用命令分别创建C++功能包和Python功能包

1
2
ros2 pkg create cpp01_topic --build-type ament_cmake --dependencies rclcpp std_msgs base_interfaces [--node-name demo01_talker]
ros2 pkg create py01_topic --build-type ament_python --dependencies rclpy std_msgs base_interfaces [--node-name demo01_talker_str_py]

对比ros1好处：CMakeList里自动配置，python现在依赖的setup.py也会自动配置

本笔记不记录python，只针对C++进行学习

原生消息

发布方实现

功能包cpp01_topic的src目录下

demo01_talker_str.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
/*  
  需求：以某个固定频率发送文本“hello world!”，文本后缀编号，每发送一条消息，编号递增1。
  步骤：
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建发布方；
      3-2.创建定时器；
      3-3.组织消息并发布。
    4.调用spin函数，并传入节点对象指针；
    5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals; // 可以直接输入时间单位

// 3.自定义类节点
class Talker: public rclcpp::Node{
public:
  Talker():Node("talker_node_cpp"),count(0){ // 调用基类构造函数设置节点名
    RCLCPP_INFO(this->get_logger(),"发布节点创建！");
    // 3-1.创建发布方；
    /* 
      模板：被发布的消息类型；
      <>内为消息类型
      参数:
        1.话题名称;
        2.QOS(消息队列长度）
      返回值：发布对象指针
    */
    publisher_ = this->create_publisher<std_msgs::msg::String>("chatter",10);
    // 3-2.创建定时器；
    /* 
    参数:
      1.时问间隔;-> 引入 using namespace std::chrono_literals
      2.回调函数;
    返回值：定时器对象指针
    */
    timer_ = this->create_wall_timer(1s,std::bind(&Talker::on_timer,this));
    /* 
    std::bind()
    参数：
      1.要绑定的函数指针（&Talker::on_timer）
      2.调用该函数的对象（this当前节点）
    */
  }
private:
  void on_timer(){
    // 3-3.组织消息并发布
    auto message =std_msgs::msg::String();
    message.data = "hello world!"+std::to_string(count++);
    RCLCPP_INFO(this->get_logger(),"发布方数据:%s",message.data.c_str());
    publisher_->publish(message);
  }
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  size_t count;
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.调用spin函数，并传入节点类对象；
  rclcpp::spin(std::make_shared<Talker>()); 
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

订阅方实现

功能包cpp01_topic的src目录下

demo02_listener_str.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
/*  
    需求：订阅发布方发布的消息，并输出到终端。
    步骤：
        1.包含头文件；
        2.初始化 ROS2 客户端；
        3.定义节点类；
            3-1.创建订阅方；
            3-2.处理订阅到的消息。
        4.调用spin函数，并传入节点对象指针；
        5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using std::placeholders::_1; // 引入函数参数占位符
// 可以直接使用 _1 和 _2 代替冗长的 std::placeholders::_1
// _1 表示新函数对象的第一个参数 _2 表示新函数对象的 第二个参数 以此类推

// 3.定义节点类；
class Listener: public rclcpp::Node{
public:
    Listener(): Node("listener_node_cpp"){
        RCLCPP_INFO(this->get_logger(),"订阅方创建！");
        // 3-1.创建订阅方；
        /* 
            模板：消息类型;
            参数:
                1.话题名称;
                2.QOS，队列长度；
            返回值：订阅对象指针
        */
        subcription_ = this->create_subscription<std_msgs::msg::String>("chatter",10,
            std::bind(&Listener::do_cb,this,_1));
    }
private:
    void do_cb(const std_msgs::msg::String &msg){
        // 3-2.解析并输出数据
        RCLCPP_INFO(this->get_logger(),"订阅的消息：'%s'",msg.data.c_str());
    }
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subcription_;
};


int main(int argc, char const *argv[])
{
    // 2.初始化 ROS2 客户端；
    rclcpp::init(argc,argv);
    // 4.调用spin函数，并传入节点对象指针
    rclcpp::spin(std::make_shared<Listener>());
    // 5.释放资源；
    rclcpp::shutdown();
    return 0;
}

CMakeLists.txt中发布和订阅程序核心配置如下，注意增加listener部分

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
add_executable(demo01_talker_str src/demo01_talker_str.cpp)
add_executable(demo02_listener_str src/demo02_listener_str.cpp)

ament_target_dependencies(
  demo01_talker_str
  "rclcpp"
  "std_msgs"
)
ament_target_dependencies(
  demo02_listener_str
  "rclcpp"
  "std_msgs"
)

install(TARGETS 
  demo01_talker_str
  demo02_listener_str
  DESTINATION lib/${PROJECT_NAME})

CMakeLists配置包括add_executable、ament_target_dependencies、install三部分

编译

1
colcon build --packages-select cpp01_topic

配置接口消息

创建并编辑 .msg 文件

功能包base_interfaces下新建 msg 文件夹，msg文件夹下新建Student.msg文件，文件中输入如下内容：

1
2
3
string   name
int32    age
float64  height

编辑配置文件

在package.xml中需要添加一些依赖包

不用特别记忆，调用ros2 pkg list | grep -i rosidl

rosidl 是 ROS 2 的核心组件，代表了ROS 接口定义语言

1
2
3
4
5
6
7
8
9
10
<!-- 编译依赖 -->
<build_depend>rosidl_default_generators</build_depend>
<!-- 执行依赖 -->
<exec_depend>rosidl_default_runtime</exec_depend>
<!-- 声明当前包所属的功能包组 -->
<member_of_group>rosidl_interface_packages</member_of_group>
<!-- 删除部分
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
-->

为了将.msg文件转换成对应的C++和Python代码，还需要在CMakeLists.txt中添加如下配置

1
2
3
4
5
6
7
# 添加依赖
find_package(rosidl_default_generators REQUIRED)

# 为接口文件生成源码
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Student.msg"
)

编译功能包

1
colcon build --packages-select base_interfaces

编译完成之后，在工作空间下的nstall目录下将生成Student.msg文件对应的C++和Python文件

测试

通过如下命令查看文件定义以及编译是否正常

1
2
. install/setup.bash
ros2 interface show base_interfaces/msg/Student

正常情况下，终端将会输出与Student.msg文件一致的内容

自定义消息

准备

C++文件中包含自定义消息相关头文件时，可能会抛出异常，可以配置VSCode中c_cpp_properties.json文件，在文件中的 includePath属性下添加

1
${workspaceFolder}/install/base_interfaces/include/**

其他接口文件或接口包的使用也与此同理

修改配置文件

package.xml无需修改，CMakeLists.txt文件需要修改：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
add_executable(demo03_talker_stu src/demo03_talker_stu.cpp)
ament_target_dependencies(
  demo03_talker_stu
  "rclcpp"
  "std_msgs"
  "base_interfaces"
)

add_executable(demo04_listener_stu src/demo04_listener_stu.cpp)
ament_target_dependencies(
  demo04_listener_stu
  "rclcpp"
  "std_msgs"
  "base_interfaces"
)

install(TARGETS 
  demo01_talker_str
  demo02_listener_str
  demo03_talker_stu
  demo04_listener_stu
  DESTINATION lib/${PROJECT_NAME})

发布方实现

功能包cpp01_topic的src目录下

demo03_talker_stu.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
/*  
  需求：以某个固定频率发送文本学生信息，包含学生的姓名、年龄、身高等数据。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "base_interfaces/msg/student.hpp"

using namespace std::chrono_literals; // 可以直接输入时间单位

// 3.自定义类节点
class TalkerStu: public rclcpp::Node{ 
public:
  TalkerStu():Node("talkerstu_node_cpp"),count(0){ // 调用基类构造函数设置节点
    RCLCPP_INFO(this->get_logger(),"发布节点创建！");
    // 3-1.创建发布方；
    publisher_ = this->create_publisher<base_interfaces::msg::Student>("chatter_stu",10);
    // 3-2.创建定时器；
    timer_ = this->create_wall_timer(1s,std::bind(&TalkerStu::on_timer,this));
  }
private:
  void on_timer(){
    // 3-3.组织消息并发布
    auto stu = base_interfaces::msg::Student();
    stu.name = "张三";
    stu.age = count++;
    stu.height = 1.65;
    RCLCPP_INFO(this->get_logger(),"学生信息:name=%s,age=%d,height=%.2f",
    stu.name.c_str(),stu.age,stu.height);
    publisher_->publish(stu);
  }
  rclcpp::Publisher<base_interfaces::msg::Student>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  size_t count;
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.调用spin函数，并传入节点类对象；
  rclcpp::spin(std::make_shared<TalkerStu>()); 
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

订阅方实现

功能包cpp01_topic的src目录下

demo04_listener_stu.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
/*  
    需求：订阅发布方发布的学生消息，并输出到终端。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "base_interfaces/msg/student.hpp"

using std::placeholders::_1; // 表示"这里保留一个位置，实际参数将在调用时传入"

// 3.定义节点类；
class ListenerStu: public rclcpp::Node{
public:
    ListenerStu(): Node("listenerstu_node_cpp"){
        RCLCPP_INFO(this->get_logger(),"订阅方创建！");
        // 3-1.创建订阅方；
        subcription_ = this->create_subscription<base_interfaces::msg::Student>("chatter_stu",10,
            std::bind(&ListenerStu::do_cb,this,_1));
    }
private:
    void do_cb(const base_interfaces::msg::Student &stu){
        // 3-2.解析并输出数据
        RCLCPP_INFO(this->get_logger(),"订阅的学生消息:name=%s,age=%d,height=%.2f",
        stu.name.c_str(),stu.age,stu.height);
    }
    rclcpp::Subscription<base_interfaces::msg::Student>::SharedPtr subcription_;
};

int main(int argc, char const *argv[])
{
    // 2.初始化 ROS2 客户端；
    rclcpp::init(argc,argv);
    // 4.调用spin函数，并传入节点对象指针
    rclcpp::spin(std::make_shared<ListenerStu>());
    // 5.释放资源；
    rclcpp::shutdown();
    return 0;
}

编译执行即可

服务通信

服务通信也是ROS中一种极其常用的通信模式，服务通信是基于请求响应模式的，是一种应答机制

一个节点A向另一个节点B发送请求，B接收处理请求并产生响应结果返回给A

服务通信更适用于对实时性有要求、具有一定逻辑处理的应用场景

发送请求数据的对象称为客户端，接收请求并发送响应的对象称之为服务端，同话题通信一样，客户端和服务端也通过话题相关联，不同的是服务通信的数据传输是双向交互式的

一般是一对多的关系(一个服务器，多个服务端)

2.3服务通信模型2 (1)

案例需求

需求：编写服务通信，客户端可以提交两个整数到服务端，服务端接收请求并解析两个整数求和，然后将结果响应回客户端

终端下进入工作空间的src目录，调用命令创建C++功能包，同时创建一个cpp文件

1
ros2 pkg create cpp02_service --build-type ament_cmake --dependencies rclcpp base_interfaces --node-name demo01_server

配置接口消息

创建并编辑 .srv 文件

功能包base_interfaces下新建srv文件夹，srv文件夹下新建AddInts.srv文件，文件中输入如下内容：

1
2
3
4
int32 num1
int32 num2
---
int32 sum

编辑配置文件

srv文件与msg文件的包依赖一致

如果是新建的功能包添加srv文件，那么直接参考定义msg文件时package.xml 配置即可

1
2
3
4
5
6
<!-- 编译依赖 -->
<build_depend>rosidl_default_generators</build_depend>
<!-- 执行依赖 -->
<exec_depend>rosidl_default_runtime</exec_depend>
<!-- 声明当前包所属的功能包组 -->
<member_of_group>rosidl_interface_packages</member_of_group>

由于使用的同一个工作空间，无需修改

为了将.srv文件转换成对应的C++和Python代码，还需要在CMakeLists.txt中添加如下配置

1
2
3
4
5
find_package(rosidl_default_generators REQUIRED) # 添加依赖
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Student.msg"
  "srv/AddInts.srv" # 增添部分
)

编译

终端中进入当前工作空间，编译功能包

1
colcon build --packages-select base_interfaces

测试

编译完成之后，在工作空间下的 install 目录下将生成AddInts.srv文件对应的C++和Python文件

可以在终端下进入工作空间，通过如下命令查看文件定义以及编译是否正常

1
2
. install/setup.bash
ros2 interface show base_interfaces/srv/AddInts

正常情况下，终端将会输出与AddInts.srv文件一致的内容

案例实现

CMakeLists配置参考话题通信

服务端实现

功能包cpp02_service的src目录下

demo01_server.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
/*  
  需求：编写服务端，接收客户端发送请求，提取其中两个整型数据，相加后将结果响应回客户端。
  步骤：
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建服务端；
      3-2.处理请求数据并响应结果。
    4.调用spin函数，并传入节点对象指针；
    5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "base_interfaces/srv/add_ints.hpp"

using base_interfaces::srv::AddInts;
using std::placeholders::_1;
using std::placeholders::_2;

// 3.自定义类节点
class AddIntsServer: public rclcpp::Node{ 
public:
  AddIntsServer():Node("addints_server_node_cpp"){ 
    RCLCPP_INFO(this->get_logger(),"服务端创建成功！");
    // 3-1.创建服务端；
    /* 模板：服务接口类型;
	     参数:
		    1.服务话题;
		    2.回调函数。
	     返回值：服务对象指针
	  */
    server = this->create_service<AddInts>("add_ints",
        std::bind(&AddIntsServer::add,this,_1,_2));
  }
private:
  // 3-2.处理请求数据并响应结果。
  void add(const AddInts::Request::SharedPtr req,const AddInts::Response::SharedPtr res)
  {
    res->sum = req->num1 + req->num2;
    RCLCPP_INFO(this->get_logger(),"请求数据:(%d,%d),响应结果:%d", req->num1, req->num2, res->sum);
  }
  rclcpp::Service<AddInts>::SharedPtr server;
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.调用spin函数，并传入节点类对象；
  rclcpp::spin(std::make_shared<AddIntsServer>()); 
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

验证实现：

1
ros2 service call /add_ints base_interfaces/srv/AddInts "{'num1':10,'num2':2}"

/add_ints是服务的话题名称，后面跟数据类型，可以[TAB]查看

输入参数的写法需要注意

客户端实现

注意客户端不需要挂起，不需要spin

功能包cpp02_service的src目录下

demo02_client.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
93
94
95
96
97
98
99
/*  
  需求：编写客户端，发送两个整型变量作为请求数据，并处理响应结果。
  步骤：
    前提：main中需要验证提交参数是否正确
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建客户端；
      3-2.等待服务连接；
      3-3.组织请求数据并发送；
    4.创建对象指针调用其功能
      调用连接服务函数，根据连接结果进行下一步
      连接成功后，调用请求发送函数
      处理响应结果
    5.释放资源
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "base_interfaces/srv/add_ints.hpp"

using base_interfaces::srv::AddInts;
using namespace std::chrono_literals; // 可以直接输入时间

// 3.自定义类节点
class AddIntsClient: public rclcpp::Node{ 
public:
  AddIntsClient():Node("addints_client_node_cpp"){  
	  RCLCPP_INFO(this->get_logger(),"客户端创建成功！"); 
    // 3-1.创建客户端
    client_ = this->create_client<AddInts>("add_ints");
  }
  // 3-2.等待服务连接；
  bool connect_server(){
      // 循环以5s时间连接服务器，直到服务器连接成功
      while(!client_->wait_for_service(5s)) // 连接上返回true，超过时间返回false
      {
        // 对ctrl+c作出特殊处理
        // 按下ctrl+c是结束R0S2程序，意味着要释放资源，例如关闭context，无法再调用类this
        // rclcpp::get_logger("name")创建 logger 对象不依赖于context
        if (!rclcpp::ok())
        {
          RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"强制退出！");
          return 0;
        }
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"服务连接中，请稍候...");
      }
      return 1;
    }
    // 3-3.组织请求数据并发送；
    //编写发送请求函数，参数是两个整型数据，返回值是提交请求后服务端的返回结果
    rclcpp::Client<AddInts>::FutureAndRequestId send_request(int32_t num1,int32_t num2)
    {
      auto request = std::make_shared<AddInts::Request>();
      request->num1 = num1;
      request->num2 = num2;
      return client_->async_send_request(request);
      // async_send_request返回: a FutureAndRequestId instance.
    }

private:
  rclcpp::Client<AddInts>::SharedPtr client_;
};

int main(int argc, char ** argv)
{
  if(argc!=3)
  {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"请提交两个int数据");
    return 1;
  }
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 客户端不需要spin
  // 3-1 创建客户端对象
  auto client = std::make_shared<AddIntsClient>(); 
  // 根据连接结果做进一步处理
  bool flag = client->connect_server();
  if(!flag)
  {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"服务器连接失败");
    return 0;
  }
  // 调用请求提交函数，接收并处理响应结果。
  // 发送异步请求
  auto response = client->send_request(atoi(argv[1]),atoi(argv[2]));
  // 处理响应
  // 判断是否成功
  if (rclcpp::spin_until_future_complete(client,response) == rclcpp::FutureReturnCode::SUCCESS)
  {
    RCLCPP_INFO(client->get_logger(),"请求正常处理");
    RCLCPP_INFO(client->get_logger(),"响应结果:%d!", response.get()->sum);
  } else {
    RCLCPP_INFO(client->get_logger(),"请求异常");
  }
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

动作通信

导航是一个过程，是耗时操作，如果使用服务通信，那么只有在导航结束时，才会产生响应结果，而在导航过程中，节点A是不会获取到任何反馈的，从而可能出现程序”假死”的现象

更合理的方案应该是：导航过程中，可以连续反馈当前机器人状态信息，当导航终止时，再返回最终的执行结果

在ROS中，该实现策略称为action 通信，适用于耗时的请求响应场景，用以获取连续的状态反馈

功能而言动作通信类似于服务通信，动作客户端可以发送请求到动作服务端，并接收动作服务端响应的最终结果，不过动作通信可以在请求响应过程中获取连续反馈，并且也可以向动作服务端发送任务取消请求

底层实现而言动作通信是建立在话题通信和服务通信之上的，目标发送实现是对服务通信的封装，结果的获取也是对服务通信的封装，而连续反馈则是对话题通信的封装

2.4action通信模型 (1)

案例需求

需求：编写动作通信，动作客户端提交一个整型数据N，动作服务端接收请求数据并累加1-N之间的所有整数，将最终结果返回给动作客户端，且每累加一次都需要计算当前运算进度并反馈给动作客户端

终端下进入工作空间的src目录，调用命令创建C++功能包

1
ros2 pkg create cpp03_action --build-type ament_cmake --dependencies rclcpp rclcpp_action base_interfaces --node-name demo01_action_server

配置接口消息

创建并编辑 .action 文件

功能包base_interfaces下新建action文件夹，action文件夹下新建Progress.action文件，文件中输入如下内容

1
2
3
4
5
int64 num
---
int64 sum
---
float64 progress

编辑配置文件

如果单独构建action功能包，需要在package.xml中需要添加一些依赖包，具体内容如下：

1
2
3
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>action_msgs</depend>
<member_of_group>rosidl_interface_packages</member_of_group>

使用的是之前的包，已经为 msg 、srv 文件添加过了一些依赖，所以 package.xml 中添加如下内容即可

1
2
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>action_msgs</depend>

注意，和之前的是有差异的，之前是 <build_depend>rosidl_default_generators</build_depend>

为了将.action文件转换成对应的C++和Python代码，还需要在CMakeLists.txt 中添加如下配置：

1
2
3
4
5
6
7
find_package(rosidl_default_generators REQUIRED) # 添加依赖

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Student.msg"
  "srv/AddInts.srv"
  "action/Progress.action" # 增添的部分
)

编译

终端中进入当前工作空间，编译功能包

1
colcon build --packages-select base_interfaces

测试

编译完成之后，在工作空间下的 install 目录下将生成Progress.action文件对应的C++和Python文件

也可以在终端下进入工作空间，通过如下命令查看文件定义以及编译是否正常

1
2
. install/setup.bash
ros2 interface show base_interfaces/action/Progress

案例实现

CMakeLists配置参考话题通信

动作服务端实现

cpp03_action的src目录下

demo01_action_server.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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
/*  
  需求：编写动作服务端，需要解析客户端提交的数字，遍历该数字并累加求和，最终结果响应回
       客户端，且请求响应过程中需要生成连续反馈。
  分析:
    1.创建动作服务端对象;
    2. 处理提交的目标值;
    3.生成连续反馈;
    4.响应最终结果；
    5.处理取消请求。
  步骤：
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建动作服务端对象；
      3-2.处理提交的目标值（回调函数）；
      3-3.处理取消请求（回调函数）
      3-4.生成连续反馈与最终响应（回调涵数）
    4.调用spin函数，并传入节点对象指针；
    5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces/action/progress.hpp"

using base_interfaces::action::Progress;
using std::placeholders::_1;
using std::placeholders::_2;
// using namespace std::placeholders; 或者直接这一句

// 3.自定义类节点
class ProgressActionServer: public rclcpp::Node{ 
public:
  ProgressActionServer():Node("progressa_action_server_node_cpp"){  // 节点名称一般小写
	RCLCPP_INFO(this->get_logger(),"action 服务端创建成功!");
  // 3-1.创建动作服务端对象；
  /* 参数列表
      (NodeT node, 
      const std::string &name, 
      rclcpp_action::Server<ActionT>::GoalCallback handle_goal, 
      rclcpp_action::Server<ActionT>::CancelCallback handle_cancel, 
      rclcpp_action::Server<ActionT>::AcceptedCallback handle_accepted
  */
  server_ = rclcpp_action::create_server<Progress>(
      this,
      "get_sum",
      std::bind(&ProgressActionServer::handle_goal,this,_1,_2),
      std::bind(&ProgressActionServer::handle_cancel,this,_1),
      std::bind(&ProgressActionServer::handle_accepted,this,_1));
      RCLCPP_INFO(this->get_logger(),"动作服务端创建，等待请求...");
  }
private:
  rclcpp_action::Server<Progress>::SharedPtr server_;
  // 3-2.处理提交的目标值（回调函数）；
  // GoalCallback =  std::function<GoalResponse(const GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)>;
  rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const Progress::Goal> goal)
  {
    (void)uuid; // 标识目前没用上,避免警告
    // 业务逻辑：判断的端提交数字是否大于1，是接受，不是拒绝
    if(goal->num<=1)
    {
      RCLCPP_INFO(this->get_logger(),"提交的目标值必须大于1");
      return rclcpp_action::GoalResponse::REJECT;
    }
    RCLCPP_INFO(this->get_logger(),"提交目标成功");
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }
  // 3-3.处理取消请求（回调函数）
  // CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
  rclcpp_action::CancelResponse handle_cancel(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
  {
    (void)goal_handle;
    RCLCPP_INFO(this->get_logger(),"接收到取消请求");
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  // 3-4.生成连续反馈与最终响应（回调涵数）  
  // AcceptedCallback = std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>
  void execute(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle){
    // 1. 生成连续反馈
    // 首先要获取目标值，然后遍历，追历中进行累加，且每循环一次就计算进度，并作为连续反馈发布
    RCLCPP_INFO(this->get_logger(), "开始执行任务");
    int num = goal_handle->get_goal()->num;
    int sum = 0;
    double progress = 0.0;
    double progress_pro;
    // void publish_feedback(std::shared_ptr<base_interfaces::action::Progress_Feedback> feedback_msg)
    auto feedback = std::make_shared<Progress::Feedback>();
    auto result = std::make_shared<Progress::Result>();

    // 设置休眠
    rclcpp::Rate rate(1.0);
    for (int i = 0; i <= num; i++)
    {
      // 检查取消请求
      if (goal_handle->is_canceling()) {
        result->sum = sum;
		    // void canceled(std::shared_ptr<base_interfaces::action::Progress_Result> result_msg)
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "任务取消!当前计算结果为%d", sum);
        break;
      }
      sum += i; 
      progress = i/ (double) num; // 计算进度
      feedback->progress = progress;
      progress_pro = progress * 100;
      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(),"任务进行中，当前进度%.2f%%",progress_pro);
      rate.sleep();
    }
    
    // 2. 形成最终响应结果
    // void succeed(std::shared_ptr<base_interfaces::action::Progress_Result> result_msg)
    if(rclcpp::ok())
    {
      result->sum = sum;
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(),"任务完成，最终结果:%d",sum);
    }
  }

  void handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
  {
    // 新建线程处理耗时的主逻辑
    std::thread{std::bind(&ProgressActionServer::execute,this,_1),goal_handle}.detach();
  }
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.调用spin函数，并传入节点类对象；
  rclcpp::spin(std::make_shared<ProgressActionServer>()); 
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

编写回调函数查看源码

create_server -> Server

动作客户端实现

cpp03_action的src目录下

demo02_action_client.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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
/*  
  需求：编写动作客户端，可以发送一个整型数据到服务端，并处理服务端的连续反馈和最终响应结果。
  步骤：
    前提：可以解析终端下动态传入的参数
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建动作客户端；
      3-2.发送请求;
      3-3.处理关于目标值的服务端响应（回调函数）
      3-4.处理连续反馈（回调函数）
      3-5.处理最终响应（回调函数）
    4.调用spin函数，并传入节点对象指针；
    5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces/action/progress.hpp"

using base_interfaces::action::Progress;
using namespace std::placeholders;
using namespace std::chrono_literals; // 可以直接输入时间

// 3.自定义类节点
class ProgressActionClient: public rclcpp::Node{ 
public:
  ProgressActionClient():Node("progress_action_client_node_cpp"){  // 节点名称一般小写
	RCLCPP_INFO(this->get_logger(),"action 客户端创建成功!");
  // 3-1.创建动作客户端；
  /* 参数列表
  NodeT node, 
  const std::string &name
  */
  client_ = rclcpp_action::create_client<Progress>(this,"get_sum");
  }
  // 3-2.发送请求;
  void send_goal(int num){
    // 1.需要连接到服务端
    if(!client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(),"服务器连接失败！");
      return;
    }
    // 2. 发送具体请求
    /* 
    std::shared_future<rclcpp_action::ClientGoalHandle<base_interfaces::action::Progress>::SharedPtr> 
    async_send_goal(
        const base_interfaces::action::Progress::Goal &goal,
        const rclcpp_action::Client<base_interfaces::action::Progress>::SendGoalOptions &options)
    */
    auto goal = Progress::Goal();
    goal.num = num;
    rclcpp_action::Client<Progress>::SendGoalOptions options;
    options.goal_response_callback = std::bind(&ProgressActionClient::goal_response_callback,this,_1);
    options.feedback_callback = std::bind(&ProgressActionClient::feedback_callback,this,_1,_2);
    options.result_callback = std::bind(&ProgressActionClient::result_callback,this,_1);
    auto future = client_ -> async_send_goal(goal,options);
  }
  void cancel_goal() {
    if (client_) {
      RCLCPP_INFO(this->get_logger(), "发送取消请求");
      client_->async_cancel_all_goals();
    }
  }

private:
  rclcpp_action::Client<Progress>::SharedPtr client_;
  // 3-3.处理关于目标值的服务端响应（回调函数）
  /* 
    using GoalHandle = ClientGoalHandle<ActionT>;
    using GoalResponseCallback = std::function<void (typename GoalHandle::SharedPtr)>;
  */
  void goal_response_callback(rclcpp_action::ClientGoalHandle<Progress>::SharedPtr goal_handle){
    if(!goal_handle){
      RCLCPP_ERROR(this->get_logger(),"目标请求被服务端拒绝!");
      rclcpp::shutdown();
    } else{
      RCLCPP_INFO(this->get_logger(),"目标处理中!目标ID: %s",goal_uuid_to_string(goal_handle->get_goal_id()).c_str());
    }
  }
  // 3-4.处理连续反馈（回调函数）
  /* 
    using GoalHandle = ClientGoalHandle<ActionT>;
    using FeedbackCallback =
    std::function<void (
        typename ClientGoalHandle<ActionT>::SharedPtr,
        const std::shared_ptr<const Feedback>)>;
    using FeedbackCallback = typename GoalHandle::FeedbackCallback;
  */
  void feedback_callback(rclcpp_action::ClientGoalHandle<Progress>::SharedPtr goal_handle, const std::shared_ptr<const Progress::Feedback> feedback){
    (void)goal_handle;
    double progress = feedback->progress;
    double progress_pro = progress * 100;
    RCLCPP_INFO(this->get_logger(),"当前进度:%.2f%%",progress_pro);
  }
  // 3-5.处理最终响应（回调函数）
  /* 
    using ResultCallback = typename GoalHandle::ResultCallback;
    template<typename ActionT>
    class ClientGoalHandle { struct WrappedResult }
    using ResultCallback = std::function<void (const WrappedResult & result)>;
  */
  void result_callback(const rclcpp_action::ClientGoalHandle<Progress>::WrappedResult & result){
    // result.code
    // 通过状态码判断结果状态
    if(result.code == rclcpp_action::ResultCode::SUCCEEDED)
    {
      RCLCPP_INFO(this->get_logger(),"最终结果:%ld",result.result->sum);

    } else if(result.code == rclcpp_action::ResultCode::ABORTED)
    {
      RCLCPP_INFO(this->get_logger(),"任务被中止!");

    } else if(result.code == rclcpp_action::ResultCode::CANCELED){
      RCLCPP_INFO(this->get_logger(),"任务被取消!");

    } else{
      RCLCPP_INFO(this->get_logger(),"未知异常!");
    }
    rclcpp::shutdown();
  }

  // 将UUID转换为可读字符串
  std::string goal_uuid_to_string(const rclcpp_action::GoalUUID& uuid) {
      std::stringstream ss;
      for (auto byte : uuid) {
          ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte);
      }
      return ss.str();
  }
};

int main(int argc, char ** argv)
{
  if(argc!= 2)
  {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"请提交一个>1的整型数据!");
    return 1;
  }
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.调用spin函数，并传入节点类对象；
  auto node = std::make_shared<ProgressActionClient>();
  node->send_goal(atoi(argv[1]));
  // 添加键盘监听线程
  std::thread input_thread([node]() {
    RCLCPP_INFO(node->get_logger(), "按 'c' 键取消目标");
    while (rclcpp::ok()) {
      char c = std::cin.get();
      if (c == 'c' || c == 'C') {
        node->cancel_goal();
        break;
      }
    }
  });
  input_thread.detach();
  rclcpp::spin(node); 
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

编写回调函数查看源码

SendGoalOptions

参数服务

参数服务是以共享的方式实现不同节点之间数据交互的一种通信模式

保存参数的节点称之为参数服务端，调用参数的节点称之为参数客户端

参数客户端与参数服务端的交互是基于请求响应的，且参数通信的实现本质上对服务通信的进一步封装

参数服务保存的数据类似于编程中“全局变量”的概念，可以在不同的节点之间共享数据

案例需求

需求：在参数服务端设置一些参数，参数客户端访问服务端并操作这些参数

终端下进入工作空间的src目录，调用命令创建C++功能包

1
ros2 pkg create cpp04_param --build-type ament_cmake --dependencies rclcpp --node-name demo00_param

参数数据类型

在ROS2中，参数由键、值和描述符三部分组成，其中键是字符串类型，值可以是bool、int64、float64、string、byte[]、bool[]、int64[]、float64[]、string[]中的任一类型，描述符默认情况下为空，但是可以设置参数描述、参数数据类型、取值范围或其他约束等信息

C++客户端对应的类是rclcpp::Parameter

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
/*  
  需求：
  步骤：
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1 参数对象创建
      3-2 参数对象解析
    4.调用spin函数，并传入节点对象指针；
    5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"

// 3.自定义类节点
class MyParam: public rclcpp::Node{ 
public:
  MyParam():Node("my_param_node_cpp"){  // 节点名称一般小写
	  RCLCPP_INFO(this->get_logger(),"参数API创建成功!");
    // 3-1 参数对象创建
    rclcpp::Parameter p1("car","tiger");
    rclcpp::Parameter p2("width",0.15); //参数值为浮点类型
    rclcpp::Parameter p3("wheels",2); //参数值为整型
    // 3-2 参数对象解析
    // 获取参数值
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"car_name = %s", p1.as_string().c_str());
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"car_width = %.2f", p2.as_double());
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"wheels = %ld", p3.as_int());
    // 获取参数键
    RCLCPP_INFO(this->get_logger(),"p1 name = %s",p1.get_name().c_str());
    // 获取参数数据类型
    RCLCPP_INFO(this->get_logger(),"p1 type_name = %s", p1.get_type_name().c_str());
    // 将参数值转换成字符串类型
    RCLCPP_INFO(this->get_logger(),"p2 value_to_msg = %s", p2.value_to_string().c_str());
  }
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.调用spin函数，并传入节点类对象；
  rclcpp::spin(std::make_shared<MyParam>()); 
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

案例实现

packages.xml无需修改，配置CMakeList

参数服务端

cpp04_param的src目录下

demo01_param_server.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
/*  
  需求：
  步骤：
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
        3-1.声明参数；
        3-2.查询参数；
        3-3.修改参数；
        3-4.删除参数。
    4.调用spin函数，并传入节点对象指针；
    5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"

// 3.自定义类节点
class ParamServer: public rclcpp::Node{ 
public:
    // 允许删除 需要通过NodeOptions声明
    ParamServer():Node("param_server_node_cpp",rclcpp::NodeOptions().allow_undeclared_parameters(true)){  
		RCLCPP_INFO(this->get_logger(),"参数服务器创建成功!");
	}
    void declare_param(){
        RCLCPP_INFO(this->get_logger(),"----------增加----------");
        // 声明参数并设置默认值
        this->declare_parameter("car_name","tiger");
        this->declare_parameter("height",1.50); 
        this->declare_parameter("wheels",4); 
        // set_parameter才可删除
        this->set_parameter(rclcpp::Parameter("undcl_test",100));
    }
    void get_param(){
        RCLCPP_INFO(this->get_logger(),"----------查询----------");
        // 指定查询
        rclcpp::Parameter car_name = this->get_parameter("car_name");
        RCLCPP_INFO(this->get_logger(),"car_name:%s",car_name.as_string().c_str());
        // 判断包含
        RCLCPP_INFO(this->get_logger(),"包含car_name? %d",this->has_parameter("car_name"));
        RCLCPP_INFO(this->get_logger(),"包含car_type? %d",this->has_parameter("car_type"));
        // 获取所有
        auto params = this->get_parameters({"car_name","height","wheels"});
        for (auto &param : params)
        {
            RCLCPP_INFO(this->get_logger(),"name = %s, value = %s", param.get_name().c_str(), param.value_to_string().c_str());
        }
    }
    void update_param(){
        RCLCPP_INFO(this->get_logger(),"----------修改----------");
        this->set_parameter(rclcpp::Parameter("height",1.75));

    }
    void del_param(){
        RCLCPP_INFO(this->get_logger(),"----------删除----------");
        //this->undeclare_parameter("car_name"); //会报错，无法删除
        // RCLCPP_INFO(this->get_logger(),"删除后还包含car_name吗? %d",this->has_parameter("car_name")); 
        this->undeclare_parameter("undcl_test");
        RCLCPP_INFO(this->get_logger(),"删除后还包含undcl_test吗? %d",this->has_parameter("undcl_test")); 
    }
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.调用spin函数，并传入节点类对象；
  auto paramServer = std::make_shared<ParamServer>();
  paramServer->declare_param();
  paramServer->get_param();
  paramServer->update_param();
  paramServer->del_param();
  rclcpp::spin(paramServer); 
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

参数客户端

cpp04_param的src目录下

demo02_param_client.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
/*
    需求：编写参数客户端，获取或修改服务端参数。
    步骤：
        1.包含头文件；
        2.初始化 ROS2 客户端；
        3.定义节点类；
            3-1.创建参数客户端对象
            3-2.连接到客户端
            3-3.查询参数
            3-4.修改参数
        4.创建节点对象指针，调用参数操作函数；
        5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"

using namespace std::chrono_literals;

// 3.自定义类节点
class ParamClient: public rclcpp::Node{ 
public:
    ParamClient():Node("param_client_node_cpp"){  // 节点名称一般小写
		RCLCPP_INFO(this->get_logger(),"参数客户端创建成功!");
    // 3-1.创建参数客户端对象
    // 参数1: 当前对象节点 参数2: 远程连接节点
    param_client = std::make_shared<rclcpp::SyncParametersClient>(this,"param_server_node_cpp");
    }
    // 3-2.连接到客户端
    bool connect_server(){
      while(!param_client->wait_for_service(5s)){
        if(!rclcpp::ok()){
          return false;
        }
        RCLCPP_INFO(this->get_logger(),"服务连接中...");
      }
      return true;
    }
    void get_param(){
      RCLCPP_INFO(this->get_logger(),"-----------参数客户端查询参数-----------");
      // 获取某个参数
      std::string car_name = param_client->get_parameter<std::string>("car_name");
      double height = param_client->get_parameter<double>("height");
      RCLCPP_INFO(this->get_logger(),"car_name=%s,height=%.2f",car_name.c_str(),height);
      // 获取多个参数
      auto params = param_client->get_parameters({"car_name","height","wheels"});
      for (auto &&param : params)
      {
        RCLCPP_INFO(this->get_logger(),"%s = %s",param.get_name().c_str(),param.value_to_string().c_str());
      }
      RCLCPP_INFO(this->get_logger(),"包含car_name吗?%d",param_client->has_parameter("car_name"));
    }
    void update_param(){
      RCLCPP_INFO(this->get_logger(),"-----------参数客户端修改参数-----------");
      param_client->set_parameters({rclcpp::Parameter("car_name","Mouse"),
      rclcpp::Parameter("height",2.0),
      rclcpp::Parameter("width",0.15),  // 设置一个不存在的参数，必须在参数服务端声明allow_undeclared_parameters(true)
      rclcpp::Parameter("wheels",6)});
      RCLCPP_INFO(this->get_logger(),"新设置的参数:%.2f",param_client->get_parameter<double>("width"));
    }

private:
  rclcpp::SyncParametersClient::SharedPtr param_client;
};


int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.调用spin函数，并传入节点类对象；
  auto client = std::make_shared<ParamClient>();
  bool flag = client->connect_server();
  if(!flag){
    return 0;
  }
  client->get_param();
  client->update_param();
  client->get_param();
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

问题：服务通信不是通过服务话题关联吗？为什么参数客户端是通过参数服务端的节点名称关联？

  1. 参数服务端启动后，底层封装了多个服务通信的服务端；
  2. 每个服务端的话题，都是采用/服务端节点名称/XXXX;
  3. 参数客户端创建后，也会封装多个服务通信的客户端；
  4. 这些客户端与服务端相呼应，也要使用相同的话题，因此客户端再创建时需要使用服务端节点名称

本章小节

无论何种通信机制，实现框架都是类似的

比如：通信必然涉及到双方，双方需要通过“话题”关联，通信还都必然涉及到数据，一般可以通过接口文件来定义数据格式

不同的通信机制其实现模型也存在明显差异

  • 话题通信是基于广播的单向数据交互模式
  • 服务通信是基于请求响应的问答式交数据互模式
  • 动作通信则是在请求响应的过程中又包含连续反馈的数据交互模式
  • 参数服务是基于服务通信的，可以在不同节点间实现数据共享

ROS2通信机制补充

ROS2程序构建时可能遇到的问题：

  1. 怎么实现分布式架构？
  2. 不同工作空间下的功能包重名时会出现什么问题？
  3. 怎么管理功能包？
  4. 节点重名怎么处理？
  5. 话题重名怎么处理？

分布式

分布式通信是指可以通过网络在不同主机之间实现数据交互的一种通信策略

ROS2所基于的中间件是DDS，当处于同一网络中时，通过DDS的域ID机制(ROS_DOMAIN_ID)可以实现分布式

大致流程：在启动节点之前，可以设置域ID的值，不同节点如果域ID相同，那么可以自由发现并通信

默认情况下，所有节点启动时所使用的域ID为0，换言之，只要保证在同一网络，不需要做任何配置，不同ROS2设备上的不同节点即可实现分布式通信

如果要将不同节点划分为多个组，那么可以在终端中启动节点前设置该节点的域ID(比如设置为6)，具体执行命令为：

1
export ROS_DOMAIN_ID=6

上述指令执行后，该节点将被划分到ID为6的域内

如果要为当前设备下的所有节点设置统一的域ID，那么可以执行如下指令

1
echo "export ROS_DOMAIN_ID=6" >> ~/.bashrc

执行完毕后再重新启动终端，运行的所有节点将自动被划分到ID为6的域内

注意

在设置ROS_DOMAIN_ID的值时并不是随意的，也是有一定约束的：

  1. 建议ROS_DOMAIN_ID的取值在[0,101] 之间，包含0和101；
  2. 每个域ID内的节点总数是有限制的，需要小于等于120个；
  3. 如果域ID为101，那么该域的节点总数需要小于等于54个

DDS 域 ID 值的计算规则(补充内容)

DDS是基于TCP/IP或UDP/IP网络通信协议的，网络通信时需要指定端口号，端口号由2个字节的无符号整数表示，其取值范围在[0,65535]之间

端口号的分配也是有其规则的，并非可以任意使用的，根据DDS协议规定以7400作为起始端口，也即可用端口为[7400,65535]，又已知按照DDS协议默认情况下，每个域ID占用250个端口，那么域ID的个数为：(65535-7400)/250 = 232(个)，对应的其取值范围为[0,231]

操作系统还会设置一些预留端口，在DDS中使用端口时，还需要避开这些预留端口，以免使用中产生冲突，不同的操作系统预留端口又有所差异，其最终结果是，在Linux下，可用的域ID为[0,101]与[215-231]，在Windows和Mac中可用的域ID为[0,166]

综上，为了兼容多平台，建议域ID在[0,101] 范围内取值

每个域ID默认占用250个端口，且每个ROS2节点需要占用两个端口，另外，按照DDS协议每个域ID的端口段内，第1、2个端口是Discovery Multicast端口与User Multicast端口，从第11、12个端口开始是域内第一个节点的Discovery Unicast端口与User Unicast，后续节点所占用端口依次顺延，那么一个域ID中的最大节点个数为：(250-10)/2 = 120(个)

特殊情况：域ID值为101时，其后半段端口属于操作系统的预留端口，其节点最大个数为54个

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
域 ID 与节点所占用端口示意

Domain ID:      0
Participant ID: 0

Discovery Multicast Port: 7400
User Multicast Port:      7401
Discovery Unicast Port:   7410
User Unicast Port:        7411

---

Domain ID:      1
Participant ID: 2
Discovery Multicast Port: 7650
User Multicast Port:      7651
Discovery Unicast Port:   7664
User Unicast Port:        7665

# 第1个域+250 节点ID为2->第三个节点 7660开始 (7660 7661) (7662 7663) (7664 7665)

工作空间覆盖

同一工作空间下不允许出现功能包重名的情况，但是当存在多个工作空间时，不同工作空间下的功能包是可以重名的，那么当功能包重名时，会调用哪一个呢？

所谓工作空间覆盖，是指不同工作空间存在重名功能包时，重名功能包的调用会产生覆盖的情况，需要极力避免

演示

在不同的工作空间下创建turtlesim功能包

ws00_helloworld -> turtlesim_node | ws01_plumbing -> turtlesim_node

在 ~/.bashrc 文件下追加如下内容

1
2
source /home/ros2/ws00_helloworld/install/setup.bash
source /home/ros2/ws01_plumbing/install/setup.bash

新建终端运行 ros2 run turtlesim turtlesim_node

会发现执行的是 ws01_plumbing 功能包下的 turtlesim

这与~/.bashrc中不同工作空间的setup.bash文件的加载顺序有关：

  1. ROS2 会解析 ~/.bashrc 文件，并生成全局环境变量 AMENT_PREFIX_PATH 与 PYTHONPATH，两个环境变量取值分别对应了 ROS2 中 C++ 和 Python 功能包，环境变量的值由功能包名称组成
  2. 对于自定义的工作空间而言，后加载的配置文件优先级更高，后配置的工作空间的功能包在环境变量值组成的前部，但是ROS2系统的setup.bash文件不适用此规则，无论该文件在哪加载，优先级始终最低
  3. 调用功能包时，会按照 AMENT_PREFIX_PATH 或 PYTHONPATH 中包配置顺序从前往后依次查找相关功能包，查找到功能包时会停止搜索

隐患

  1. 可能会出现功能包调用混乱，出现实际调用与预期调用结果不符的情况；
  2. 即便可以通过 ~/.bashrc 来配置不同工作空间的优先级，但是经过测试，修改 ~/.bashrc 文件之后不一定马上生效，还需要删除工作空间下build与install目录重新编译，才能生效，这个过程繁琐且有不确定性

元功能包

在ROS2中，提供了一种方式可以将不同的功能包打包成一个功能包，当安装某个功能模块时，直接调用打包后的功能包即可，该包又称之为元功能包(metapackage)

例如：sudo apt install ros-<ros2-distro>-desktop 命令安装 ros2 时就使用了元功能包，该元功能包依赖于 ROS2 中的其他一些功能包，安装该包时会一并安装依赖

新建一个功能包

1
ros2 pkg create tutorails_plumbing

修改 package.xml 文件，添加执行时所依赖的包：

1
2
3
4
5
<exec_depend>base_interfaces</exec_depend>
<exec_depend>cpp01_topic</exec_depend>
<exec_depend>cpp02_service</exec_depend>
<exec_depend>cpp03_action</exec_depend>
<exec_depend>cpp04_param</exec_depend>

通过这个方法可以学习如何查看别人的包

节点重名

由于节点名称一致，虽然实际有多个节点但是在计算图上显示一个，并且节点名称也会和话题名称、服务名称、动作名称、参数等产生关联，届时也可能会导致通信逻辑上的混乱

避免重名问题，一般有两种策略：

  1. 名称重映射，也即为节点起别名；
  2. 命名空间，是为节点名称添加前缀，可以有多级，格式：/xxx/yyy/zzz

这也是在 ROS2 中解决重名问题的常用策略

上述两种策略的实现途径主要有如下三种：

  1. ros2 run 命令实现；
  2. launch 文件实现；
  3. 编码实现

run2 run

设置命名空间

语法：ros2 run 包名 节点名 –ros-args –remap __ns:=命名空间

1
ros2 run turtlesim turtlesim_node --ros-args --remap __ns:=/t1

名称重映射

语法：

  • ros2 run 包名 节点名 –ros-args –remap __name:=新名称
  • ros2 run 包名 节点名 –ros-args –remap __node:=新名称
1
ros2 run turtlesim turtlesim_node --ros-args --remap __name:=turtle1

命名空间与名称重映射叠加

语法：

1
ros2 run turtlesim turtlesim_node --ros-args --remap __ns:=/t1 --remap __name:=turtle1

launch

关于launch文件的基本使用可以参考launch部分

python方式

可以通过类 launch_ros.actions.Node来创建被启动的节点对象，在对象的构造函数中提供了 name 和 namespace 参数来设置节点的名称与命名空间，使用示例如下：

1
2
3
4
5
6
7
8
9
10
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():

    return LaunchDescription([
        Node(package="turtlesim",executable="turtlesim_node",name="turtle1"),
        Node(package="turtlesim",executable="turtlesim_node",namespace="t1"),
        Node(package="turtlesim",executable="turtlesim_node",namespace="t1", name="turtle1")
    ])

XML方式

可以通过 node 标签中 name 和 namespace 属性来设置节点的名称与命名空间，使用示例如下：

1
2
3
4
5
<launch>
    <node pkg="turtlesim" exec="turtlesim_node" name="turtle1" />
    <node pkg="turtlesim" exec="turtlesim_node" namespace="t1" />
    <node pkg="turtlesim" exec="turtlesim_node" namespace="t1" name="turtle1" />
</launch>

YAML方式

可以通过 node 属性中 name 和 namespace 属性来设置节点的名称与命名空间，使用示例如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
launch:
- node:
    pkg: turtlesim
    exec: turtlesim_node
    name: turtle1
- node:
    pkg: turtlesim
    exec: turtlesim_node
    namespace: t1
- node:
    pkg: turtlesim
    exec: turtlesim_node
    namespace: t1
    name: turtle1

运行方式

1
ros2 launch [包名] [可执行程序]

编码

在 rclcpp 和 rclpy 中，节点类的构造函数中，都分别提供了设置节点名称与命名空间的参数

rclcpp中节点类的构造函数如下：

1
2
Node (const std::string &node_name, const NodeOptions &options=NodeOptions())
Node (const std::string &node_name, const std::string &namespace_, const NodeOptions &options=NodeOptions())

构造函数1中可以直接通过node_name设置节点名称，构造函数2既可以通过node_name设置节点名称也可以通过namespace_设置命名空间

1
MyNode():Node("my_node_cpp","ns")

话题重名

在 ROS2 不同的节点之间通信都依赖于话题，话题名称也可能出现重名的情况，话题重名时，系统虽然不会抛出异常，但是通过相同话题名称关联到一起的节点可能并不属于同一通信逻辑，从而导致通信错乱，甚至出现异常,这种情况下可能就需要将相同的话题名称设置为不同

又或者，两个节点是属于同一通信逻辑的，但是节点之间话题名称不同，导致通信失败。这种情况下就需要将两个节点的话题名称由不同修改为相同

与节点重名的解决思路类似的，为了避免话题重名问题，一般有两种策略：

  1. 名称重映射，也即为话题名称起别名；
  2. 命名空间，是为话题名称添加前缀，可以有多级，格式：/xxx/yyy/zzz

需要注意的是，通过命名空间设置话题名称时，需要保证话题是非全局话题

与节点重名解决方案类似的，修改话题名称的方式主要有如下三种：

  1. ros2 run 命令实现；
  2. launch 文件实现；
  3. 编码实现

run2 run

设置命名空间

该实现与节点重名的方法相同

语法：ros2 run 包名 节点名 –ros-args –remap __ns:=命名空间

1
ros2 run turtlesim turtlesim_node --ros-args --remap __ns:=/t1

话题名称重映射

语法：ros2 run 包名 节点名 –ros-args –remap 原话题名称:=新话题名称

1
ros2 run turtlesim turtlesim_node --ros-args --remap /turtle1/cmd_vel:=/cmd_vel

当为节点添加命名空间时，节点下的所有非全局话题都会前缀命名空间，而重映射的方式只是修改指定话题

launch

python方式

可以通过类 launch_ros.actions.Node的构造函数中的参数 remappings 修改话题名称，使用示例如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():

    return LaunchDescription([
        Node(package="turtlesim",executable="turtlesim_node",namespace="t1"),
        Node(package="turtlesim",
            executable="turtlesim_node",
            remappings=[("/turtle1/cmd_vel","/cmd_vel")]
        )

    ])

XML方式

通过 node 标签的子标签 remap（属性from取值为被修改的话题名称，属性to的取值为修改后的话题名称） 修改话题名称

1
2
3
4
5
6
<launch>
    <node pkg="turtlesim" exec="turtlesim_node" namespace="t1" />
    <node pkg="turtlesim" exec="turtlesim_node">
        <remap from="/turtle1/cmd_vel" to="/cmd_vel" />
    </node>
</launch>

YAML方式

通过 node 属性中 remap（属性from取值为被修改的话题名称，属性to的取值为修改后的话题名称） 修改话题名称

1
2
3
4
5
6
7
8
9
10
11
12
launch:
- node:
    pkg: turtlesim
    exec: turtlesim_node
    namespace: t1
- node:
    pkg: turtlesim
    exec: turtlesim_node
    remap:
    -
        from: "/turtle1/cmd_vel"
        to: "/cmd_vel

编码

话题名称大致可以分为三种类型:

  • 全局话题(话题参考ROS系统，与节点命名空间平级)；
  • 相对话题(话题参考的是节点的命名空间，与节点名称平级)；
  • 私有话题(话题参考节点名称，是节点名称的子级)

全局话题

定义时以/开头的名称，和命名空间、节点名称无关

1
publisher_ = this->create_publisher<std_msgs::msg::String>("/topic/chatter", 10);

话题名称为 /topic/chatter，与命名空间 xxx 以及节点名称 yyy 无关

相对话题

非/开头的名称，参考命名空间设置话题名称，和节点名称无关

1
publisher_ = this->create_publisher<std_msgs::msg::String>("topic/chatter", 10);

话题名称为 /xxx/topic/chatter，与命名空间 xxx 有关，与节点名称 yyy 无关

私有话题

以~/开头的名称，和命名空间、节点名称都有关系

1
publisher_ = this->create_publisher<std_msgs::msg::String>("~/topic/chatter", 10);

话题名称为 /xxx/yyy/topic/chatter，使用命名空间 xxx 以及节点名称 yyy 作为话题名称前缀

话题名称设置规则在rclcpp与rclpy中基本一致，且上述规则也同样适用于ros2 run指令与launch文件

时间相关API

Rate

示例：周期性输出一段文本

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
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
using namespace std::chrono_literals;

// 3.自定义节点类
class MyNode: public rclcpp::Node{ 
public:
    MyNode():Node("time_node_cpp"){  // 节点名称一般小写
        RCLCPP_INFO(this->get_logger(),"创建成功!");
        demo_rate();
    }
private:
    void demo_rate(){
      // 1. 创建Rate对象
      rclcpp::Rate rate(500ms);  // 设置时间间隔
      // rclcpp::Rate rate(1.0); // 设置频率，输入浮点型数据
      // 2. 调用Rate的sleep函数
      while (rclcpp::ok())
      {
        RCLCPP_INFO(this->get_logger(),"---------");
        rate.sleep();
      }
    }
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.调用spin函数,并传入节点类对象;
  rclcpp::spin(std::make_shared<MyNode>()); 
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

Time

示例：

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
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
using namespace std::chrono_literals;

// 3.自定义节点类
class MyNode: public rclcpp::Node{ 
public:
    MyNode():Node("time_node_cpp"){  // 节点名称一般小写
        RCLCPP_INFO(this->get_logger(),"创建成功!");
        // demo_rate();
        demo_time();
    }
private:
    void demo_time(){
      // 1. 创建 Time 对象
      rclcpp::Time t1(500000000L); // 0.5s
      rclcpp::Time t2(2,500000000L); // 2.5s
      // rclcpp::Time right_now = this->get_clock()->now();
      rclcpp::Time right_now = this->now();
      // 2. 调用Time对象函数
      // 秒为单位的输出 和 纳秒为单位的输出
      RCLCPP_INFO(this->get_logger(),"s = %.2f, ns = %ld",t1.seconds(),t1.nanoseconds());  
      RCLCPP_INFO(this->get_logger(),"s = %.2f, ns = %ld",t2.seconds(),t2.nanoseconds());
      RCLCPP_INFO(this->get_logger(),"s = %.2f, ns = %ld",right_now.seconds(),right_now.nanoseconds());
    }
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.调用spin函数,并传入节点类对象;
  rclcpp::spin(std::make_shared<MyNode>()); 
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

Time 的典型场景

  • 为消息添加时间戳（如 sensor_msgs/msg/Image 的 header.stamp）
  • 记录事件发生的具体时刻（如机器人到达某个位置的时间）

Duration

示例：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
using namespace std::chrono_literals;

// 3.自定义节点类
class MyNode: public rclcpp::Node{ 
public:
    MyNode():Node("time_node_cpp"){  // 节点名称一般小写
        RCLCPP_INFO(this->get_logger(),"创建成功!");
        // demo_rate();
        // demo_time();
        demo_duration();
    }
private:
    void demo_duration(){
      // 1. 创建 Duraiton 对象
      rclcpp::Duration du1(1s); // 1s
      rclcpp::Duration du2(2,500000000); // 2.5s
      // 2. 调用 Duration 函数
      RCLCPP_INFO(this->get_logger(),"s = %.2f, ns = %ld",du1.seconds(),du1.nanoseconds());
      RCLCPP_INFO(this->get_logger(),"s = %.2f, ns = %ld",du2.seconds(),du2.nanoseconds());
    }
};

Duration 的典型场景

  • 设置定时器周期
  • 实现延时等待

问题：Time与Duration有什么区别？

API使用类似，但二者有着本质区别：

rclcpp:Time t2(2,500000000L) –> 指的是一个具体时刻(时间点)

rclcpp::Duration du2(2,500000000) –> 指的是一个时间段

运算

Time ± Duration → Time：时间点加减时间段 → 新时间点

Time - Time → Duration：两个时间点相减 → 时间段

Duration ± Duration → Duration：时间段加减 → 新时间段

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
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
using namespace std::chrono_literals;

// 3.自定义节点类
class MyNode: public rclcpp::Node{ 
public:
    MyNode():Node("time_node_cpp"){  // 节点名称一般小写
        RCLCPP_INFO(this->get_logger(),"创建成功!");
        // demo_rate();
        // demo_time();
        // demo_duration();
        demo_opt();
    }
private:
    void demo_opt(){
      rclcpp::Time t1(10,0);
      rclcpp::Time t2(30,0);

      rclcpp::Duration du1(8,0);
      rclcpp::Duration du2(17,0);
      // 比较
      RCLCPP_INFO(this->get_logger(),"t1 >= t2 ? %d",t1 >= t2);
      RCLCPP_INFO(this->get_logger(),"t1 < t2 ? %d",t1 < t2);
      rclcpp::Duration du3 = t2 - t1;
      rclcpp::Time t3 = t1 + du1;
      rclcpp::Time t4 = t1 - du1;
      RCLCPP_INFO(this->get_logger(), "t3 = %.2f",t3.seconds());  
      RCLCPP_INFO(this->get_logger(), "t4 = %.2f",t4.seconds()); 
      RCLCPP_INFO(this->get_logger(), "du3 = %.2f",du3.seconds()); 
      // 比较
      RCLCPP_INFO(this->get_logger(),"du1 >= du2 ? %d", du1 >= du2);
      RCLCPP_INFO(this->get_logger(),"du1 < du2 ? %d", du1 < du2);
      // 数学运算
      rclcpp::Duration du4 = du1 * 3.0;
      rclcpp::Duration du5 = du1 + du2;
      rclcpp::Duration du6 = du1 - du2;
      RCLCPP_INFO(this->get_logger(), "du4 = %.2f",du4.seconds()); 
      RCLCPP_INFO(this->get_logger(), "du5 = %.2f",du5.seconds()); 
      RCLCPP_INFO(this->get_logger(), "du6 = %.2f",du6.seconds()); 
    }
};

Duration 可以负数，但 Time 不可以负数

通信机制工具

在ROS2中，通信机制相关的工具有两种类型，分别是命令行工具和图形化工具（rqt）

前者是一系列终端命令的集合，后者则是ROS2基于QT框架，针对机器人开发的一系列可视化工具的集合

命令工具

关于命令的使用一般都会提供帮助文档

1
2
命令 -h -> ros2 node -h
命令 --help -> ros2 node -help

常用命令整合：

ros2 interace 整合了rosmsg

ROS2 将消息、服务、动作等抽象为“接口”，简化工具链

命令 参数
ros2 node info → 输出节点信息
list → 输出运行中的节点的列表
ros2 interace list → 输出所有可用的接口消息
package → 输出指定功能包下的接口
packages → 输出全部包含接口消息的功能包
proto → 输出接口消息原型
show → 输出接口消息定义格式
ros2 topic bw → 输出话题消息传输占用的带宽
delay → 输出带有 header 的话题延迟
echo → 输出某个话题下的消息
find → 根据类型查找话题
hz → 输出消息发布频率
info → 输出话题相关信息
list → 输出运行中的话题列表
pub → 向指定话题发布消息
type → 输出话题使用的接口类型
ros2 service call → 向某个服务发送请求
find → 根据类型查找服务
list → 输出运行中的服务列表
type → 输出服务使用的接口类型
ros2 action info → 输出指定动作的相关信息
list → 输出运行中的动作的列表
send_goal → 向指定动作发送请求
ros2 param delete → 删除参数
describe → 输出参数的描述信息
dump → 将节点参数写入到磁盘文件
get → 获取某个参数
list → 输出可用的参数的列表
load → 从磁盘文件加载参数到节点
set → 设置参数

rqt工具箱

一般只要安装的是desktop版本就会默认安装rqt工具箱

常用的启动命令为：

1
rqt

启动rqt之后，可以通过plugins添加所需的插件

可以在rqt中向话题发送消息

ROS2通信机制实操

终端下进入工作空间的src目录，调用如下命令创建C++功能包

1
ros2 pkg create cpp07_exercise --build-type ament_cmake --dependencies rclcpp turtlesim base_interfaces geometry_msgs rclcpp_action

话题通信案例

需求：启动两个turtlesim_node节点，节点2中的乌龟自动调头180°，可以通过键盘控制节点1中的乌龟运动，但是不能控制节点2的乌龟，需要自实现功能：可以根据乌龟1的速度生成并发布控制乌龟2运动的速度指令，最终两只乌龟做镜像运动

核心实现是如何订阅乌龟1的速度并生成发布控制乌龟2运动的速度指令的，并且该节点需要在掉头完毕后启动

package.xml文件无需修改

CMakeLists.txt中配置launch文件

1
2
3
4
5
install(TARGETS 
  test01_topic
  DESTINATION lib/${PROJECT_NAME})
# 增加
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

launch文件

启动小乌龟节点，查看action类型

1
2
3
ros2 action list
ros2 action info /turtle1/rotate_absolute
ros2 action send_goal /turtle1/rotate_absolute

后面不清楚的就[TAB]补齐即可

明确在终端如何通过命令控制乌龟运动

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
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess,RegisterEventHandler
from launch.event_handlers import OnProcessExit

def generate_launch_description():
    # 1. 启动两个turtlesim_node，其中一个设置命名空间
    t1 = Node(package="turtlesim",executable="turtlesim_node")
    t2 = Node(package="turtlesim",executable="turtlesim_node",namespace="t2")
    # 2. 控制第二个乌龟掉头
    rotate = ExecuteProcess(
        cmd = ["ros2 action send_goal /t2/turtle1/rotate_absolute turtlesim/action/RotateAbsolute \"{'theta':3.14}\""], # 记得转义字符
        output = "both",
        shell = True  # cmd当成终端指令执行
    )
    # 3. 调用自定义的节点，并且节点调用顺序有要求
    test01 = Node(package="cpp07_exercise",executable="test01_topic")
    # 4. 控制节点启动,需要通过注册事件完成
    # 创建事件注册对象，在对象中声明针对哪个目标节点，在哪个事件触发时，执行哪种操作
    register_rotate_exit_event = RegisterEventHandler(
        # 创建一个新对象，监听rotate的启动，退出后启动test01
        event_handler = OnProcessExit(
            target_action=rotate, # 目标节点
            on_exit=test01) # 触发事件
    )
    return LaunchDescription([t1,t2,rotate,register_rotate_exit_event])

速度订阅与发布

test01_topic.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
/*
  需求：订阅窗口1中乌龟的位姿信息，解析出线速度和角速度，生成控制窗口2乌龟运动的指令并发布
  明确：
      订阅话题：/turtle1/pose
      订阅消息：turtlesim/msg/Pose
              x: 0.0
              y: 0.0
              theta: 0.0
              linear_velocity: 0.0
              angular_velocity: 0.0

      发布话题：/t2/turtle1/cmd_vel
      发布消息：geometry_msgs/msg/Twist
              linear:
                x: 0.0
                y: 0.0
                z: 0.0
              angular:
                x: 0.0  翻滚
                y: 0.0  俯仰
                z: 0.0  左右转
  步骤：
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1 创建发布方
      3-2 创建订阅方(订阅乌龟1位姿，解析速度)
      3-3 订阅方回调函数处理速度，并生成发布控制乌龟2的指令
    4.调用spin函数,并传入节点对象指针;
    5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "turtlesim/msg/pose.hpp"

using std::placeholders::_1;

// 3.自定义节点类
class Test01PubSub: public rclcpp::Node{ 
public:
    Test01PubSub():Node("test01_pub_sub_node_cpp"){  // 节点名称一般小写
        RCLCPP_INFO(this->get_logger(),"创建成功!");
        // 3-1 创建发布方
        twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/t2/turtle1/cmd_vel",10);
        // 3-2 创建订阅方(订阅乌龟1位姿，解析速度)
        pose_sub_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose",10,std::bind(&Test01PubSub::do_pose,this,_1));
    }

private:
    void do_pose(const turtlesim::msg::Pose & pose){
      // 处理速度，并生成发布控制乌龟2的指令
      geometry_msgs::msg::Twist twist;
      twist.linear.x = (pose.linear_velocity); // 线速度不变
      twist.angular.z = -(pose.angular_velocity); // 角速度取反
      twist_pub_->publish(twist);
    }
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_sub_;
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.调用spin函数,并传入节点类对象;
  rclcpp::spin(std::make_shared<Test01PubSub>()); 
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

BUG描述：乌龟1后退时，乌龟2仍然前进

BUG原因：

  1. 和乌龟位姿发布有关，当乌龟实际速度为负数时，位姿中的速度仍是正数；
  2. 发布的乌龟2的速度，与位姿中的线速度一致

BUG修复：(修改源码)

1
2
// p->linear_velocity = std::sqrt(lin_vel_x_ * lin_vel_x_ + lin_vel_y_ * lin_vel_y_);
p->linear_velocity = lin_vel_x_;

服务通信案例

需求：在turtlesim_node节点的窗体中在指定位置生成一只新乌龟并可以输出两只乌龟之间的直线距离

需要关注的问题有两个：

  1. 如何在指定位置生成一只新乌龟？
  2. 计算两只乌龟的距离应该使用何种通信模式又如何实现？

问题1可以通过调用turtlesim_node内置的名称为/spawn的服务功能来实现新乌龟的创建；

问题2可以通过服务通信来实现，客户端发送新生成的乌龟的位姿到服务端，服务端根据该坐标以及原生乌龟的坐标计算距离并响应。当然如果使用服务通信，还需要自定义服务接口

自定义接口消息

功能包base_interfaces_demo的srv目录下，新建srv文件Distance.srv

1
2
3
4
5
float32 x
float32 y
float32 theta
---
float32 distance

CMakeList.txt配置(其余部分之前配置过)

1
2
3
4
5
6
7
# 为接口文件生成源码
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Student.msg"
  "srv/AddInts.srv"
  "action/Progress.action"
  "srv/Distance.srv" # 新增
)

检查是否配置成功；

先创建cpp和launch.py文件，配置功能包目录下的CMakeList，launch文件的配置因为用的是同一功能包不需要重复

launch文件

服务端：

1
2
3
4
5
6
7
8
9
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    # 1. turtlesim node
    t1 = Node(package="turtlesim",executable="turtlesim_node")
    # 2. 自定义的服务端
    server = Node(package="cpp07_exercise",executable="test02_server")
    return LaunchDescription([t1,server])

客户端：

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 Node
from launch.actions import ExecuteProcess

def generate_launch_description():
    # 1. 指定位置生成一只新乌龟
    x = 6.0
    y = 9.0
    theta = 1.57
    name = "t2"
    spawn = ExecuteProcess(
        cmd = ["ros2 service call /spawn turtlesim/srv/Spawn \"{'x': "
               + str(x) +",'y': "+ str(y) +
               ",'theta': "+ str(theta) +
               ",'name':'"+ name +"'}\""], # 记得转义字符
        output = "both",
        shell = True  # cmd当成终端指令执行
    )
    client = Node(package="cpp07_exercise",
                  executable="test02_client",
                  arguments=[str(x),str(y),str(theta)])
    # 相当于ros2 run cpp07_exercise test02_client 6 9 0.0 --ros-args

    # 2. 调用客户端发送目标点坐标 
    return LaunchDescription([spawn,client])

服务端实现

test02_server.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
/*
  需求:解析客户端提交的目标点坐标，获取原生乌龟坐标，计算二者距离并响应回客户端。
  步骤:
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建一个订阅方（原生乌龟位姿/turtlel/pose)；
      3-2.创建一个服务端；
      3-3.回调函数需要解析客户端数据并响应结果到客户端。
    4.调用spin函数,并传入节点对象指针;
    5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "turtlesim/msg/pose.hpp"
#include "base_interfaces/srv/distance.hpp"

using std::placeholders::_1;
using std::placeholders::_2;

using base_interfaces::srv::Distance;

// 3.自定义节点类
class Test02Server: public rclcpp::Node{ 
public:
    Test02Server():Node("test02_server_node_cpp"),x(0.0),y(0.0){  // 节点名称一般小写
        RCLCPP_INFO(this->get_logger(),"创建成功!");
        pose_sub_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose",10,std::bind(&Test02Server::pose_cb,this,_1));
        server_ = this->create_service<Distance>("distance",std::bind(&Test02Server::distance_cb,this,_1,_2));
    }
private:
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_sub_;
    rclcpp::Service<Distance>::SharedPtr server_;
    float x,y;
    void pose_cb(const turtlesim::msg::Pose::SharedPtr pose){
      x = pose->x;
      y = pose->y;
    }
    void distance_cb(const Distance::Request::SharedPtr request,const Distance::Response::SharedPtr response){
      // 解析目标点坐标
      float goal_x = request->x;
      float goal_y = request->y;
      // 计算距离
      float distance_x = goal_x - x;
      float distance_y = goal_y - y;
      float distance = std::sqrt(distance_x * distance_x + distance_y * distance_y);
      // 设置进响应
      response->distance = distance;
      RCLCPP_INFO(this->get_logger(),"目标坐标:(%.2f,%.2f),距离:%.2f",goal_x,goal_y,response->distance);
    }
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.调用spin函数,并传入节点类对象;
  rclcpp::spin(std::make_shared<Test02Server>()); 
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

客户端实现

test02_client.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
/*
  需求：客户端需要提交目标点坐标，并解析响应结果。
  步骤：
    0.解析传入的数据
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.构造函数创建客户端;
      3-2.客户端需要连接服务端;
      3-3.发送请求数据。
    4.调用对象服务连接、发送请求、处理响应相关函数;
    5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "base_interfaces/srv/distance.hpp"

using base_interfaces::srv::Distance;
using namespace std::chrono_literals;

// 3.自定义节点类
class Test02Client: public rclcpp::Node{ 
public:
    Test02Client():Node("test02_client_node_cpp"){  // 节点名称一般小写
        RCLCPP_INFO(this->get_logger(),"创建成功!");
        // 3-1.构造函数创建客户端;
        distance_client_ = this->create_client<Distance>("distance");
    }
    // 3-2.客户端需要连接服务端;
    bool connect_server(){
        while(!distance_client_->wait_for_service(1s)){
          if (!rclcpp::ok())
          {
          RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"客户端退出！");
          return false;
          }
          RCLCPP_INFO(this->get_logger(),"服务连接中");
        }
        return true;
    }
    rclcpp::Client<Distance>::FutureAndRequestId send_goal(float x, float y, float theta)
    {
      auto request = std::make_shared<Distance::Request>();
      request->x = x;
      request->y = y;
      request->theta = theta;
      return distance_client_->async_send_request(request);
    }

private:
    rclcpp::Client<Distance>::SharedPtr distance_client_;
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  if (argc != 5) // 5是因为还有name，一共其实是4个参数
  {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"请传入目标的位姿参数:(x,y,theta)");
    return 1;
  }
  rclcpp::init(argc,argv);
  // 4.调用对象服务连接、处理响应相关函数
  auto client = std::make_shared<Test02Client>();
  // 解析提交的参数
  float goal_x = atoi(argv[1]);
  float goal_y = atoi(argv[2]);
  float goal_theta = atoi(argv[3]);
  bool flag = client->connect_server();
  if(!flag)
  {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"服务连接失败!");
    return 1;
  }
  // 发送请求
  auto distance_future = client->send_goal(goal_x,goal_y,goal_theta);
  // 判断响应状态
  if(rclcpp::spin_until_future_complete(client,distance_future)==rclcpp::FutureReturnCode::SUCCESS)
  {
    RCLCPP_INFO(client->get_logger(),"两只乌龟相距%.2f米。",distance_future.get()->distance);
  } else{
    RCLCPP_ERROR(client->get_logger(),"获取距离服务失败!");
  }
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

动作通信案例

需求：处理请求发送的目标点，控制乌龟向该目标点运动，并连续反馈乌龟与目标点之间的剩余距离

在上述案例与服务通信案例类似，需要关注控制原生乌龟向目标乌龟运动并连续反馈剩余距离应该使用何种通信模式又如何实现？

思路：可以通过动作通信来实现，动作客户端发送目标信息到动作服务端，服务端根据该坐标以及原生乌龟的坐标计计算出二者距离，计算速度并控制原生乌龟运动。当然如果使用动作通信，还需要自定义动作接口。

动作接口文件

功能包base_interfaces_demo的action目录下，新建action文件Nav.action

1
2
3
4
5
6
7
8
9
float32 goal_x
float32 goal_y
float32 goal_theta
---
float32 turtle_x
float32 turtle_y
float32 turtle_theta
---
float32 distance

编辑CMakeList(其余之前配置过)

1
2
3
4
5
6
7
8
# 为接口文件生成源码
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Student.msg"
  "srv/AddInts.srv"
  "srv/Distance.srv"
  "action/Progress.action"
  "action/Nav.action" #新增
)

launch文件

launch和服务通信的基本相同，复制内容修改节点即可

服务端：

1
2
3
4
5
6
7
8
9
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    # 1. turtlesim node
    t1 = Node(package="turtlesim",executable="turtlesim_node")
    # 2. 自定义的服务端
    server = Node(package="cpp07_exercise",executable="test03_action_server")
    return LaunchDescription([t1,server])

客户端：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess

def generate_launch_description():
    x = 6.0
    y = 9.0
    theta = 1.57
    client = Node(package="cpp07_exercise",
                  executable="test03_action_client",
                  arguments=[str(x),str(y),str(theta)])
    # 相当于ros2 run cpp07_exercise test02_client 6 9 0.0 --ros-args
    # 调用客户端发送目标点坐标 
    return LaunchDescription([client])

动作服务端

按住ctrl 点击create_server→Server

根据模板编写action服务端的创建代码

1
2
3
4
using GoalCallback = std::function<GoalResponse(
      const GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)>;
using CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
using AcceptedCallback = std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>;

test03_action_server.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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
/*
  需求:处理客户端发送的控制乌龟向目标点运动，且要连续反馈剩余距离
  步骤:
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建原生乌龟位姿订阅方，回调函数中获取乌龟位姿；
      3-2.创建原生乌龟速度发布方；
      3-3.创建动作服务端；
      3-4.解析动作客户端发送的请求；
      3-5.处理动作客户端发送的取消请求；
      3-6.创建新线程处理主逻辑请求；
      3-7.新线程产生连续反馈并响应最终结果。
    4.调用spin函数,并传入节点对象指针;
    5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "turtlesim/msg/pose.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces/action/nav.hpp"

using namespace std::placeholders;
using base_interfaces::action::Nav;

// 3.自定义节点类
class Test03ActionServer: public rclcpp::Node{ 
public:
    Test03ActionServer():Node("test03_action_server_node_cpp"),x(0.0),y(0.0){  // 节点名称一般小写
        RCLCPP_INFO(this->get_logger(),"动作服务端创建成功!");
        // 3-1.创建原生乌龟位姿订阅方，回调函数中获取乌龟位姿
        pose_sub_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose",10,std::bind(&Test03ActionServer::pose_cb,this,_1));
        // 3-2.创建原生乌龟速度发布方；
        cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel",10);
        // 3-3.创建动作服务端；
        /* 
        (NodeT node, 
        const std::string &name,
        rclcpp_action::Server<ActionT>::GoalCallback handle_goal, 
        rclcpp_action::Server<ActionT>::CancelCallback handle_cancel, 
        rclcpp_action::Server<ActionT>::AcceptedCallback handle_accepted
        */
        action_server_ = rclcpp_action::create_server<Nav>(
          this,
          "nav",
          std::bind(&Test03ActionServer::handle_goal,this,_1,_2),
          std::bind(&Test03ActionServer::handle_cancel,this,_1),
          std::bind(&Test03ActionServer::handle_accepted,this,_1));
          RCLCPP_INFO(this->get_logger(),"动作服务端创建，等待请求...");
    }
private:
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_sub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
    rclcpp_action::Server<Nav>::SharedPtr action_server_;
    float x,y;
    void pose_cb(const turtlesim::msg::Pose::SharedPtr pose){
      x = pose->x;
      y = pose->y;
    }
    // 3-4.解析动作客户端发送的请求；
    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const Nav::Goal> goal)
    {
      (void)uuid; // 标识目前没用上,避免警告
      // 判断目标的x,y值是否超出框
      float goal_x = goal->goal_x;
      float goal_y = goal->goal_y;
      if(goal_x < 0 || goal_x > 11.08 || goal_y < 0 || goal_y > 11.08)
      {
        RCLCPP_INFO(this->get_logger(),"无效目标值!");
        return rclcpp_action::GoalResponse::REJECT;
      }
      RCLCPP_INFO(this->get_logger(),"目标点合法!");
      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }
    rclcpp_action::CancelResponse handle_cancel(std::shared_ptr<rclcpp_action::ServerGoalHandle<Nav>> goal_handle)
    {
      (void)goal_handle;
      RCLCPP_INFO(this->get_logger(),"接收到取消请求!");
      return rclcpp_action::CancelResponse::ACCEPT;
    }
    // 主逻辑处理
    void execute(std::shared_ptr<rclcpp_action::ServerGoalHandle<Nav>> goal_handle)
    {
      RCLCPP_INFO(this->get_logger(),"开始执行任务......");
      // 创建连续反馈对象指针；
      auto feedback = std::make_shared<Nav::Feedback>();
      // 创建最终结果对象指针；
      auto result = std::make_shared<Nav::Result>();
      // 生成连续反馈
      rclcpp::Rate rate(1.0);
      while(true)
      {
        // 任务执行中，关于客户端发送取消请求的处理；
        if(goal_handle->is_canceling()){
          goal_handle->canceled(result);
          return;
        }
        // 解析目标点坐标与原生乌龟位姿数据；
        float goal_x = goal_handle->get_goal()->goal_x;
        float goal_y = goal_handle->get_goal()->goal_y;

        // 计算剩余距离并发布
        float distance_x = goal_x - x;
        float distance_y = goal_y - y;
        float distance = std::sqrt(distance_x * distance_x + distance_y * distance_y);
        feedback->distance = distance;
        goal_handle->publish_feedback(feedback);

        // 计算速度指令并发布
        float scale = 0.5;
        geometry_msgs::msg::Twist twist;
        twist.linear.x = scale * distance_x;
        twist.linear.y = scale * distance_y;
        cmd_vel_pub_->publish(twist);

        // 循环结束条件
        if(distance < 0.05)
        {
          RCLCPP_INFO(this->get_logger(),"导航至目标点！");
          break;
        }
        rate.sleep();
      }
      // 生成最终响应
      if(rclcpp::ok()){
        result->turtle_x = x;
        result->turtle_y = y;
        goal_handle->succeed(result);
      }
    }
    void handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<Nav>> goal_handle)
    {
      // 新建线程处理耗时的主逻辑
      std::thread{std::bind(&Test03ActionServer::execute,this,_1),goal_handle}.detach();
    }
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.调用spin函数,并传入节点类对象;
  rclcpp::spin(std::make_shared<Test03ActionServer>()); 
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

test03_action_client.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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
/*
  需求:向动作服务端发送目标点数据，并处理响应数据。
  步骤:
    0.解析launch文件传入的参数
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建动作客户端；
      3-2.发送请求数据，并处理服务端响应；
      3-3.处理目标响应；
      3-4.处理响应的连续反馈；
      3-5.处理最终响应。
    4.调用spin函数,并传入节点对象指针;
    5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces/action/nav.hpp"

using namespace std::placeholders;
using namespace std::chrono_literals;
using base_interfaces::action::Nav;

// 3.自定义节点类
class Test03ActionClient: public rclcpp::Node{ 
public:
    Test03ActionClient():Node("test03_action_client_node_cpp"){  // 节点名称一般小写
        RCLCPP_INFO(this->get_logger(),"动作客户端创建成功!");
      // 3-1.创建动作客户端；
      client_ = rclcpp_action::create_client<Nav>(this,"nav");
    }
    // 3-2.发送请求数据，并处理服务端响应；
    void send_goal(float x, float y, float theta){
      // 连接服务端
      if(!client_ -> wait_for_action_server(10s))
      {
        RCLCPP_INFO(this->get_logger(),"连接超时!");
      }
      // 组织发送数据
      Nav::Goal goal;
      goal.goal_x = x;
      goal.goal_y = y;
      goal.goal_theta = theta;
      rclcpp_action::Client<Nav>::SendGoalOptions options;
      options.goal_response_callback = std::bind(&Test03ActionClient::goal_response_callback,this,_1);
      options.feedback_callback = std::bind(&Test03ActionClient::feedback_callback,this,_1,_2);
      options.result_callback = std::bind(&Test03ActionClient::result_callback,this,_1);
      client_->async_send_goal(goal,options);
    }

private: 
    rclcpp_action::Client<Nav>::SharedPtr client_;
    // 将UUID转换为可读字符串
    std::string goal_uuid_to_string(const rclcpp_action::GoalUUID& uuid) {
      std::stringstream ss;
      for (auto byte : uuid) {
          ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte);
      }
      return ss.str();
    }
    // 3-3.处理目标响应；
    void goal_response_callback(rclcpp_action::ClientGoalHandle<Nav>::SharedPtr goal_handle){
      if(!goal_handle){
        RCLCPP_ERROR(this->get_logger(),"目标请求被服务端拒绝!");
        rclcpp::shutdown();
      } else{
        RCLCPP_INFO(this->get_logger(),"目标处理中!目标ID: %s",goal_uuid_to_string(goal_handle->get_goal_id()).c_str());
      }
    }
    // 3-4.处理响应的连续反馈；
    void feedback_callback(rclcpp_action::ClientGoalHandle<Nav>::SharedPtr goal_handle, const std::shared_ptr<const Nav::Feedback> feedback){
      (void)goal_handle;
      RCLCPP_INFO(this->get_logger(),"剩余距离:%.2f",feedback->distance);
    }
    // 3-5.处理最终响应。
    void result_callback(const rclcpp_action::ClientGoalHandle<Nav>::WrappedResult & result){
      // 通过状态码判断结果状态
      if(result.code == rclcpp_action::ResultCode::SUCCEEDED)
      {
        RCLCPP_INFO(this->get_logger(),
          "乌龟最终坐标:(%.2f,%.2f),航向:%.2f",
          result.result->turtle_x,
          result.result->turtle_y,
          result.result->turtle_theta);
      } else if(result.code == rclcpp_action::ResultCode::ABORTED)
      {
        RCLCPP_INFO(this->get_logger(),"任务被中止!");

      } else if(result.code == rclcpp_action::ResultCode::CANCELED){
        RCLCPP_INFO(this->get_logger(),"任务被取消!");

      } else{
        RCLCPP_INFO(this->get_logger(),"未知异常!");
      }
      rclcpp::shutdown();
    }
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  if (argc != 5)
  {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"请传入合法的目标位姿参数:(x,y,theta)");
    return 1;
  }
  rclcpp::init(argc,argv);
  // 不能直接spin
  auto client = std::make_shared<Test03ActionClient>();
  client->send_goal(atof(argv[1]),atof(argv[2]),atof(argv[3]));
  // 4.调用spin函数,并传入节点类对象;
  rclcpp::spin(client); 
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

参数服务案例分析

需求：动态修改乌龟窗口的背景颜色

配置自配

launch文件

1
2
3
4
5
6
7
8
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    t = Node(package="turtlesim",executable="turtlesim_node")
    param = Node(package="cpp07_exercise",executable="test04_param")

    return LaunchDescription([t,param])

test04_param.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
/*
  需求: 修改turtlesim_node的背景颜色
  步骤:
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建参数客户端；
      3-2.连接参数服务端；
      3-3.更新参数。
    4.创建对象指针,并调用其函数;
    5.释放资源。
*/

// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"

using namespace std::chrono_literals;

// 3.自定义节点类
class Test04Param: public rclcpp::Node{ 
public:
    Test04Param():Node("test04_param_node_cpp"){  // 节点名称一般小写
        RCLCPP_INFO(this->get_logger(),"参数服务器创建成功!");
        // 3-1.创建参数客户端
        client_ = std::make_shared<rclcpp::SyncParametersClient>(this,"/turtlesim");
    }
    // 3-2.连接参数服务端
    bool connect_server(){
        while(!client_->wait_for_service(5s)){
            if(!rclcpp::ok())
            {
              RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"强制退出！");
            }
            RCLCPP_INFO(this->get_logger(),"服务连接中...");
        }
      return true;
    }
    // 3-3.更新参数
    void update_param(){
        // 背景色渐变修改
        // background_r[0,255]
        // 获取参数
        red = client_->get_parameter<int32_t>("background_r");
        rclcpp::Rate rate(30.0);
        // 编写循环，修改参数
        int count = red;
        while(rclcpp::ok())
        {
          /* 
            计数器在[0,255)递增，[255,510]递减，大于等于510时归0
          */
          count < 255 ? red +=  5 : red -= 5;
          count += 5;
          if(count>=510) count = 0;

          // 修改服务器参数
          client_->set_parameters({rclcpp::Parameter("background_r",red)});
          RCLCPP_INFO(this->get_logger(),"red = %d", red);
          rate.sleep();
        }
    }
private:
    rclcpp::SyncParametersClient::SharedPtr client_;
    int32_t red;
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);
  // 4.创建对象指针,并调用其函数
  auto client = std::make_shared<Test04Param>();
  // 判断连接状态
  if(!client->connect_server())
  {
    return 1;
  }
  client->update_param();
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}