ROS2 Humble 学习【openEuler】

1 介绍

1.1 概述

ROS（Robot Operating System，下文简称“ROS”）是一个适用于机器人的开源的元操作系统。它提供了操作系统应有的服务，包括硬件抽象，底层设备控制，常用函数的实现，进程间消息传递，以及包管理。它也提供用于获取、编译、编写、和跨计算机运行代码所需的工具和库函数。

ROS 2 Documentation humble
小鱼–动手学ROS2
机器人操作系统入门讲义
本文在 openEuler 22.03 虚拟机中按照 ROS2 Humble 官方教程、小鱼文章大纲、机器人操作系统入门讲义 进行学习调试记录

1.2 ROS2 详细介绍

ROS2 详细介绍

1.3 openEuler 安装 ROS2 Humble

openEuler 安装 ROS2 Humble

1.4 ROS2 系统架构

• 抽象DDS层-RMW（ROS Middleware Interface）
• RCL（ROS Client Library）ROS客户端库，其实就是ROS的一种API，提供了对ROS话题、服务、参数、Action等接口。
  

2 ROS2 基础

2.1 节点编写、编译、运行【简单示例】

ROS2的C++节点

节点编写

• 创建文件 ros2_node.cpp

touch ros2_node.cpp

• 编写代码

#include "rclcpp/rclcpp.hpp"

int main(int argc, char **argv)
{
    // 调用rclcpp的初始化函数
    rclcpp::init(argc, argv);
    // 调用rclcpp的循环运行我们创建的first_node节点
    rclcpp::spin(std::make_shared<rclcpp::Node>("first_node"));
    return 0;
}

节点编译 g++

• 编译报错

[euler@Euler node]$ ls
ros2_node.cpp
[euler@Euler node]$ g++ ros2_node.cpp 
ros2_node.cpp:1:10: 致命错误：rclcpp/rclcpp.hpp：没有那个文件或目录
    1 | #include "rclcpp/rclcpp.hpp"
      |          ^~~~~~~~~~~~~~~~~~~
编译中断。
[euler@Euler node]$

• 定位头文件

/opt/ros/humble/include/rclcpp
ls rclcpp/* | grep rclcpp.h

[euler@Euler rclcpp]$ ls rclcpp/* | grep rclcpp.h
rclcpp/rclcpp.hpp
[euler@Euler rclcpp]$

• g++ 指定相应目录

g++ ros2_node.cpp -I /opt/ros/humble/include/rclcpp/

[euler@Euler node]$ g++ ros2_node.cpp -I /opt/ros/humble/include/rclcpp/
In file included from /opt/ros/humble/include/rclcpp/rclcpp/executors/multi_threaded_executor.hpp:25,
                 from /opt/ros/humble/include/rclcpp/rclcpp/executors.hpp:21,
                 from /opt/ros/humble/include/rclcpp/rclcpp/rclcpp.hpp:155,
                 from ros2_node.cpp:1:
/opt/ros/humble/include/rclcpp/rclcpp/executor.hpp:30:10: 致命错误：rcl/guard_condition.h：没有那个文件或目录
   30 | #include "rcl/guard_condition.h"
      |          ^~~~~~~~~~~~~~~~~~~~~~~
编译中断。
[euler@Euler node]$

• g++ 指令多层依赖目录

g++ ros2_node.cpp -lrclcpp -lrcutils -std=c++17 \
-I/opt/ros/humble/include/rclcpp/ \
-I /opt/ros/humble/include/rcl/ \
-I /opt/ros/humble/include/rcutils/ \
-I /opt/ros/humble/include/rmw \
-I /opt/ros/humble/include/rcl_yaml_param_parser/ \
-I /opt/ros/humble/include/rosidl_runtime_c \
-I /opt/ros/humble/include/rosidl_typesupport_interface \
-I /opt/ros/humble/include/rcpputils \
-I /opt/ros/humble/include/builtin_interfaces \
-I /opt/ros/humble/include/rosidl_runtime_cpp \
-I /opt/ros/humble/include/tracetools \
-I /opt/ros/humble/include/rcl_interfaces \
-I /opt/ros/humble/include/libstatistics_collector \
-I /opt/ros/humble/include/statistics_msgs \
-L /opt/ros/humble/lib/ \
-Wl,-rpath,/opt/ros/humble/lib/

-Wl: 这个前缀告诉编译器，接下来的选项是传递给链接器（ld）的。
-rpath: 这个选项指定了运行时库的搜索路径。

节点运行

打开新的终端，使用ros2 node list查看正在运行的节点，是否有first_node。

source /opt/ros/humble/setup.bash

节点编译 make

• 安装 make
• 编写 Makefile
  新建Makefile

build:
	g++ ros2_node.cpp -lrclcpp -lrcutils -std=c++17 \
	-I/opt/ros/humble/include/rclcpp/ \
	-I /opt/ros/humble/include/rcl/ \
	-I /opt/ros/humble/include/rcutils/ \
	-I /opt/ros/humble/include/rmw \
	-I /opt/ros/humble/include/rcl_yaml_param_parser/ \
	-I /opt/ros/humble/include/rosidl_runtime_c \
	-I /opt/ros/humble/include/rosidl_typesupport_interface \
	-I /opt/ros/humble/include/rcpputils \
	-I /opt/ros/humble/include/builtin_interfaces \
	-I /opt/ros/humble/include/rosidl_runtime_cpp \
	-I /opt/ros/humble/include/tracetools \
	-I /opt/ros/humble/include/rcl_interfaces \
	-I /opt/ros/humble/include/libstatistics_collector \
	-I /opt/ros/humble/include/statistics_msgs \
	-L /opt/ros/humble/lib/ \
	-Wl,-rpath,/opt/ros/humble/lib/

clean:
	rm a.out

euler@Euler node]$ ls
Makefile  ros2_node.cpp
[euler@Euler node]$ make build

[euler@Euler node]$ ls
a.out  Makefile  ros2_node.cpp
[euler@Euler node]$ make clean
rm a.out
[euler@Euler node]$

节点编译 CMakeLists.txt

使用 make 调用 Makefile，需要自己手写 Makefile，而 cmake 通过调用 CMakeLists.txt 可以直接生成 Makefile。

• 新建CMakeLists.txt

touch CMakeLists.txt

• 编写

cmake_minimum_required(VERSION 3.22)

project(first_node)

#include_directories 添加特定的头文件搜索路径 ，相当于指定g++编译器的-I参数
include_directories(/opt/ros/humble/include/rclcpp/)
include_directories(/opt/ros/humble/include/rcl/)
include_directories(/opt/ros/humble/include/rcutils/)
include_directories(/opt/ros/humble/include/rcl_yaml_param_parser/)
include_directories(/opt/ros/humble/include/rosidl_runtime_c/)
include_directories(/opt/ros/humble/include/rosidl_typesupport_interface/)
include_directories(/opt/ros/humble/include/rcpputils/)
include_directories(/opt/ros/humble/include/builtin_interfaces/)
include_directories(/opt/ros/humble/include/rmw/)
include_directories(/opt/ros/humble/include/rosidl_runtime_cpp/)
include_directories(/opt/ros/humble/include/tracetools/)
include_directories(/opt/ros/humble/include/rcl_interfaces/)
include_directories(/opt/ros/humble/include/libstatistics_collector/)
include_directories(/opt/ros/humble/include/statistics_msgs/)

# link_directories - 向工程添加多个特定的库文件搜索路径，相当于指定g++编译器的-L参数
link_directories(/opt/ros/humble/lib/)

# add_executable - 生成first_node可执行文件
add_executable(first_node ros2_node.cpp)

# target_link_libraries - 为first_node(目标) 添加需要动态链接库，相同于指定g++编译器-l参数
# 下面的语句代替 -lrclcpp -lrcutils
target_link_libraries(first_node rclcpp rcutils)

# c++17
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")

[euler@Euler node]$ ls
CMakeLists.txt  ros2_node.cpp
[euler@Euler node]$ mkdir build
[euler@Euler node]$ ls
build  CMakeLists.txt  ros2_node.cpp
[euler@Euler node]$ cd build/
[euler@Euler build]$ ls
[euler@Euler build]$ cmake ..
[euler@Euler build]$ ls
CMakeCache.txt  CMakeFiles  cmake_install.cmake  Makefile
[euler@Euler build]$ make 
[ 50%] Building CXX object CMakeFiles/first_node.dir/ros2_node.cpp.o
[100%] Linking CXX executable first_node
[100%] Built target first_node
[euler@Euler build]$ ls
CMakeCache.txt  CMakeFiles  cmake_install.cmake  first_node  Makefile
[euler@Euler build]$

CMake依赖查找流程

用cmake虽然成功，但是CMakeLists.txt的内容有些臃肿，需要简化。

• CMakeLists.txt 优化如下

cmake_minimum_required(VERSION 3.22)
project(first_node)

find_package(rclcpp REQUIRED)
add_executable(first_node ros2_node.cpp)
target_link_libraries(first_node rclcpp::rclcpp)

• 加载 ros 环境

source /opt/ros/humble/setup.bash

• 生成和编译

mkdir build
cd build
cmake ..
make

[euler@Euler build]$ cmake ..
-- Found rclcpp: 16.0.4 (/opt/ros/humble/share/rclcpp/cmake)
-- Found rosidl_generator_c: 3.1.4 (/opt/ros/humble/share/rosidl_generator_c/cmake)
-- Found rosidl_adapter: 3.1.4 (/opt/ros/humble/share/rosidl_adapter/cmake)
-- Found rosidl_generator_cpp: 3.1.4 (/opt/ros/humble/share/rosidl_generator_cpp/cmake)
-- Using all available rosidl_typesupport_c: rosidl_typesupport_fastrtps_c;rosidl_typesupport_introspection_c
-- Using all available rosidl_typesupport_cpp: rosidl_typesupport_fastrtps_cpp;rosidl_typesupport_introspection_cpp
CMake Warning at /opt/ros/humble/share/libyaml_vendor/cmake/ament_cmake_export_libraries-extras.cmake:116 (message):
  Package 'libyaml_vendor' exports library 'yaml' which couldn't be found
Call Stack (most recent call first):
  /opt/ros/humble/share/libyaml_vendor/cmake/libyaml_vendorConfig.cmake:41 (include)
  /opt/ros/humble/share/rcl_yaml_param_parser/cmake/ament_cmake_export_dependencies-extras.cmake:21 (find_package)
  /opt/ros/humble/share/rcl_yaml_param_parser/cmake/rcl_yaml_param_parserConfig.cmake:41 (include)
  /opt/ros/humble/share/rcl/cmake/ament_cmake_export_dependencies-extras.cmake:21 (find_package)
  /opt/ros/humble/share/rcl/cmake/rclConfig.cmake:41 (include)
  /opt/ros/humble/share/libstatistics_collector/cmake/ament_cmake_export_dependencies-extras.cmake:21 (find_package)
  /opt/ros/humble/share/libstatistics_collector/cmake/libstatistics_collectorConfig.cmake:41 (include)
  /opt/ros/humble/share/rclcpp/cmake/ament_cmake_export_dependencies-extras.cmake:21 (find_package)
  /opt/ros/humble/share/rclcpp/cmake/rclcppConfig.cmake:41 (include)
  CMakeLists.txt:4 (find_package)


-- Found rmw_implementation_cmake: 6.1.1 (/opt/ros/humble/share/rmw_implementation_cmake/cmake)
-- Found rmw_fastrtps_cpp: 6.2.2 (/opt/ros/humble/share/rmw_fastrtps_cpp/cmake)
-- Using RMW implementation 'rmw_fastrtps_cpp' as default
-- Configuring done
-- Generating done
-- Build files have been written to: /home/euler/worthsen/node/build
[euler@Euler build]$

• find_package查找路径对应的环境变量如下。

<package>_DIR
CMAKE_PREFIX_PATH
CMAKE_FRAMEWORK_PATH
CMAKE_APPBUNDLE_PATH
PATH

• echo $PATH

[euler@Euler build]$ echo $PATH
/opt/ros/humble/bin:/usr/share/Modules/bin:/usr/local/bin:/usr/bin:/bin:/usr/local/sbin:/usr/sbin
[euler@Euler build]$

PATH中的路径如果以bin或sbin结尾，则自动回退到上一级目录，接着检查这些目录下的

<prefix>/(lib/<arch>|lib|share)/cmake/<name>*/          (U)
<prefix>/(lib/<arch>|lib|share)/<name>*/                (U)
<prefix>/(lib/<arch>|lib|share)/<name>*/(cmake|CMake)/  (U)

cmake找到这些目录后，会开始依次找Config.cmake或Find.cmake文件。找到后即可执行该文件并生成相关链接信息。

打开 /opt/ros/humble/share/rclcpp/cmake 会发现 rclcppConfig.cmake 就在其中。

Python 依赖查找流程

python 打包和引入依赖，比C++简单很多。

• 编写 ROS2 的 Python 节点

touch ros2_node.py

vim ros2_node.py 输入如下内容

# 导入rclpy库
import rclpy
from rclpy.node import Node
# 调用rclcpp的初始化函数
rclpy.init() 
# 调用rclcpp的循环运行我们创建的second_node节点
rclpy.spin(Node("second_node"))

• 运行

python3 ros2_node.py

• 查看

[euler@Euler ~]$ source /opt/ros/humble/setup.bash
[euler@Euler ~]$ ros2 node list
/second_node
[euler@Euler ~]$

• Python 包查找流程
  Python3 运行 import rclpy 是通过环境变量 PYTHONPATH 查找到的

echo $PYTHONPATH

[euler@Euler node]$ echo $PYTHONPATH
/opt/ros/humble/lib/python3.9/site-packages
[euler@Euler node]$ ls -l /opt/ros/humble/lib/python3.9/site-packages | grep rclpy
drwxr-xr-x.  3 root root  4096  7月 26 14:27 examples_rclpy_executors
drwxr-xr-x.  2 root root  4096  7月 26 14:27 examples_rclpy_executors-0.15.1-py3.9.egg-info
drwxr-xr-x.  3 root root  4096  7月 26 14:27 examples_rclpy_guard_conditions
drwxr-xr-x.  2 root root  4096  7月 26 14:27 examples_rclpy_guard_conditions-0.15.1-py3.9.egg-info
drwxr-xr-x.  3 root root  4096  7月 26 14:27 examples_rclpy_minimal_action_client
drwxr-xr-x.  2 root root  4096  7月 26 14:27 examples_rclpy_minimal_action_client-0.15.1-py3.9.egg-info
drwxr-xr-x.  3 root root  4096  7月 26 14:27 examples_rclpy_minimal_action_server
drwxr-xr-x.  2 root root  4096  7月 26 14:27 examples_rclpy_minimal_action_server-0.15.1-py3.9.egg-info
drwxr-xr-x.  3 root root  4096  7月 26 14:27 examples_rclpy_minimal_client
drwxr-xr-x.  2 root root  4096  7月 26 14:27 examples_rclpy_minimal_client-0.15.1-py3.9.egg-info
drwxr-xr-x.  3 root root  4096  7月 26 14:27 examples_rclpy_minimal_publisher
drwxr-xr-x.  2 root root  4096  7月 26 14:27 examples_rclpy_minimal_publisher-0.15.1-py3.9.egg-info
drwxr-xr-x.  3 root root  4096  7月 26 14:27 examples_rclpy_minimal_service
drwxr-xr-x.  2 root root  4096  7月 26 14:27 examples_rclpy_minimal_service-0.15.1-py3.9.egg-info
drwxr-xr-x.  3 root root  4096  7月 26 14:27 examples_rclpy_minimal_subscriber
drwxr-xr-x.  2 root root  4096  7月 26 14:27 examples_rclpy_minimal_subscriber-0.15.1-py3.9.egg-info
drwxr-xr-x.  3 root root  4096  7月 26 14:27 examples_rclpy_pointcloud_publisher
drwxr-xr-x.  2 root root  4096  7月 26 14:27 examples_rclpy_pointcloud_publisher-0.15.1-py3.9.egg-info
drwxr-xr-x.  5 root root  4096  7月 26 14:27 rclpy
drwxr-xr-x.  2 root root  4096  7月 26 14:27 rclpy-3.3.8-py3.9.egg-info
drwxr-xr-x.  3 root root  4096  7月 26 14:27 rclpy_message_converter
drwxr-xr-x.  2 root root  4096  7月 26 14:27 rclpy_message_converter-2.0.1-py3.9.egg-info
drwxr-xr-x.  4 root root  4096  7月 26 14:33 rclpy_message_converter_msgs
drwxr-xr-x.  2 root root  4096  7月 26 14:33 rclpy_message_converter_msgs-2.0.1-py3.9.egg-info
[euler@Euler node]$

2.2 节点交互、启动、查看【命令】

• ROS2 四种通信方式：
  1）话题-topics
  2）服务-services
  3）动作-Action
  4）参数-parameters

• 启动节点命令

# 启动 包 中的节点
ros2 run <package_name> <executable_name>

eg: ros2 run turtlesim turtlesim_node

• 查看节点

ros2 node list

• 查看节点信息

ros2 node info <node_name>

[euler@Euler ~]$ ros2 node info /first_node
/first_node
  Subscribers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
  Publishers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
  Service Servers:
    /first_node/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /first_node/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /first_node/get_parameters: rcl_interfaces/srv/GetParameters
    /first_node/list_parameters: rcl_interfaces/srv/ListParameters
    /first_node/set_parameters: rcl_interfaces/srv/SetParameters
    /first_node/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:

  Action Clients:

• 重映射节点名称

ros2 run turtlesim turtlesim_node --ros-args --remap __node:=my_turtle

• 运行节点时设置参数

ros2 run example_parameters_rclcpp parameters_basic --ros-args -p rcl_log_level:=10

2.3 ROS2 工作空间、功能包、构建工具 Colcon

工作空间

一个工作空间下可以有多个功能包，一个功能包可以有多个节点存在。

eg: 创建 ros2 工作区，其中包含 src 文件夹
mkdir worthsen
cd worthsen
mkdir -p ros2/src

功能包

功能包中存放节点，ROS2 中功能包根据编译方式的不同分为三种类型。
（1）ament_python，适用于python程序
（2）cmake，适用于C++
（3）ament_cmake，适用于C++程序,是cmake的增强版

功能包获取【安装】

sudo yum install ros-<version>-package_name

安装获取会自动放置到系统目录，不用再次手动source。

功能包获取【手动编译】

手动编译之后，需要手动source工作空间的install目录。

功能包指令 ros2 pkg

create       Create a new ROS2 package
executables  Output a list of package specific executables
list         Output a list of available packages
prefix       Output the prefix path of a package
xml          Output the XML of the package manifest or a specific tag

• 创建功能包

ros2 pkg create <package-name>  --build-type  {cmake,ament_cmake,ament_python}  --dependencies <依赖名字>

• 列出可执行文件

# 列出所有
ros2 pkg executables

# 列出turtlesim功能包的所有可执行文件
ros2 pkg executables turtlesim

• 列出所有的包

ros2 pkg list

• 输出某个包所在路径的前缀

ros2 pkg prefix  <package-name>

eg: 比如小乌龟
ros2 pkg prefix turtlesim

[euler@Euler ~]$ source /opt/ros/humble/setup.bash 
[euler@Euler ~]$ ros2 pkg prefix turtlesim
/opt/ros/humble
[euler@Euler ~]$

• 列出包的清单描述文件
  每一个功能包都有一个标配的manifest.xml文件，用于记录这个包的名字，构建工具，编译信息，拥有者，干啥用的等信息。
  通过这个信息，就可以自动为该功能包安装依赖，构建时确定编译顺序等

ros2 pkg xml turtlesim

[euler@Euler ~]$ ros2 pkg xml turtlesim
<package format="3">
  <name>turtlesim</name>
  <version>1.4.2</version>
  <description>
    turtlesim is a tool made for teaching ROS and ROS packages.
  </description>

  <maintainer email="audrow@openrobotics.org">Audrow Nash</maintainer>
  <maintainer email="michael.jeronimo@openrobotics.org">Michael Jeronimo</maintainer>

  <license>BSD</license>

  <url type="website">http://www.ros.org/wiki/turtlesim</url>
  <url type="bugtracker">https://github.com/ros/ros_tutorials/issues</url>
  <url type="repository">https://github.com/ros/ros_tutorials</url>

  <author email="dthomas@osrfoundation.org">Dirk Thomas</author>
  <author>Josh Faust</author>
  <author email="mabel@openrobotics.org">Mabel Zhang</author>
  <author email="sloretz@openrobotics.org">Shane Loretz</author>

  <build_depend>qt5-qmake</build_depend>
  <build_depend>qtbase5-dev</build_depend>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>rosidl_default_generators</buildtool_depend>

  <exec_depend>libqt5-core</exec_depend>
  <exec_depend>libqt5-gui</exec_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>

  <depend>ament_index_cpp</depend>
  <depend>geometry_msgs</depend>
  <depend>rclcpp</depend>
  <depend>rclcpp_action</depend>
  <depend>std_msgs</depend>
  <depend>std_srvs</depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>
[euler@Euler ~]$

构建工具 Colcon 介绍 & 安装

ROS2 默认是没有安装 colcon 的，colcon 想当于 ros1 中的 catkin 工具。
colcon是一系列python脚本，是对cmake等编译工具的封装。

• 安装

# Using pip on any platform
# 普通用户权限安装，避障权限问题
pip install -U colcon-common-extensions

openEuler 安装 ROS2 Humble【编译系统】

• 克隆示例代码

[euler@Euler colcon]$ pwd
/home/euler/worthsen/node/colcon
[euler@Euler colcon]$ git clone https://github.com/ros2/examples src/examples -b humble
[euler@Euler colcon]$ tree -L 4
.
└── src
    └── examples
        ├── CONTRIBUTING.md
        ├── launch_testing
        │   └── launch_testing_examples
        ├── LICENSE
        ├── rclcpp
        │   ├── actions
        │   ├── composition
        │   ├── executors
        │   ├── README.md
        │   ├── services
        │   ├── timers
        │   ├── topics
        │   └── wait_set
        ├── rclpy
        │   ├── actions
        │   ├── executors
        │   ├── guard_conditions
        │   ├── pytest.ini
        │   ├── services
        │   └── topics
        └── README.md

18 directories, 5 files
[euler@Euler colcon]$

• 编译

colcon build

eg：
[euler@Euler colcon]$ source /opt/ros/humble/setup.bash 
[euler@Euler colcon]$ colcon build
...
Summary: 22 packages finished [5min 2s]
[euler@Euler colcon]$ tree -L 1
.
├── build
├── install
├── log
└── src

4 directories, 0 files

• 目录
  （1）build 目录存储的是中间文件。对于每个包，将创建一个子文件夹，在其中调用例如CMake
  （2）install 目录是每个软件包将安装到的位置。默认情况下，每个包都将安装到单独的子目录中。
  （3）log 目录包含有关每个colcon调用的各种日志信息。

• 终端 1 运行订阅者节点

# source 一下资源
source install/setup.bash
# 运行一个订阅节点
ros2 run examples_rclcpp_minimal_subscriber subscriber_member_function

• 终端 2 运行发布者节点

# source 一下资源
source install/setup.bash
# 运行一个订阅节点
ros2 run examples_rclcpp_minimal_publisher publisher_member_function

2.4 编写节点

编写节点【RCLCPP】

创建工作空间

[euler@Euler node]$ mkdir -p cpp_ws/src

创建功能包

创建example_cpp功能包，使用ament-cmake作为编译类型，并为其添加rclcpp依赖。

[euler@Euler node]$ cd cpp_ws/src/
[euler@Euler src]$ ros2 pkg create example_cpp --build-type ament_cmake --dependencies rclcpp
[euler@Euler src]$ tree -L 3
.
└── example_cpp
    ├── CMakeLists.txt
    ├── include
    │   └── example_cpp
    ├── package.xml
    └── src

4 directories, 2 files

（1）pkg create 是创建包的意思
（2）–build-type 用来指定该包的编译类型，一共有三个可选项 ament_python、ament_cmake、cmake
（3）–dependencies 指的是这个功能包的依赖，这里使用 ros2 的 C++ 客户端接口 rclcpp

创建节点

在example_cpp/src下创建一个node_01.cpp文件：

[euler@Euler src]$ tree -L 3
.
└── example_cpp
    ├── CMakeLists.txt
    ├── include
    │   └── example_cpp
    ├── package.xml
    └── src
        └── node_01.cpp

4 directories, 3 files

编写代码

• node_01.cpp 输入代码

#include "rclcpp/rclcpp.hpp"


int main(int argc, char **argv)
{
    /* 初始化rclcpp  */
    rclcpp::init(argc, argv);
    /*产生一个node_01的节点*/
    auto node = std::make_shared<rclcpp::Node>("node_01");
    // 打印一句自我介绍
    RCLCPP_INFO(node->get_logger(), "node_01节点已经启动.");
    /* 运行节点，并检测退出信号 Ctrl+C*/
    rclcpp::spin(node);
    /* 停止运行 */
    rclcpp::shutdown();
    return 0;
}

• 修改 CMakeLists.txt
  在CmakeLists.txt最后一行加入下面代码。

# 添加可执行文件
add_executable(node_01 src/node_01.cpp)
# 添加依赖项
ament_target_dependencies(node_01 rclcpp)

# 添加安装目标
install(TARGETS
  node_01
  DESTINATION lib/${PROJECT_NAME}
)

编译运行

[euler@Euler cpp_ws]$ ls
src
[euler@Euler cpp_ws]$ colcon build
Starting >>> example_cpp
Finished <<< example_cpp [8.13s]                     

Summary: 1 package finished [8.52s]
[euler@Euler cpp_ws]$ tree -L 1
.
├── build
├── install
├── log
└── src

4 directories, 0 files

测试

[euler@Euler cpp_ws]$ source install/setup.bash
[euler@Euler cpp_ws]$ ros2 run example_cpp node_01
[INFO] [1722828608.116903186] [node_01]: node_01节点已经启动.

新开窗口查看节点列表：

[euler@Euler ~]$ source /opt/ros/humble/setup.bash 
[euler@Euler ~]$ ros2 node list
/node_01
[euler@Euler ~]$

编写节点【RCLPY】

创建工作空间

[euler@Euler node]$ mkdir -p py_ws/src

创建功能包

创建example_py功能包，使用ament-cmake作为编译类型，并为其添加rclpy依赖。

[euler@Euler node]$ cd cpp_ws/src/
[euler@Euler src]$ ros2 pkg create example_py --build-type ament_python --dependencies rclpy
[euler@Euler src]$ tree -L 3
.
└── example_py
    ├── example_py
    │   └── __init__.py
    ├── package.xml
    ├── resource
    │   └── example_py
    ├── setup.cfg
    ├── setup.py
    └── test
        ├── test_copyright.py
        ├── test_flake8.py
        └── test_pep257.py

4 directories, 8 files

（1）pkg create 是创建包的意思
（2）–build-type 用来指定该包的编译类型，一共有三个可选项 ament_python、ament_cmake、cmake
（3）–dependencies 指的是这个功能包的依赖，这里使用 ros2 的 python 客户端接口 rclpy

创建节点

example_py下创建 node_02.py

[euler@Euler py_ws]$ ls
src
[euler@Euler py_ws]$ touch src/example_py/example_py/node_02.py

编写代码

• node_02.cpp 输入代码

import rclpy
from rclpy.node import Node

def main(args=None):
    """
    ros2运行该节点的入口函数
    编写ROS2节点的一般步骤
    1. 导入库文件
    2. 初始化客户端库
    3. 新建节点对象
    4. spin循环节点
    5. 关闭客户端库
    """
    rclpy.init(args=args) # 初始化rclpy
    node = Node("node_02")  # 新建一个节点
    node.get_logger().info("大家好，我是node_02.")
    rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
    rclpy.shutdown() # 关闭rclpy

• 修改setup.py
  声明一个ROS2的节点，声明后使用colcon build才能检测到，从而将其添加到install目录下。

    entry_points={
        'console_scripts': [
            "node_02 = example_py.node_02:main"
        ],
    },
)

编译运行

[euler@Euler py_ws]$ ls
src
[euler@Euler py_ws]$ colcon build
Starting >>> example_py
Finished <<< example_py [1.56s]          

Summary: 1 package finished [1.92s]
[euler@Euler py_ws]$ tree -L 1
.
├── build
├── install
├── log
└── src

4 directories, 0 files

测试

[euler@Euler py_ws]$ source install/setup.bash
[euler@Euler py_ws]$ ros2 run example_py node_02
[INFO] [1722833143.392257307] [node_02]: 大家好，我是node_02.

新开窗口查看节点列表：

[euler@Euler ~]$ source /opt/ros/humble/setup.bash 
[euler@Euler ~]$ ros2 node list
/node_02
[euler@Euler ~]$

2.5 节点编写、编译、运行【面向对象编程 OOP】

面向过程编程思想。缩写：POP
面向对象编程思想。缩写：OOP
函数式思想。缩写：FP

C++ 示例

#include "rclcpp/rclcpp.hpp"

/*
    创建一个类节点，名字叫做Node03,继承自Node.
*/
class Node03 : public rclcpp::Node
{

public:
    // 构造函数,有一个参数为节点名称
    Node03(std::string name) : Node(name)
    {
        // 打印一句
        RCLCPP_INFO(this->get_logger(), "大家好，我是%s.",name.c_str());
    }

private:
   
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    /*产生一个node_03的节点*/
    auto node = std::make_shared<Node03>("node_03");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

Python

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node


class Node04(Node):
    """
    创建一个Node04节点，并在初始化时输出一个话
    """
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("大家好，我是%s!" % name)


def main(args=None):
    rclpy.init(args=args) # 初始化rclpy
    node = Node04("node_04")  # 新建一个节点
    rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
    rclpy.shutdown() # 关闭rclpy

2.6 Colcon 介绍

colcon

colcon是一个命令行工具，用于改进构建，测试和使用多个软件包的工作流程。它自动化了流程，处理了订购并设置了使用软件包的环境。

catkin_make

该工具仅调用 CMake 一次，并使用 CMake 的函数在单个上下文中处理所有包。虽然这是一种有效的方法，因为所有包中的所有目标都可以并行化，但它具有明显的缺点。由于所有函数名称、目标和测试都共享一个命名空间，并且规模更大，这很容易导致冲突。

ament_tools

ament_tools 由用于构建 ROS 2 包的独立 Python 3 包提供。它是为引导ROS 2项目而开发的，因此仅针对Python 3，并且可以在Linux，MacOS和Windows上运行。

构建指令

–packages-select ，仅生成单个包（或选定的包）。
–packages-up-to，构建选定的包，包括其依赖项。
–packages-above，整个工作区，然后对其中一个包进行了更改。此指令将重构此包以及（递归地）依赖于此包的所有包。

指定构建后安装的目录

可以通过 --build-base参数和–install-base，指定构建目录和安装目录。

合并构建目录

–merge-install，使用 作为所有软件包的安装前缀，而不是安装基中的软件包特定子目录。–install-base
如果没有此选项，每个包都将提供自己的环境变量路径，从而导致非常长的环境变量值。
使用此选项时，添加到环境变量的大多数路径将相同，从而导致环境变量值更短。

符号链接安装

启用–symlink-install后将不会把文拷贝到install目录，而是通过创建符号链接的方式。

错误时继续安装

启用–continue-on-error，当发生错误的时候继续进行编译。

CMake参数

–cmake-args，将任意参数传递给CMake。与其他选项匹配的参数必须以空格为前缀。

控制构建线程

• –executor EXECUTOR，用于处理所有作业的执行程序。默认值是根据所有可用执行程序扩展的优先级选择的。要查看完整列表，请调用 colcon extensions colcon_core.executor --verbose。

• • sequential [colcon-core]
    一次处理一个包。
• • parallel [colcon-parallel-executor]
    处理多个作业平行.

• –parallel-workers NUMBER

• • 要并行处理的最大作业数。默认值为 os.cpu_count() 给出的逻辑 CPU 内核数。

开启构建日志

使用–log-level可以设置日志级别，比如–log-level info。

2.7 ROS2节点发现与多机通信

在没有 ROSMASTER 的情况下，ROS2 如何实现互相发现。

ROS 2用于通讯的默认中间件是DDS。在DDS中，不同逻辑网络共享物理网络的主要机制称为域(Domain) ID。同一域上的ROS 2节点可以自由地相互发现并发送消息，而不同域上的ROS 2节点则不能。所有ROS 2节点默认使用域ID为0。

选择域ID (短版本)

选择一个介于0和101之间的安全的域ID (包括0和101)。

选择域ID (长版本)

DDS使用域ID计算将用于发现和通讯的UDP端口。在网络中，UDP端口是 无符号16位整型 。因此可以分配的最大端口号是65535。

• Linux
  默认情况下，linux内核使用端口32768-60999作为临时端口。这意味着域ID 0-101 和 215-232 可以安全使用，而不会与临时端口发生冲突。临时端口范围可在Linux中通过在 /proc/sys/net/ipv4/ip_local_port_range 中设置自定义值进行配置。如果使用自定义临时端口范围，则可能需要相应地调整上述数字。

• Windows
  默认情况下，Windows上的临时端口范围为49152-65535。这意味着域ID 0-166可以安全使用，不会与临时端口发生冲突。临时的端口范围可通过 使用netsh 在窗口中配置。如果使用自定义临时端口范围，则可能需要相应地调整上述数字。

对于计算机上运行的每个ROS 2进程，将创建一个DDS “participant” 。由于每个DDS参与者占用计算机上的两个端口，因此在一台计算机上运行120个以上的 ROS 2 进程可能会溢出到其他域ID或临时端口。

2.8 通信–话题与服务

TCP/UDP

ping 192.168.0.1

# 终端 1： 监听端口
nc -l 1234

# 终端 2： 发送数据
echo "Hello, TCP!" | nc 127.0.0.1 1234

基于共享内存的进程间通信（IPC）方式

在Linux命令行中，可以使用ipcs和ipcrm命令来管理共享内存段。

通过ipcs命令查看当前系统中的共享内存段：

ipcs -m

使用ipcrm命令删除不再需要的共享内存段：

ipcrm -m <shmid>

ZeroMQ

FastDDS 官网强调自己比 ZeroMQ 性能要好。
ZeroMQ 非常的轻量，也就是小巧，占用资源少，Zero Message Queue，零消息队列。

PyZmq

https://pyzmq.readthedocs.io/en/latest/

话题

同一个话题，所有的发布者和接收者必须使用相同消息接口。

# 终端 1
[euler@Euler cpp_ws]$ source install/setup.bash
[euler@Euler cpp_ws]$ ros2 run example_cpp node_01

# 终端 2
[euler@Euler py_ws]$ source install/setup.bash
[euler@Euler py_ws]$ ros2 run example_py node_02

# 终端 3
rqt_graph

openEuler 没有安装 rqt，安装后再补充。

命令

# 返回系统中当前活动的所有主题的列表
ros2 topic list

# 增加消息类型
ros2 topic list -t

# 打印实时话题内容
ros2 topic echo
eg: ros2 topic echo /chatter

# 查看主题信息
ros2 topic info
eg: ros2 topic info  /chatter

# 查看消息类型
ros2 interface show
eg: ros2 interface show std_msgs/msg/String

# 手动发布命令
ros2 topic pub arg
eg: ros2 topic pub /chatter std_msgs/msg/String 'data: "123"'

实现 RCLCPP

控制节点创建一个话题发布者，发布控制命令（command）话题，接口类型为字符串（string），控制接点通过发布者发布控制命令（前进、后退、左转、右转、停止）。

被控节点创建一个订阅者，订阅控制命令，收到控制命令后根据命令内容打印对应速度出来。

节点创建示例

• 工作空间 topic_ws
• 功能包 example_topic_rclcpp
• 文件 topic_publisher_01.cpp

[euler@Euler node]$ mkdir -p topic_ws/src
[euler@Euler node]$ cd topic_ws/src/
[euler@Euler src]$ source /opt/ros/humble/setup.bash 
[euler@Euler src]$ ros2 pkg create example_topic_rclcpp --build-type ament_cmake --dependencies rclcpp
[euler@Euler src]$ touch example_topic_rclcpp/src/topic_publisher_01.cpp

[euler@Euler topic_ws]$ tree -L 4
.
└── src
    └── example_topic_rclcpp
        ├── CMakeLists.txt
        ├── include
        │   └── example_topic_rclcpp
        ├── package.xml
        └── src
            └── topic_publisher_01.cpp

5 directories, 3 files

#include "rclcpp/rclcpp.hpp"

class TopicPublisher01 : public rclcpp::Node
{
public:
    // 构造函数,有一个参数为节点名称
    TopicPublisher01(std::string name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
    }

private:
    // 声明节点
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<TopicPublisher01>("topic_publisher_01");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

• CMakeLists.txt

add_executable(topic_publisher_01 src/topic_publisher_01.cpp)
ament_target_dependencies(topic_publisher_01 rclcpp)

install(TARGETS
  topic_publisher_01
  DESTINATION lib/${PROJECT_NAME}
)

• 运行

[euler@Euler topic_ws]$ colcon build --packages-select example_topic_rclcpp
Starting >>> example_topic_rclcpp
Finished <<< example_topic_rclcpp [8.60s]                     

Summary: 1 package finished [9.10s]
[euler@Euler topic_ws]$ source install/setup.sh 
[euler@Euler topic_ws]$ ros2 run example_topic_rclcpp topic_publisher_01

创建订阅节点

• 接口介绍
  创建发布者，需要调用node的成员函数create_publisher并传入对应的参数。
  （1）消息类型（msgT）
  （2）话题名称（topic_name）
  （3）服务指令（qos）
  
  
  通过消息接口，ROS2才能完成消息的序列化和反序列化

• 库导入
  ament_cmake类型功能包导入消息接口分为三步：
  （1）在CMakeLists.txt中导入，具体是先find_packages再ament_target_dependencies。
  （2）在packages.xml中导入，具体是添加depend标签并将消息接口写入。
  （3）在代码中导入，C++中是#include"消息功能包/xxx/xxx.hpp"。

• • CMakeLists.txt

find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

add_executable(topic_publisher_01 src/topic_publisher_01.cpp)
ament_target_dependencies(topic_publisher_01 rclcpp std_msgs)

• • packages.xml

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>

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

• • topic_publisher_01.cpp

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"	// 接口导入

class TopicPublisher01 : public rclcpp::Node

• 创建

• • 接口导入，std_msgs/msg/string.h。
• • 话题名称（topic_name），我们就用control_command。
• • Qos，Qos支持直接指定一个数字，这个数字对应的是KeepLast队列长度。一般设置成10，即如果一次性有100条消息，默认保留最新的10个，其余的都扔掉。

• 使用定时器定时发布数据 create_wall_timer
  period，回调函数调用周期。
  callback，回调函数。
  group，调用回调函数所在的回调组，默认为nullptr。
  
  

• 代码

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

class TopicPublisher01 : public rclcpp::Node
{
public:
    // 构造函数,有一个参数为节点名称
    TopicPublisher01(std::string name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "大家好，我是%s.", name.c_str());
        // 创建发布者
        command_publisher_ = this->create_publisher<std_msgs::msg::String>("command", 10);
        // 创建定时器，500ms为周期，定时发布
        timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&TopicPublisher01::timer_callback, this));
    }

private:
    void timer_callback()
    {
        // 创建消息
        std_msgs::msg::String message;
        message.data = "forward";
        // 日志打印
        RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
        // 发布消息
        command_publisher_->publish(message);
    }
    // 声名定时器指针
    rclcpp::TimerBase::SharedPtr timer_;
    // 声明话题发布者指针
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr command_publisher_;
};

• 发布消息
  std_msgs::msg::String是通过ROS2的消息文件自动生成的类，其原始消息文件内容可以通过命令行查询

ros2 interface show std_msgs/msg/String 

• 运行

# 查看列表
ros2 topic list
# 输出内容
ros2 topic echo /command

创建发布节点

• topic_subscribe_01.cpp
• CMakeLists.txt添加下std_msgs依赖

ament_target_dependencies(topic_subscribe_01 rclcpp std_msgs)

• packages.xml就不需要了，同一个功能包，已经添加了。
• 代码

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

class TopicSubscribe01 : public rclcpp::Node
{
public:
    TopicSubscribe01(std::string name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "大家好，我是%s.", name.c_str());
          // 创建一个订阅者订阅话题
        command_subscribe_ = this->create_subscription<std_msgs::msg::String>("command", 10, std::bind(&TopicSubscribe01::command_callback, this, std::placeholders::_1));
    }

private:
     // 声明一个订阅者
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr command_subscribe_;
     // 收到话题数据的回调函数
    void command_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        double speed = 0.0f;
        if(msg->data == "forward")
        {
            speed = 0.2f;
        }
        RCLCPP_INFO(this->get_logger(), "收到[%s]指令，发送速度 %f", msg->data.c_str(),speed);
    }
};

• 手动发布数据测试订阅者

ros2 topic pub /command std_msgs/msg/String "{data: forward}"

实现 RCLPY

略

服务

查看服务

# 终端 1 启动服务 
ros2 run examples_rclpy_minimal_service service
# 终端 2 查看服务
ros2 service list
# 终端 2 调用服务
ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 5,b: 10}"

# 查看服务类型
ros2 service type /add_two_ints

# 查找使用某一接口的服务
ros2 service find example_interfaces/srv/AddTwoInts

[euler@Euler node]$ mkdir -p service_ws/src
[euler@Euler node]$ cd service_ws/src/
[euler@Euler src]$ source /opt/ros/humble/setup.bash 
[euler@Euler src]$ ros2 pkg create example_service_rclcpp --build-type ament_cmake --dependencies rclcpp
[euler@Euler src]$ touch example_service_rclcpp/src/service_server_01.cpp
[euler@Euler src]$ touch example_service_rclcpp/src/service_client_01.cpp

• service_server_01.cpp

#include "example_interfaces/srv/add_two_ints.hpp"
#include "rclcpp/rclcpp.hpp"

class ServiceServer01 : public rclcpp::Node {
public:
  ServiceServer01(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
    // 创建服务
    add_ints_server_ =
      this->create_service<example_interfaces::srv::AddTwoInts>(
        "add_two_ints_srv",
        std::bind(&ServiceServer01::handle_add_two_ints, this,
                  std::placeholders::_1, std::placeholders::_2));
  }

private:
  // 声明一个服务
  rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr
    add_ints_server_;

  // 收到请求的处理函数
  void handle_add_two_ints(
    const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
    std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response) {
    RCLCPP_INFO(this->get_logger(), "收到a: %ld b: %ld", request->a,
                request->b);
    response->sum = request->a + request->b;
  };
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ServiceServer01>("service_server_01");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

• service_client_01.cpp

#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"

class ServiceClient01 : public rclcpp::Node {
public:
  // 构造函数,有一个参数为节点名称
  ServiceClient01(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
    // 创建客户端
    client_ = this->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints_srv");
  }

  void send_request(int a, int b) {
    RCLCPP_INFO(this->get_logger(), "计算%d+%d", a, b);

    // 1.等待服务端上线
    while (!client_->wait_for_service(std::chrono::seconds(1))) {
      //等待时检测rclcpp的状态
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(), "等待服务的过程中被打断...");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "等待服务端上线中");
    }

    // 2.构造请求的
    auto request =
      std::make_shared<example_interfaces::srv::AddTwoInts_Request>();
    request->a = a;
    request->b = b;

    // 3.发送异步请求，然后等待返回，返回时调用回调函数
    client_->async_send_request(
      request, std::bind(&ServiceClient01::result_callback_, this,
                         std::placeholders::_1));
  };

private:
  // 声明客户端
  rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client_;

  void result_callback_(
    rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture
      result_future) {
    auto response = result_future.get();
    RCLCPP_INFO(this->get_logger(), "计算结果：%ld", response->sum);
  }
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  /*创建对应节点的共享指针对象*/
  auto node = std::make_shared<ServiceClient01>("service_client_01");
  /* 运行节点，并检测退出信号*/
  //增加这一行，node->send_request(5, 6);，计算5+6结果
  node->send_request(5, 6);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

• CMakeLists.txt

# 把服务端和客户端对example_interfaces的依赖都加上
find_package(example_interfaces REQUIRED)

add_executable(service_client_01 src/service_client_01.cpp)
ament_target_dependencies(service_client_01 rclcpp example_interfaces)

add_executable(service_server_01 src/service_server_01.cpp)
ament_target_dependencies(service_server_01 rclcpp example_interfaces)

install(TARGETS
  service_server_01
  DESTINATION lib/${PROJECT_NAME}
)

install(TARGETS
  service_client_01
  DESTINATION lib/${PROJECT_NAME}
)

• packages.xml

<depend>example_interfaces</depend>

• 头文件

#include "example_interfaces/srv/add_two_ints.hpp"

• 查看接口定义

ros2 interface show example_interfaces/srv/AddTwoInts

• create_service
  

• • ServiceT，消息接口example_interfaces::srv::AddTwoInts
• • service_name，服务名称
• • callback，回调函数，使用成员函数作为回调函数，std::bind进行转换
• • qos_profile，服务质量配置文件，默认rmw_qos_profile_services_default
• • group，调用服务的回调组，默认nullptr

• 客户端不运行，可用命令行调用服务

ros2 service call /add_two_ints_srv example_interfaces/srv/AddTwoInts "{a: 5,b: 10}"

• create_client
  

• async_send_request
  
  request，请求的消息，这里用于放a，b两个数。
  CallBack，回调函数，异步接收服务器的返回的函数。

• wait_for_service
  这个函数是用于等待服务上线。
  
  
  参数：等待的时间
  返回值：bool类型，上线了 true，不上线 false。

• result_callback_
  回调函数void result_callback_(rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture result_future)
  
  std::shared_future创建的SharedResponse类模板。
  类模板 std::shared_future 提供访问异步操作结果的机制，类似 std::future ，除了允许多个线程等候同一共享状态。
  https://en.cppreference.com/w/cpp/thread/shared_future

• 运行
  

实现 RCLCPP

上述内容便是

实现 RCLPY

略

接口 interface

• 数据类型接口

std_msgs/msg/String
std_msgs/msg/UInt32

• 数据帧接口
  雷达：sensor_msgs/msg/LaserScan

• 命令查看某一个接口包下所有的接口

ros2 interface package sensor_msgs

• 接口命令

# 查看接口列表
ros2 interface list

# 查看某一个接口详细的内容
ros2 interface show std_msgs/msg/String

数据类型【基础、包装】

• 基础类型
  基础类型后面加上[]可形成数组

bool
byte
char
float32,float64
int8,uint8
int16,uint16
int32,uint32
int64,uint64
string

• 包装类型
  在已有的接口类型上进行包含

uint32 id
string image_name
sensor_msgs/Image

接口转代码【msg、srv、action文件转换为Python和C++的头文件】

示例：
节点1：执行移动指令，完成指令后应答当前位置
节点1：下发移动指令，定时获取机器人当前状态

• 服务接口 MoveRobot.srv

# 前进后退的距离
float32 distance
---
# 当前的位置
float32 pose

• 话题接口，采用基础类型 RobotStatus.msg

uint32 STATUS_MOVEING = 1
uint32 STATUS_STOP = 1
uint32  status
float32 pose

• 话题接口，混合包装类型 RobotPose.msg

uint32 STATUS_MOVEING = 1
uint32 STATUS_STOP = 2
uint32  status
geometry_msgs/Pose pose

• 创建功能包指令

ros2 pkg create example_ros2_interfaces --build-type ament_cmake --dependencies rosidl_default_generators geometry_msgs

• 创建过程

[euler@Euler node]$ mkdir -p interface/src
[euler@Euler node]$ cd interface/src/
[euler@Euler src]$ ros2 pkg create example_ros2_interfaces --build-type ament_cmake --dependencies rosidl_default_generators geometry_msgs
[euler@Euler src]$ tree -L 3
.
└── example_ros2_interfaces
    ├── CMakeLists.txt
    ├── include
    │   └── example_ros2_interfaces
    ├── package.xml
    └── src

4 directories, 2 files

• 添加 msg 文件，srv 文件

[euler@Euler src]$ tree
.
└── example_ros2_interfaces
    ├── CMakeLists.txt
    ├── include
    │   └── example_ros2_interfaces
    ├── msg
    │   ├── RobotPose.msg
    │   └── RobotStatus.msg
    ├── package.xml
    ├── src
    └── srv
        └── MoveRobot.srv

6 directories, 5 files

• CMakeLists.txt

find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)
# 添加下面的内容
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/RobotPose.msg"
  "msg/RobotStatus.msg"
  "srv/MoveRobot.srv"
  DEPENDENCIES geometry_msgs
)

• package.xml

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rosidl_default_generators</depend>
  <depend>geometry_msgs</depend>
  
  <member_of_group>rosidl_interface_packages</member_of_group> #添加这一行

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

• 编译

colcon build --packages-select example_ros2_interfaces

[euler@Euler interface_ws]$ colcon build --packages-select example_ros2_interfaces
Starting >>> example_ros2_interfaces
Finished <<< example_ros2_interfaces [9.28s]                     

Summary: 1 package finished [9.63s]

[euler@Euler interface_ws]$ tree -L 7
.
...
├── install
│   ├── COLCON_IGNORE
│   ├── example_ros2_interfaces
│   │   ├── include
│   │   │   └── example_ros2_interfaces
│   │   │       └── example_ros2_interfaces
│   │   │           ├── msg
│   │   │           │   ├── detail
│   │   │           │   ├── robot_pose.h
│   │   │           │   ├── robot_pose.hpp
│   │   │           │   ├── robot_status.h
│   │   │           │   ├── robot_status.hpp
│   │   │           │   ├── rosidl_generator_c__visibility_control.h
│   │   │           │   ├── rosidl_typesupport_fastrtps_cpp__visibility_control.h
│   │   │           │   ├── rosidl_typesupport_fastrtps_c__visibility_control.h
│   │   │           │   └── rosidl_typesupport_introspection_c__visibility_control.h
│   │   │           └── srv
│   │   │               ├── detail
│   │   │               ├── move_robot.h
│   │   │               └── move_robot.hpp
│   │   ├── lib
│   │   └── share
│   ...
│ 
└── src
    └── example_ros2_interfaces
        ├── CMakeLists.txt
        ├── include
        ├── msg
        ├── package.xml
        ├── src
        └── srv

43 directories, 45 files

实现 RCLCPP

创建功能包和节点

• 设计两个节点
  example_interfaces_robot_01，机器人节点，对外提供控制机器人移动服务并发布机器人的状态。
  example_interfaces_control_01，控制节点，发送机器人移动请求，订阅机器人状态话题。

cd node_ws/
ros2 pkg create example_interfaces_rclcpp --build-type ament_cmake --dependencies rclcpp example_ros2_interfaces --destination-directory src --node-name example_interfaces_robot_01 
touch src/example_interfaces_rclcpp/src/example_interfaces_control_01.

• CMakeLists.txt

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(example_ros2_interfaces REQUIRED)

add_executable(example_interfaces_robot_01 src/example_interfaces_robot_01.cpp)
target_include_directories(example_interfaces_robot_01 PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_features(example_interfaces_robot_01 PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
  example_interfaces_robot_01
  "rclcpp"
  "example_ros2_interfaces"
)

install(TARGETS example_interfaces_robot_01
  DESTINATION lib/${PROJECT_NAME})


add_executable(example_interfaces_control_01 src/example_interfaces_control_01.cpp)
target_include_directories(example_interfaces_control_01 PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_features(example_interfaces_control_01 PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
  example_interfaces_control_01
  "rclcpp"
  "example_ros2_interfaces"
)

install(TARGETS example_interfaces_control_01
  DESTINATION lib/${PROJECT_NAME})

• example_interfaces_control_01.cpp

#include "rclcpp/rclcpp.hpp"

class ExampleInterfacesControl : public rclcpp::Node {
public:
  // 构造函数,有一个参数为节点名称
  ExampleInterfacesControl(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
  }

private:
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ExampleInterfacesControl>("example_interfaces_control_01");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

• example_interfaces_robot_01.cpp

#include "rclcpp/rclcpp.hpp"

/*创建一个机器人类，模拟真实机器人*/
class Robot {
public:
  Robot() = default;
  ~Robot() = default;
private:
};


class ExampleInterfacesRobot : public rclcpp::Node {
public:
  ExampleInterfacesRobot(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
  }

private:
  Robot robot;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ExampleInterfacesRobot>("example_interfaces_robot_01");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

• 编译测试

cd node_ws/
colcon build
source install/setup.bash

编写机器人类

对外提供获取当前状态、获取当前位置和移动一定的距离三个接口，其中移动指定距离这个函数每移动一步会休眠500ms

// 导入上一节定义的消息接口
#include "example_ros2_interfaces/msg/robot_status.hpp"
#include "example_ros2_interfaces/srv/move_robot.hpp"
#include "rclcpp/rclcpp.hpp"

/*
 * 测试指令：ros2 service call /move_robot example_ros2_interfaces/srv/MoveRobot "{distance: 5}"
 */

class Robot {
public:
  Robot() = default;
  ~Robot() = default;
  /**
   * @brief 移动指定的距离
   *
   * @param distance
   * @return float
   */
  float move_distance(float distance) {
    status_ = example_ros2_interfaces::msg::RobotStatus::STATUS_MOVEING;
    target_pose_ += distance;
    // 当目标距离和当前距离大于0.01则持续向目标移动
    while (fabs(target_pose_ - current_pose_) > 0.01) {
      // 每一步移动当前到目标距离的1/10
      float step = distance / fabs(distance) * fabs(target_pose_ - current_pose_) * 0.1;
      current_pose_ += step;
      std::cout << "移动了：" << step << "当前位置：" << current_pose_ << std::endl;
      // 当前线程休眠500ms
      std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }
    status_ = example_ros2_interfaces::msg::RobotStatus::STATUS_STOP;
    return current_pose_;
  }
  /**
   * @brief Get the current pose
   *
   * @return float
   */
  float get_current_pose() { return current_pose_; }

  /**
   * @brief Get the status
   *
   * @return int
   *  1 example_ros2_interfaces::msg::RobotStatus::STATUS_MOVEING
   *  2 example_ros2_interfaces::msg::RobotStatus::STATUS_STOP
   */
  int get_status() { return status_; }

private:
  // 声明当前位置
  float current_pose_ = 0.0;
  // 目标距离
  float target_pose_ = 0.0;
  int status_ = example_ros2_interfaces::msg::RobotStatus::STATUS_STOP;
};

编写机器人节点逻辑

利用接口编写机器人节，利用定时器不断发送数据，收到请求后调用机器人类的move_distance接口来移动机器人。

class ExampleInterfacesRobot : public rclcpp::Node {
public:
  ExampleInterfacesRobot(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
    /*创建move_robot服务*/
    move_robot_server_ = this->create_service<example_ros2_interfaces::srv::MoveRobot>(
      "move_robot", std::bind(&ExampleInterfacesRobot::handle_move_robot, this, std::placeholders::_1, std::placeholders::_2));
    /*创建发布者*/
    robot_status_publisher_ = this->create_publisher<example_ros2_interfaces::msg::RobotStatus>("robot_status", 10);
    /*创建一个周期为500ms的定时器*/
    timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&ExampleInterfacesRobot::timer_callback, this));
  }

private:
  Robot robot; /*实例化机器人*/
  rclcpp::TimerBase::SharedPtr timer_; /*定时器，用于定时发布机器人位置*/
  rclcpp::Service<example_ros2_interfaces::srv::MoveRobot>::SharedPtr move_robot_server_; /*移动机器人服务*/
  rclcpp::Publisher<example_ros2_interfaces::msg::RobotStatus>::SharedPtr robot_status_publisher_; /*发布机器人位姿发布者*/

  /**
   * @brief 500ms 定时回调函数，
   * 
   */
  void timer_callback() {
    // 创建消息
    example_ros2_interfaces::msg::RobotStatus message;
    message.status = robot.get_status();
    message.pose = robot.get_current_pose();
    RCLCPP_INFO(this->get_logger(), "Publishing: %f", robot.get_current_pose());
    // 发布消息
    robot_status_publisher_->publish(message);
  };

  /**
   * @brief 收到话题数据的回调函数
   * 
   * @param request 请求共享指针，包含移动距离
   * @param response 响应的共享指针，包含当前位置信息
   */
  void handle_move_robot(const std::shared_ptr<example_ros2_interfaces::srv::MoveRobot::Request> request,
                         std::shared_ptr<example_ros2_interfaces::srv::MoveRobot::Response> response) {
    RCLCPP_INFO(this->get_logger(), "收到请求移动距离：%f，当前位置：%f", request->distance, robot.get_current_pose());
    robot.move_distance(request->distance);
    response->pose = robot.get_current_pose();
  };
};

编写控制节点

• 头文件

#include "rclcpp/rclcpp.hpp"
#include "example_ros2_interfaces/srv/move_robot.hpp"
#include "example_ros2_interfaces/msg/robot_status.hpp"

class ExampleInterfacesControl : public rclcpp::Node {
public:
  ExampleInterfacesControl(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
    /*创建move_robot客户端*/
    client_ = this->create_client<example_ros2_interfaces::srv::MoveRobot>(
      "move_robot");
    /*订阅机器人状态话题*/
    robot_status_subscribe_ = this->create_subscription<example_ros2_interfaces::msg::RobotStatus>("robot_status", 10, std::bind(&ExampleInterfacesControl::robot_status_callback_, this, std::placeholders::_1));
  }


  /**
   * @brief 发送移动机器人请求函数
   * 步骤：1.等待服务上线
   *      2.构造发送请求
   * 
   * @param distance 
   */
  void move_robot(float distance) {
    RCLCPP_INFO(this->get_logger(), "请求让机器人移动%f", distance);

    /*等待服务端上线*/
    while (!client_->wait_for_service(std::chrono::seconds(1))) {
      //等待时检测rclcpp的状态
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(), "等待服务的过程中被打断...");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "等待服务端上线中");
    }

    // 构造请求
    auto request = 
      std::make_shared<example_ros2_interfaces::srv::MoveRobot::Request>();
    request->distance = distance;

    // 发送异步请求，然后等待返回，返回时调用回调函数
    client_->async_send_request(
      request, std::bind(&ExampleInterfacesControl::result_callback_, this,
                         std::placeholders::_1));
  };

private:
  // 声明客户端
  rclcpp::Client<example_ros2_interfaces::srv::MoveRobot>::SharedPtr client_;
  rclcpp::Subscription<example_ros2_interfaces::msg::RobotStatus>::SharedPtr robot_status_subscribe_;
  /* 机器人移动结果回调函数 */
  void result_callback_(
    rclcpp::Client<example_ros2_interfaces::srv::MoveRobot>::SharedFuture
      result_future) {
    auto response = result_future.get();
    RCLCPP_INFO(this->get_logger(), "收到移动结果：%f", response->pose);
  }

  /**
   * @brief 机器人状态话题接收回调函数
   * 
   * @param msg 
   */
  void robot_status_callback_(const example_ros2_interfaces::msg::RobotStatus::SharedPtr msg)
  {
    RCLCPP_INFO(this->get_logger(), "收到状态数据位置：%f 状态：%d", msg->pose ,msg->status);
  }
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ExampleInterfacesControl>("example_interfaces_control_01");
  /*这里调用了服务，让机器人向前移动5m*/
  node->move_robot(5.0);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

测试运行

colcon build --packages-up-to example_interfaces_rclcpp

–packages-up-to 这个指令 以先后顺序编译了example_ros2_interfaces再编译example_interfaces_rclcpp

实现 RCLPY

略

原始类型，后面加上 [] 将变成数组

bool
byte
char
float32, float64
int8, uint8
int16, uint16
int32, uint32
int64, uint64
string

扩展类型（套娃）

• 套娃 (第一层)
  ROS2 定义的拓展类型，如图像数据类型 sensor_msgs/Image

ros2 interface show sensor_msgs/msg/Image

输出：
std_msgs/Header header  # Header timestamp should be acquisition time of image
uint32 height           # image height, that is, number of rows
uint32 width            # image width, that is, number of columns
string encoding         # Encoding of pixels -- channel meaning, ordering, size
uint8 is_bigendian      # is this data bigendian?
uint32 step             # Full row length in bytes
uint8[] data            # actual matrix data, size is (step * rows)

• 套娃 (第二层)
  上述中的 std_msgs/Header header 查看

ros2 interface show std_msgs/msg/Header

输出：
builtin_interfaces/Time stamp # Two-integer timestamp that is expressed as seconds and nanoseconds.
string frame_id               # Transform frame with which this data is associated.

• 套娃 (第三层)
  上述中的 builtin_interfaces/Time 查看

ros2 interface show builtin_interfaces/msg/Time 

输出：
# Time indicates a specific point in time, relative to a clock's 0 point.

# The seconds component, valid over all int32 values.
int32 sec

# The nanoseconds component, valid in the range [0, 10e9).
uint32 nanosec

通信质量Qos配置指南

Rviz显示不出数据了！一文搞懂Qos

DDS进阶之Fast-DDS环境搭建

https://fishros.com/d2lros2/#/humble/chapt3/advanced/3.DDS%E8%BF%9B%E9%98%B6%E4%B9%8BFast-DDS%E7%8E%AF%E5%A2%83%E6%90%AD%E5%BB%BA

使用DDS进行订阅发布

https://fishros.com/d2lros2/#/humble/chapt3/advanced/4.%E4%BD%BF%E7%94%A8DDS%E8%BF%9B%E8%A1%8C%E8%AE%A2%E9%98%85%E5%8F%91%E5%B8%83

2.9 通信–参数 【操作命令】

控制

开环控制 与 闭环控制。

参数

A parameter is a configuration value of a node. You can think of parameters as node settings.

• 小乌龟

终端 1: 
ros2 run turtlesim turtlesim_node

终端 2: 
ros2 run turtlesim turtle_teleop_key

ros2 param list
[euler@Euler node]$ source /opt/ros/humble/setup.bash 
[euler@Euler node]$ ros2 param list
/teleop_turtle:
  qos_overrides./parameter_events.publisher.depth
  qos_overrides./parameter_events.publisher.durability
  qos_overrides./parameter_events.publisher.history
  qos_overrides./parameter_events.publisher.reliability
  scale_angular
  scale_linear
  use_sim_time
/turtlesim:
  background_b
  background_g
  background_r
  qos_overrides./parameter_events.publisher.depth
  qos_overrides./parameter_events.publisher.durability
  qos_overrides./parameter_events.publisher.history
  qos_overrides./parameter_events.publisher.reliability
  use_sim_time
[euler@Euler node]$

查看一个参数的信息
ros2 param describe <node_name> <param_name>
eg:
ros2 param describe /turtlesim background_b

[euler@Euler node]$ ros2 param describe /turtlesim background_b
Parameter name: background_b
  Type: integer
  Description: Blue channel of the background color
  Constraints:
    Min value: 0
    Max value: 255
    Step: 1

• 查看参数值
  参数的组成由名字和值（键值组成），名字可以通过 param list 获取

ros2 param get /turtlesim background_b
eg:
[euler@Euler node]$ ros2 param get /turtlesim background_b
Integer value is: 255

• 设置参数【临时】

ros2 param set <node_name> <parameter_name> <value>

eg:
ros2 param set /turtlesim background_r 44
ros2 param set /turtlesim background_g 156
ros2 param set /turtlesim background_b 10

• 保存参数
  把参数存起来其实就相当去把当前的参数值拍一张快照，然后保存下来，后面可以用于恢复参数到当前的数值。

ros2 param dump <node_name>
eg:
ros2 param dump /turtlesim

[euler@Euler node]$ ros2 param dump /turtlesim
/turtlesim:
  ros__parameters:
    background_b: 255
    background_g: 86
    background_r: 69
    qos_overrides:
      /parameter_events:
        publisher:
          depth: 1000
          durability: volatile
          history: keep_last
          reliability: reliable
    use_sim_time: false

• 启动节点时加载参数快照

ros2 run <package_name> <executable_name> --ros-args --params-file <file_name>

eg: 
ros2 run turtlesim turtlesim_node --ros-args --params-file ./turtlesim.yaml

参数实现 RCLCPP

日志

ROS2将日志分为五个级别，在RCLCPP中通过不同的宏可以实现不同日志级别日志的打印

RCLCPP_DEBUG(this->get_logger(), "DEBUG日志!");
RCLCPP_INFO(this->get_logger(), "INFO日志!");
RCLCPP_WARN(this->get_logger(), "WARN日志!");
RCLCPP_ERROR(this->get_logger(), "ERROR日志!");
RCLCPP_FATAL(this->get_logger(), "FATAL日志!");

ROS2中可以通过已有的API设置日志的级别，RCLCPP中API如下：
this->get_logger().set_level(log_level);

实现

rclcpp: ROS Client Library for C++
https://docs.ros2.org/latest/api/rclcpp/

• 创建功能包和节点

mkdir -p chapt4/node_ws/
ros2 pkg create example_parameters_rclcpp --build-type ament_cmake --dependencies rclcpp --destination-directory src --node-name parameters_basic --maintainer-name "sen" --maintainer-email "sen@yocobot.com"

• parameters_basic.cpp

#include <chrono>
#include "rclcpp/rclcpp.hpp"
/*
    # declare_parameter            声明和初始化一个参数
    # describe_parameter(name)  通过参数名字获取参数的描述
    # get_parameter                通过参数名字获取一个参数
    # set_parameter                设置参数的值
*/
class ParametersBasicNode : public rclcpp::Node {
 public:
  // 构造函数,有一个参数为节点名称
  explicit ParametersBasicNode(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
    this->declare_parameter("rcl_log_level", 0);     /*声明参数*/
    this->get_parameter("rcl_log_level", log_level); /*获取参数*/
    /*设置日志级别*/
    this->get_logger().set_level((rclcpp::Logger::Level)log_level);
    using namespace std::literals::chrono_literals;
    timer_ = this->create_wall_timer(
        500ms, std::bind(&ParametersBasicNode::timer_callback, this));
  }

 private:
  int log_level;
  rclcpp::TimerBase::SharedPtr timer_;

  void timer_callback() {
    this->get_parameter("rcl_log_level", log_level); /*获取参数*/
    /*设置日志级别*/
    this->get_logger().set_level((rclcpp::Logger::Level)log_level);
    std::cout<<"======================================================"<<std::endl;
    RCLCPP_DEBUG(this->get_logger(), "DEBUG日志!");
    RCLCPP_INFO(this->get_logger(), "INFO日志!");
    RCLCPP_WARN(this->get_logger(), "WARN日志!");
    RCLCPP_ERROR(this->get_logger(), "ERROR日志!");
    RCLCPP_FATAL(this->get_logger(), "FATAL日志!");
  }
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  /*创建对应节点的共享指针对象*/
  auto node = std::make_shared<ParametersBasicNode>("parameters_basic");
  /* 运行节点，并检测退出信号*/
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

• 构建测试

colcon build  --packages-select example_parameters_rclcpp
source install/setup.bash
ros2 run example_parameters_rclcpp parameters_basic
# 传递参数
ros2 run example_parameters_rclcpp parameters_basic --ros-args -p rcl_log_level:=10

• ros2 自带的日志级别配置

ros2 run package-name node-name --ros-args --log-level debug

可以通过 rqt 可视化设置和查看

参数实现 RCLPY

略

2.10 动作

参数是由服务构建出来，而Action是由话题和服务共同构建出来

Action 的三大组成 (目标 Goal、反馈 Feedback 和结果 Result )

• 目标（Goal）：这是行动的起点，指明行动所要达成的具体结果或状态。目标应该是清晰、可衡量的，以便于跟踪进度和最终的完成情况。

eg: RCS 目标下发，AMR 收到目标应答(ACK)，一次握手

• 反馈（Feedback）：在行动过程中，反馈提供了关于行动效果的信息。它可以帮助行动者了解当前的进展情况，是否需要调整策略或方法。反馈可以是内在的，比如个人的自我评估，也可以是外在的，比如他人的评价或市场的反应。

eg: 可以放在心跳 (HeartBeat) 中，或者 status 通道中

• 结果（Result）：这是行动的最终产物，是目标达成后的具体表现。结果可以是正面的，也可以是负面的，它们为行动者提供了学习和改进的机会。

eg: 目标达成后，可以返回一个结果，比如：成功、失败、超时等
RCS 可以一直请求目标状态 (queryStatus)，也可以约定好 AMR 定时上报，这样就不需要④这一步了
任务并行时，结果这个机制的④和⑤都需要。

Action 操作命令

# 获取目前系统中的action列表
[euler@Euler Desktop]$ ros2 action list
/turtle1/rotate_absolute

# 获取目前系统中的action列表以及类型
[euler@Euler Desktop]$ ros2 action list -t
/turtle1/rotate_absolute [turtlesim/action/RotateAbsolute]
[euler@Euler Desktop]$ ros2 interface show turtlesim/action/RotateAbsolute
# The desired heading in radians
float32 theta
---
# The angular displacement in radians to the starting position
float32 delta
---
# The remaining rotation in radians
float32 remaining

# 查看 action 信息
[euler@Euler Desktop]$ ros2 action info /turtle1/rotate_absolute 
Action: /turtle1/rotate_absolute
Action clients: 1
    /teleop_turtle
Action servers: 1
    /turtlesim

# 发送 goal 给服务端
[euler@Euler Desktop]$ ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: 1.6}"
Waiting for an action server to become available...
Sending goal:
     theta: 1.6

Goal accepted with ID: 28db61c9537a4129b006213b2fe07925

Result:
    delta: -1.5839999914169312

Goal finished with status: SUCCEEDED

# 发送 goal 给服务端，并反馈
[euler@Euler Desktop]$ ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: 1.5}" --feedback
Waiting for an action server to become available...
Sending goal:
     theta: 1.5

Feedback:
    remaining: -0.08399999141693115

Goal accepted with ID: 06729872966a4ed0a3c2c9a1550655a1

Feedback:
    remaining: -0.0679999589920044

Feedback:
    remaining: -0.05200004577636719

Feedback:
    remaining: -0.03600001335144043

Feedback:
    remaining: -0.019999980926513672

Result:
    delta: 0.06400000303983688

Goal finished with status: SUCCEEDED
[euler@Euler Desktop]$

Action 自定义

• 创建功能包

cd action_ws/
ros2 pkg create robot_control_interfaces --build-type ament_cmake --destination-directory src --maintainer-name "sen" --maintainer-email "sen@yocobot.com"

• 创建接口文件

mkdir -p src/robot_control_interfaces/action
touch src/robot_control_interfaces/action/MoveRobot.action

• packages.xml

  <depend>rosidl_default_generators</depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

• CMakeLists.txt

find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)


rosidl_generate_interfaces(${PROJECT_NAME}
  "action/MoveRobot.action"
)

• 编写接口

# Goal: 要移动的距离
float32 distance
---
# Result: 最终的位置
float32 pose
---
# Feedback: 中间反馈的位置和状态
float32 pose
uint32 status
uint32 STATUS_MOVEING = 3
uint32 STATUS_STOP = 4

• 编译生成接口

colcon build --packages-select robot_control_interfaces

• 生产头文件

C++ Action消息头文件目录install/robot_control_interfaces/include
Python Action消息文件目录install/robot_control_interfaces/local/lib/python3.10

Action RCLCPP实现

https://docs.ros2.org/latest/api/rclcpp_action/

cd action_ws/
ros2 pkg create example_action_rclcpp --build-type ament_cmake --dependencies rclcpp rclcpp_action robot_control_interfaces --destination-directory src --node-name action_robot_01 --maintainer-name "sen" --maintainer-email "sen@yocobot.com"
touch src/example_action_rclcpp/src/action_control_01.cpp

• 创建Robot类的头文件和CPP文件

touch src/example_action_rclcpp/include/example_action_rclcpp/robot.h
touch src/example_action_rclcpp/src/robot.cpp

.
├── CMakeLists.txt
├── include
│   └── example_action_rclcpp
│       └── robot.h 
├── package.xml
└── src
    ├── action_control_01.cpp
    ├── action_robot_01.cpp
    └── robot.cpp

3 directories, 6 files

• robot.h

#endif  // EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_
#ifndef EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_
#define EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_
#include "rclcpp/rclcpp.hpp"
#include "robot_control_interfaces/action/move_robot.hpp"

class Robot {
 public:
  using MoveRobot = robot_control_interfaces::action::MoveRobot;
  Robot() = default;
  ~Robot() = default;
  float move_step(); /*移动一小步，请间隔500ms调用一次*/
  bool set_goal(float distance); /*移动一段距离*/
  float get_current_pose();
  int get_status();
  bool close_goal(); /*是否接近目标*/
  void stop_move();  /*停止移动*/

 private:
  float current_pose_ = 0.0;             /*声明当前位置*/
  float target_pose_ = 0.0;              /*目标距离*/
  float move_distance_ = 0.0;            /*目标距离*/
  std::atomic<bool> cancel_flag_{false}; /*取消标志*/
  int status_ = MoveRobot::Feedback::STATUS_STOP;
};

#endif  // EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_

• robot.cpp

#include "example_action_rclcpp/robot.h"

/*移动一小步，请间隔500ms调用一次*/
float Robot::move_step() {
  int direct = move_distance_ / fabs(move_distance_);
  float step = direct * fabs(target_pose_ - current_pose_) *
               0.1; /* 每一步移动当前到目标距离的1/10*/
  current_pose_ += step;
  std::cout << "移动了：" << step << "当前位置：" << current_pose_ << std::endl;
  return current_pose_;
}

/*移动一段距离*/
bool Robot::set_goal(float distance) {
  move_distance_ = distance;
  target_pose_ += move_distance_;

  /* 当目标距离和当前距离大于0.01同意向目标移动 */
  if (close_goal()) {
    status_ = MoveRobot::Feedback::STATUS_STOP;
    return false;
  }
  status_ = MoveRobot::Feedback::STATUS_MOVEING;
  return true;
}

float Robot::get_current_pose() { return current_pose_; }
int Robot::get_status() { return status_; }
/*是否接近目标*/
bool Robot::close_goal() { return fabs(target_pose_ - current_pose_) < 0.01; }
void Robot::stop_move() {
  status_ = MoveRobot::Feedback::STATUS_STOP;
} /*停止移动*/

• Action使用了三个回调函数，分别用于处理收到目标、收到停止、确认接受执行。
  （1）handle_goal，收到目标，反馈是否可以执行该目标，可以则返回ACCEPT_AND_EXECUTE,不可以则返回REJECT
  （2）handle_cancel，收到取消运行请求，可以则返回ACCEPT，不可以返回REJECT。
  （3）handle_accepted，处理接受请求，当handle_goal中对移动请求ACCEPT后则进入到这里进行执行，这里我们是单独开了个线程进行执行execute_move函数，目的是避免阻塞主线程。

• 创建功能包
  创建example_action_rclcpp功能包，添加robot_control_interfaces、rclcpp_action、rclcpp三个依赖，自动创建action_robot_01节点，并手动创建action_control_01.cpp节点。

• action_robot_01.cpp

#include "example_action_rclcpp/robot.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_control_interfaces/action/move_robot.hpp"

// 创建一个ActionServer类
class ActionRobot01 : public rclcpp::Node {
 public:
  using MoveRobot = robot_control_interfaces::action::MoveRobot;
  using GoalHandleMoveRobot = rclcpp_action::ServerGoalHandle<MoveRobot>;

  explicit ActionRobot01(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());

    using namespace std::placeholders;  // NOLINT

    this->action_server_ = rclcpp_action::create_server<MoveRobot>(
        this, "move_robot",
        std::bind(&ActionRobot01::handle_goal, this, _1, _2),
        std::bind(&ActionRobot01::handle_cancel, this, _1),
        std::bind(&ActionRobot01::handle_accepted, this, _1));
  }

 private:
  Robot robot;
  rclcpp_action::Server<MoveRobot>::SharedPtr action_server_;

  rclcpp_action::GoalResponse handle_goal(
      const rclcpp_action::GoalUUID& uuid,
      std::shared_ptr<const MoveRobot::Goal> goal) {
    RCLCPP_INFO(this->get_logger(), "Received goal request with distance %f",
                goal->distance);
    (void)uuid;
    if (fabs(goal->distance > 100)) {
      RCLCPP_WARN(this->get_logger(), "目标距离太远了，本机器人表示拒绝！");
      return rclcpp_action::GoalResponse::REJECT;
    }
    RCLCPP_INFO(this->get_logger(),
                "目标距离%f我可以走到，本机器人接受，准备出发！",
                goal->distance);
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  rclcpp_action::CancelResponse handle_cancel(
      const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
    (void)goal_handle;
    robot.stop_move(); /*认可取消执行，让机器人停下来*/
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void execute_move(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    const auto goal = goal_handle->get_goal();
    RCLCPP_INFO(this->get_logger(), "开始执行移动 %f 。。。", goal->distance);

    auto result = std::make_shared<MoveRobot::Result>();
    rclcpp::Rate rate = rclcpp::Rate(2);
    robot.set_goal(goal->distance);
    while (rclcpp::ok() && !robot.close_goal()) {
      robot.move_step();
      auto feedback = std::make_shared<MoveRobot::Feedback>();
      feedback->pose = robot.get_current_pose();
      feedback->status = robot.get_status();
      goal_handle->publish_feedback(feedback);
      /*检测任务是否被取消*/
      if (goal_handle->is_canceling()) {
        result->pose = robot.get_current_pose();
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal Canceled");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "Publish Feedback"); /*Publish feedback*/
      rate.sleep();
    }

    result->pose = robot.get_current_pose();
    goal_handle->succeed(result);
    RCLCPP_INFO(this->get_logger(), "Goal Succeeded");
  }

  void handle_accepted(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    using std::placeholders::_1;
    std::thread{std::bind(&ActionRobot01::execute_move, this, _1), goal_handle}
        .detach();
  }
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto action_server = std::make_shared<ActionRobot01>("action_robot_01");
  rclcpp::spin(action_server);
  rclcpp::shutdown();
  return 0;
}

• 创建客户端简单，发送请求的时候可以指定三个回调函数：
  （1）goal_response_callback，目标的响应回调函数。
  （2）feedback_callback，执行过程中进度反馈接收回调函数。
  （3）result_callback，最终结果接收的回调函数。

• action_control_01.cpp

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_control_interfaces/action/move_robot.hpp"

class ActionControl01 : public rclcpp::Node {
 public:
  using MoveRobot = robot_control_interfaces::action::MoveRobot;
  using GoalHandleMoveRobot = rclcpp_action::ClientGoalHandle<MoveRobot>;

  explicit ActionControl01(
      std::string name,
      const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions())
      : Node(name, node_options) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());

    this->client_ptr_ =
        rclcpp_action::create_client<MoveRobot>(this, "move_robot");

    this->timer_ =
        this->create_wall_timer(std::chrono::milliseconds(500),
                                std::bind(&ActionControl01::send_goal, this));
  }

  void send_goal() {
    using namespace std::placeholders;

    this->timer_->cancel();

    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
      RCLCPP_ERROR(this->get_logger(),
                   "Action server not available after waiting");
      rclcpp::shutdown();
      return;
    }

    auto goal_msg = MoveRobot::Goal();
    goal_msg.distance = 10;

    RCLCPP_INFO(this->get_logger(), "Sending goal");

    auto send_goal_options =
        rclcpp_action::Client<MoveRobot>::SendGoalOptions();
    send_goal_options.goal_response_callback =
        std::bind(&ActionControl01::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
        std::bind(&ActionControl01::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
        std::bind(&ActionControl01::result_callback, this, _1);
    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

 private:
  rclcpp_action::Client<MoveRobot>::SharedPtr client_ptr_;
  rclcpp::TimerBase::SharedPtr timer_;

  void goal_response_callback(GoalHandleMoveRobot::SharedPtr goal_handle) {
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    } else {
      RCLCPP_INFO(this->get_logger(),
                  "Goal accepted by server, waiting for result");
    }
  }

  void feedback_callback(
      GoalHandleMoveRobot::SharedPtr,
      const std::shared_ptr<const MoveRobot::Feedback> feedback) {
    RCLCPP_INFO(this->get_logger(), "Feedback current pose:%f", feedback->pose);
  }

  void result_callback(const GoalHandleMoveRobot::WrappedResult& result) {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
        return;
      default:
        RCLCPP_ERROR(this->get_logger(), "Unknown result code");
        return;
    }

    RCLCPP_INFO(this->get_logger(), "Result received: %f", result.result->pose);
    // rclcpp::shutdown();
  }
};  // class ActionControl01

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto action_client = std::make_shared<ActionControl01>("action_robot_cpp");
  rclcpp::spin(action_client);
  rclcpp::shutdown();
  return 0;
}

• CMakeList.txt

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(robot_control_interfaces REQUIRED)
find_package(example_interfaces REQUIRED)
find_package(rclcpp_action REQUIRED)

# action_robot节点

add_executable(action_robot_01 
    src/robot.cpp
    src/action_robot_01.cpp
)
target_include_directories(action_robot_01 PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_features(action_robot_01 PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
  action_robot_01
  "rclcpp"
  "rclcpp_action"
  "robot_control_interfaces"
  "example_interfaces"
)

install(TARGETS action_robot_01
  DESTINATION lib/${PROJECT_NAME})

# action_control节点

add_executable(action_control_01 
  src/action_control_01.cpp
)
target_include_directories(action_control_01 PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_features(action_control_01 PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
ament_target_dependencies(
  action_control_01
  "rclcpp"
  "rclcpp_action"
  "robot_control_interfaces"
  "example_interfaces"
)

install(TARGETS action_control_01
DESTINATION lib/${PROJECT_NAME})

• 终端 1

cd action_ws/
colcon build --packages-up-to example_action_rclcpp
source install/setup.bash 
ros2 run example_action_rclcpp  action_robot_01

• 终端 2

source install/setup.bash 
ros2 run example_action_rclcpp action_control_01

Action RCLPY实现

略

2.11 原理 & 生命周期

ROS2生命周期节点是利用状态机构成的，状态直接的转换依靠ROS2的通信机制完成。

生命周期节点主要有以下几个状态

• 未配置状态（Unconfigured） ，节点开始时的第一个状态，并在出现错误后结束。没有执行，其主要目的是错误恢复。
• 非活跃状态（Inactivate），节点持有资源（发布者、监听者等）和配置（参数、内部变量），但什么也不做。 没有执行，没有传输，传入的数据可以保存在缓冲区中，但不能读取， 主要目的是允许重新配置。
• 活跃状态（Activate）， 正常执行。
• 已完成状态（Finalized），节点已被销毁。

2.12 ## 启动管理工具–Launch

介绍

一个机器人系统，往往由很多个不同功能的节点组成，启动一个机器人系统时往往需要启动多个节点，同时根据应用场景和机器人的不同，每个节点还会有不同的配置项。

launch文件允许我们同时启动和配置多个包含 ROS 2 节点的可执行文件。

在ROS1中launch文件只有一种格式以.launch结尾的xml文档，需要了解 xml 语法。
在ROS2中可以使用Python代码来编写launch文件。

ROS2的launch文件有三种格式

ROS2的launch文件有三种格式，python、xml、yaml。其中ROS2官方推荐的时python方式编写launch文件。 原因在于，相较于XML和YAML，Python是一个编程语言，更加的灵活，我们可以利用Python的很多库来做一些其他工作（比如创建一些初始化的目录等）。

用Python编写Launch

• 创建功能包和launch文件
  创建文件夹和功能包，接着touch一个launch文件，后缀为.py。

mkdir -p chapt5/chapt5_ws/src
cd chapt5/chapt5_ws/src
ros2 pkg create robot_startup --build-type ament_cmake --destination-directory src
mkdir -p src/robot_startup/launch
touch src/robot_startup/launch/example_action.launch.py

• 启动多个节点的示例
  我们需要导入两个库，一个叫做LaunchDescription，用于对launch文件内容进行描述，一个是Node，用于声明节点所在的位置。

example_action.launch.py

# 导入库
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    """launch内容描述函数，由ros2 launch 扫描调用"""
    action_robot_01 = Node(
        package="example_action_rclcpp",
        executable="action_robot_01"
    )
    action_control_01 = Node(
        package="example_action_rclcpp",
        executable="action_control_01"
    )
    # 创建LaunchDescription对象launch_description,用于描述launch文件
    launch_description = LaunchDescription(
        [action_robot_01, action_control_01])
    # 返回让ROS2根据launch描述执行节点
    return launch_description

• 将launch文件拷贝到安装目录
  如果你编写完成后直接编译你会发现install目录下根本没有你编写的launch文件，后续launch自然也找不到这个launch文件。
  因为我们用的是ament_cmake类型功能包，所以这里要使用cmake命令进行文件的拷贝

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME})

如果是ament_python功能包版

from setuptools import setup
from glob import glob
import os

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']),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
    ],
    },
)

• 编译测试

colcon build

编译完成后，在chapt5/chapt5_ws/install/robot_startup/share/robot_startup/launch目录下你应该就可以看到被cmake拷贝过去的launch文件了。

# source 第五章的工作目录，这样才能找到对应的节点，不信你可以不source试试
source ../../chapt5/chapt5_ws/install/setup.bash
source install/setup.bash
ros2 launch robot_startup example_action.launch.py
# 新终端
ros2 node list #即可看到两个节点

添加参数&修改命名空间

接着我们尝试使用launch运行参数节点，并通过launch传递参数，和给节点以不同的命名空间。

新建chapt5/chapt5_ws/src/robot_startup/launch/example_param_rclcpp.launch.py。

编写内容如下

# 导入库
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    """launch内容描述函数，由ros2 launch 扫描调用"""
    parameters_basic1 = Node(
        package="example_parameters_rclcpp",
        namespace="rclcpp",
        executable="parameters_basic",
        parameters=[{'rcl_log_level': 40}]
    )
    parameters_basic2 = Node(
        package="example_parameters_rclpy",
        namespace="rclpy",
        executable="parameters_basic",
        parameters=[{'rcl_log_level': 50}]
    )
    # 创建LaunchDescription对象launch_description,用于描述launch文件
    launch_description = LaunchDescription(
        [parameters_basic1, parameters_basic2])
    # 返回让ROS2根据launch描述执行节点
    return launch_description

• 编译运行测试

# source 第五章的工作目录，这样才能找到对应的节点，不信你可以不source试试
source ../../chapt5/chapt5_ws/install/setup.bash
source install/setup.bash
ros2 launch robot_startup example_param_rclcpp.launch.py
# 新终端
ros2 node list #即可看到两个节点

2.12 数据可视化工具–RVIZ

https://fishros.com/d2lros2/#/humble/chapt5/get_started/3.%E6%95%B0%E6%8D%AE%E5%8F%AF%E8%A7%86%E5%8C%96%E5%B7%A5%E5%85%B7-RVIZ

2.13 调试小工具–RQT

https://fishros.com/d2lros2/#/humble/chapt5/get_started/4.%E5%B8%B8%E7%94%A8%E8%B0%83%E8%AF%95%E5%B0%8F%E5%B7%A5%E5%85%B7-RQT

2.14 数据录播工具–ROSBag

https://fishros.com/d2lros2/#/humble/chapt5/get_started/5.%E6%95%B0%E6%8D%AE%E5%BD%95%E6%92%AD%E5%B7%A5%E5%85%B7-rosbag

2.15 仿真工具–Gazebo

https://fishros.com/d2lros2/#/humble/chapt5/get_started/6.%E5%85%BC%E5%AE%B9%E4%BB%BF%E7%9C%9F%E5%B7%A5%E5%85%B7-Gazebo

参考

1、ROS 2 Documentation humble
2、官方–ROS2 Humble 教程
3、官方–ROS2 Humble 控制
4、ROS2
5、ROS2 Humble学习笔记
6、ROS2-humble学习
7、ROS2 学习（一）ROS2 简介与基本使用
8、小鱼–动手学ROS2
9、openEuler22.03SP3中的cmake3.22.0读取openssl版本号为乱码，导致使用cmakecache的其他程序出错
10、官方 colcon – Installation
11、openEuler-22.03-LTS–嵌入式ROS运行时支持
12、生肖Robot–电鼠
13、src-openEuler/ament_cmake
14、ament_cmake_python user documentation
15、ROS2 教程
16、ROS2探索总结-4.ament编译系统
17、机器人操作系统入门讲义
18、Overview and usage of RQt
19、接口–rclcpp
20、接口–rclpy