ROS2机器人操作系统教程

ROS2介绍
- ROS2简介
- ROS2框架
- ROS2参考资料
  - 官方资料
  - 推荐的视频资料与配套文档
- 学习ROS2需要用到的知识储备
- 安装ROS2
- ROS2安装进阶(可选)
  - 安装进阶的包
  - 环境进阶
    - Apt方式
    - 源码方式
- 测试ROS2
  - 示例1
  - 示例2
- 常见问题
  - Rviz2闪烁
入门操作
- 简介
- 终端环境搭建
- 命令行操作
- ROS2 HelloWorld(C++)
- ROS2 HelloWorld(Python)
- 运行优化(bash终端环境)
- VScode环境搭建
- 运行优化(colcon build)
- VScode环境进阶
  - clangd插件代码提示(可选,但是建议)
- 安装其他工具
- ROS2体系框架
工作空间与功能包
- 工作空间简介
- 源文件编译
- 配置文件
- 常用操作命令
- 核心模块_通信相关
- 核心模块_工具相关
- 功能包
- ROS2技术支持
- ROS2应用方向
四大通信
- 通信机制简介与代码模板
- 话题通信_理论
- 话题通信_实验1(C++)
- 话题通信_自定义接口信息
- 话题通信_实验2(C++)
- 话题通信_rqt查看计算图
- 服务通信_理论
- 服务通信_实验1_服务端实现(C++)
- 服务通信_实验1_客户端实现(C++)
- 动作通信_理论
- 动作通信_实验1_服务端实现(C++)
- 动作通信_实验1_客户端实现(C++)
- 参数服务_理论与API介绍(C++)
- 参数服务_实验1_服务端(C++)
- 参数服务_实验1_客户端(C++)
ROS2其他通信机制
- 概述
- 分布式搭建
- 工作空间覆盖
- 元功能包
- 节点重名
- 话题重名
- 时间相关API
- 通信机制工具
  - 命令行
  - Rqt工具箱
Launch
- 概述
- launch基本使用流程(C++)
- Launch_Python_Node
- Launch_Python_执行命令
- Launch_Python_参数设置
- Launch_Python_文件包含
- Launch_Python_分组设置
- Launch_Python_事件设置
- Launch_XML_YAML_Node
- Launch_XML_YAML_执行命令
- Launch_XML_YAML_参数设置
- Launch_XML_YAML_分组设置
- Launch_XML_YAML_文件包含
回溯rosbag2
- 概述
- rosbag2的命令工具
- rosbag2 C++案例分析及框架搭建
- rosbag2 C++ 录制数据
- rosbag2 C++ 读取数据
坐标变换TF
- 坐标变换
- 坐标变换广播
- 坐标变换监听
- 坐标变换工具
- 练习
  - 乌龟跟随
  - 乌龟护航
- 小结
可视化平台RVIZ2与URDF建模语言
- 可视化简介
- rviz2基本使用
- rviz2集成URDF基本流程
- URDF语法
  - 简介
  - robot根标签
  - link标签
  - joint标签
  - 练习
- URDF工具
  - check_urdf 语法检查
  - urdf_to_graphviz 结构查看
- SW2URDF
- xacro
- 小结
Stage_Ros2仿真平台
- stage_ros2安装与运行
- stage_ros2仿真框架搭建
- stage_ros2设置窗体
- stage_ros2基本模型
- stage_ros2添加地图
- stage_ros2添加机器人
- stage_ros2添加多机器人
Gezebo仿真平台
- 简述
- Gazebo(Gazebo Classic)
- Ignition Gazebo
- 将Ign Gazebo迁移至Gz Sim
  - SDF文件
- 本章小结
机器人导航Navigation2(仿真篇)
- 导航概述
- SLAM 定位与建图
- 地图服务
  - 保存地图(序列化)
  - 读取地图(反序列化)
    - 地图读取节点说明
    - 地图读取基本操作
- AMCL自适应蒙特卡洛定位
  - 定位节点说明
  - 定位节点基本操作
- 导航服务器
- 多车编队
ROS2 Serial
- ROS1串口库1.1
- ROS2串口库1.2
机器人硬件
- 机器人组成
- 工控机之远程开发环境
  - 远程开发SSH
  - 远程访问桌面
- 工控机之外接USB设备
- 分布式搭建
- 优化日志
- 硬件平台
- 坐标系与话题关系
- 硬件平台进阶
机器人导航Navigation2(实体篇)
- 准备工作
- 导航参数参考
- SLAM 定位与建图
  - slam_toolbox
  - cartographer
- 地图服务
  - 保存地图(序列化)
  - 读取地图(反序列化)
- AMCL自适应蒙特卡洛定位
- 导航服务器
- 多车编队
Moveit2工业机器人机械臂
- 机器人学
MicroROS
ROS2_Control
Webots仿真平台
OpenCV
- OpenCV
- CV_Bridge
  - 安装

欢迎各位Vinci芬奇机器人队成员来进行学习ROS2。

ROS2介绍
ROS2简介

The Robot Operating System (ROS) is a set of software libraries and tools that help you build robot applications. From drivers to state-of-the-art algorithms, and with powerful developer tools, ROS has what you need for your next robotics project. And it’s all open source.

机器人操作系统（ROS）是一组软件库和工具，可帮助您构建机器人应用程序。从驱动程序到最先进的算法，再加上强大的开发工具，ROS为您的下一个机器人项目提供了所需的东西。而且都是开源的。

ROS2框架

ROS2参考资料
官方资料

由于ROS2参考文档太少，所以很多教程还是需要看ROS2官网：

https://www.ros.org/

推荐的视频资料与配套文档

非常推荐以下资料与视频

https://www.bilibili.com/video/BV1VB4y137ys

赵虚左原版配套资料:[ROS2理论与实践-赵虚左<密码:qinghuamengshi>.pdf](https://sdutvincirobot.feishu.cn/file/GQgvbWU4UoKhbxx8DjHc9nQ1nWf)

暂时无法在飞书文档外展示此内容

学习ROS2需要用到的知识储备
需要用哪些知识？
1. Linux基本操作与Shell脚本语言
Linux基本教程

https://www.runoob.com/linux/linux-tutorial.html
1. Cmake基本使用
Linux C++编译环境配置
1. Git基本操作
Vinci机器人队Git入门教程
1. C/C++语言
Vinci机器人队电控组入门指导

Vinci机器人队C/C++资料
1. Python3语言
Vinci机器人队Python3教程

https://www.runoob.com/python3/python3-tutorial.html
1. XML语言
https://www.runoob.com/xml/xml-tutorial.html
1. YAML语言
http://www.noobyard.com/article/p-saghdsms-mn.html
安装ROS2

ROS1安装请看ROS机器人操作系统教程

二进制包安装ROS2(以Kubuntu Jammy 22.04安装 ROS Humble 为例)

注意:

截至2024年12月，强烈建议如果要用ROS2的话，一定要用ROS Humble,别用ROS Jazzy，这玩意有些区别，特别是Gazebo,几乎没啥教程的。

Ubuntu Jammy 22.04(LTS)请安装ROS Humble(LTS)。安装向导网站:https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html

Ubuntu Noble 24.04(LTS)请安装ROS Jazzy(LTS)。安装向导网站:https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debians.html

各个ROS1、ROS2版本所支持的发行版：https://www.ros.org/reps/rep-2000.html

进入ROS官网

一定要安装LTS版本的ROS2

安装ROS2前期准备工作

装LTS版本的ROS2点击使用debian包管理工具安装ROS2二进制文件

按照ROS2官网教程在终端里输入如下命令

依旧输入以下命令启动Ubuntu Universe Repo

换源(国内源)

这个是官网的源，是从国外服务器下载ROS2二进制文件的，对网络有要求，所以我们要给改为国内源。

百度搜索北京外国语大学镜像源(亲测，北方最快的源)

找到ROS2，然后勾上sudo和https，选择正确的Linux发行版。在使用该命令前有可能会遇到权限不够的问题，可以先往下看，如何以root权限运行所有命令。

先输入sudo passwd设置root密码（Ubuntu系发行版要干的事）（debian,Fedora等发行版在装机时就已经设置过了，无需运行）

sudo passwd

su -

然后进入管理员权限下

挨行复制输入运行

上面下载key的时候如果卡住了，那就是github又抽风了，很正常的问题，可以选择科学*上网(科学机场教程)解决，如果还解决不了，可以通过下方教程修改hosts。

https://blog.csdn.net/qq_40584960/article/details/117963644?sharetype=blog&shareId=117963644&sharerefer=APP&sharesource=qq_33274985&sharefrom=link

成功下载key之后，继续往下弄。

输入exit退出root模式

exit

通过apt安装ROS2

把以下红色框框的全部在终端里敲一遍

国内的镜像下载还是非常快的。

ROS2基础环境变量配置

复制该行

输入以下命令

sudo apt-get install vim
sudo vim ~/.bashrc

在最底部把这行加上（vim不会使用的，请自行百度）

刷新以下当前终端的环境变量

echo 'export ROSDISTRO_INDEX_URL=https://mirrors.bfsu.edu.cn/rosdistro/index-v4.yaml' >> ~/.bashrc
source ~/.bashrc

测试ROS2

请往下翻到测试ROS2章节

从源码安装ROS2(难度较高，Ubuntu用上面那个简单方法即可。)
Fedora或者Rocky

https://www.ros.org/

先进官网,选择对应的版本,比如我这里选择ROS Jazzy.

然后选择用源码进行编译,找到对应的发行版,比如我这里是Feodra.

然后根据官网教程来就行

设置区域

确保你的 locale 支持 UTF-8。如果你在一个最小的环境（比如 docker 容器）中，locale 可能是 C 等最小的东西。我们使用以下设置进行测试。但是，如果您使用的是其他支持 UTF-8 的区域设置，应该没问题。

locale  # check for UTF-8

sudo dnf install langpacks-en glibc-langpack-en
export LANG=en_US.UTF-8

locale  # verify settings

启用必须的仓库

Fedora无需额外启用仓库,RHEL需要.

安装开发工具

sudo dnf install -y \
  cmake \
  gcc-c++ \
  git \
  make \
  patch \
  python3-colcon-common-extensions \
  python3-mypy \
  python3-pip \
  python3-pydocstyle \
  python3-pytest \
  python3-pytest-cov \
  python3-pytest-mock \
  python3-pytest-repeat \
  python3-pytest-rerunfailures \
  python3-pytest-runner \
  python3-rosdep \
  python3-setuptools \
  python3-vcstool \
  wget

# install some pip packages needed for testing and
# not available as RPMs
python3 -m pip install -U --user \
  flake8-blind-except==0.1.1 \
  flake8-class-newline \
  flake8-deprecated

构建ROS2

获取ROS2源码

找一个要存放ROS2的文件夹,建议在/home分区找,然后在这个文件夹下打开终端.

mkdir -p ./ros2_jazzy/src
cd ./ros2_jazzy
vcs import --input https://raw.githubusercontent.com/ros2/ros2/jazzy/ros2.repos src

更新系统

ROS 2 软件包构建在经常更新的红帽系系统上。始终建议您在安装新软件包之前确保您的系统是最新的。

sudo dnf upgrade

初始化并更换rosdep源

https://mirrors.bfsu.edu.cn/help/rosdistro/

# 手动模拟 rosdep init
sudo mkdir -p /etc/ros/rosdep/sources.list.d/
sudo curl -o /etc/ros/rosdep/sources.list.d/20-default.list -L https://mirrors.bfsu.edu.cn/github-raw/ros/rosdistro/master/rosdep/sources.list.d/20-default.list
# 加入环境
echo 'export ROSDISTRO_INDEX_URL=https://mirrors.bfsu.edu.cn/rosdistro/index-v4.yaml' >> ~/.bashrc
# 加载.bashrc
source ~/.bashrc
# 更新rosdep
rosdep update

通过rosdep安装依赖

export ROS_PYTHON_VERSION=3
    
rosdep install --from-paths src --ignore-src -y --skip-keys "fastcdr rti-connext-dds-6.0.1 urdfdom_headers"

如果出现上面这种错误,使用pip3安装包.

sudo pip3 install flake8-docstrings

然后,使用下面命令忽略掉flake8-docstrings.

rosdep install --from-paths src --ignore-src -y \
--skip-keys "fastcdr rti-connext-dds-6.0.1 urdfdom_headers python3-flake8-docstrings"

显示成功即可

配置环境(不是红帽系的发行版不用弄)

如果你是红帽系系统需要下面配置

可以用下面这些命令暂时定义这些宏。

export RPM_PACKAGE_NAME=qt_gui_cpp   # 根据包名调整（如 ros_<package>）
export RPM_PACKAGE_VERSION=1.0.0
export RPM_PACKAGE_RELEASE=1
export RPM_ARCH=$(uname -m)

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 变量名 | 示例值 | 用途 | |:—|:—|:—| | RPM_PACKAGE_NAME | qt_gui_cpp | 定义 RPM 包名 | | RPM_PACKAGE_VERSION | 1.0.0 | 定义 RPM 包版本 | | RPM_PACKAGE_RELEASE | 1 | 定义 RPM 包发行号 | | RPM_ARCH | x86_64 或 arm64 | 定义目标系统架构 |

使用colcon构建源码

colcon build --symlink-install

等待构建完毕,下方就是构建完毕且没错误的样子

配置基础环境

找到下方文件夹,并复制路径,比如我的是~/UserFloder/Applications/ros2_jazzy

使用vim修改bash的环境

vim ~/.bashrc

使用Insert按键进行输入

添加完这两行后,按ESC,然后:wq保存并退出.

# 刷新当前环境变量
source ~/.bashrc

测试ROS2

请往下翻到测试ROS2章节

Debian

https://www.ros.org/

先进官网,选择对应的版本,比如我这里选择ROS Jazzy.

然后选择用源码进行编译,找到对应的发行版,比如我这里是Debian.

接下来，从下图这里开始，跟着官方教程一路敲，但是有几个需要注意的点，我这里都会写出来：

设置区域

locale  # check for 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

locale  # verify settings

换源

红色部分是Ubuntu要做的，我们是debian,不需要做。

蓝色部分我们不要用官方的，官方是国外源，我们替换成国内源。

先去镜像源换源：

https://mirrors.bfsu.edu.cn/help/ros2/

比如我是debian12：

sudo apt install curl gnupg2
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key  -o /usr/share/keyrings/ros-archive-keyring.gpg

echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] https://mirrors.bfsu.edu.cn/ros2/ubuntu bookworm main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update

上图成功换源后，再安装ros的工具：

sudo apt update && sudo apt install -y \
  python3-flake8-blind-except \
  python3-flake8-class-newline \
  python3-flake8-deprecated \
  python3-mypy \
  python3-pip \
  python3-pytest \
  python3-pytest-cov \
  python3-pytest-mock \
  python3-pytest-repeat \
  python3-pytest-rerunfailures \
  python3-pytest-runner \
  python3-pytest-timeout \
  ros-dev-tools

下载ROS2源码

找一个要存放ROS2的文件夹,建议在/home分区找,然后在这个文件夹下打开终端.

mkdir -p ./ros2_jazzy/src
cd ./ros2_jazzy
vcs import --input https://raw.githubusercontent.com/ros2/ros2/jazzy/ros2.repos src

补依赖之前，我们需要先初始化rosdep并给rosdep换源：

图中红色部分，不要跟着这个来，这个是官方源，会很慢很慢，建议用bfsu的仓库。

https://mirrors.bfsu.edu.cn/help/rosdistro/

# 手动模拟 rosdep init
sudo mkdir -p /etc/ros/rosdep/sources.list.d/
sudo curl -o /etc/ros/rosdep/sources.list.d/20-default.list -L https://mirrors.bfsu.edu.cn/github-raw/ros/rosdistro/master/rosdep/sources.list.d/20-default.list
# 加入环境
echo 'export ROSDISTRO_INDEX_URL=https://mirrors.bfsu.edu.cn/rosdistro/index-v4.yaml' >> ~/.bashrc
# 加载.bashrc
source ~/.bashrc
# 更新rosdep
rosdep update

出现以上红框部分，就是成功换源了。如果你不换源，下面补依赖的过程可能会巨慢（我具体没试过，猜测），除非你科学了。

sudo apt upgrade

export ROS_PYTHON_VERSION=3

rosdep install --from-paths src --ignore-src -y --skip-keys "fastcdr rti-connext-dds-6.0.1 urdfdom_headers"

编译ROS2

在ros_jazzy目录下，打开终端，跑下面命令。

colcon build --symlink-install

colcon build还有以下命令，如–packages-skip和–packages-select用来跳过和单个功能包编译。

等待构建完毕,下方就是构建完毕且没错误的样子

配置基础环境

找到下方文件夹,并复制路径,比如我的是~/UserFloder/Applications/ros2_jazzy

使用vim修改bash的环境

vim ~/.bashrc

使用Insert按键进行输入

# 配置ROS2环境
export ROSDISTRO_INDEX_URL=https://mirrors.bfsu.edu.cn/rosdistro/index-v4.yaml
source ~/UserFloder/Applcations/ros2_jazzy/install/setup.bash

添加完这两行后,按ESC,然后:wq保存并退出.

# 刷新当前环境变量
source ~/.bashrc

测试ROS2

请往下翻到测试ROS2章节

Docker安装ROS2

Docker教程

ROS2安装进阶(可选)

上面只是安装了最基本的ROS2包和配置了最基本的ROS2环境。

你安装完上面的内容后，可以选择接着进行下面的进阶配置ROS2环境。

也可以不安装进阶得东西，学到啥再安装啥就可以，当然也有想一劳永逸的同学，所以下面也会列出来最常用的ros2包和环境配置供大家一次性安装。

https://index.ros.org/?search_packages=true

安装进阶的包

以下是一些ROS2自带的导航算法和仿真环境等等的包，下方将列出这些包。

如果你是Ubuntu的话：

下面的包你全都可以用apt安装，不用手动编译！！！

如果你不是Ubuntu的话：

灰色的是上面基础教程安装过的包，

黄色是需要手动编译的包，

蓝色是不仅要手动编译，还要手动修改一些源码的包。

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | ROS的包（非Ubuntu的发行版基本都需要手动编译） | |:—| | 软件包/库名称 | 功能描述 | | ros-humble-desktop | ROS 2 Jazzy的核心桌面安装包，包含基础工具、客户端库和常用功能包，适用于桌面开发环境。 | | ros-humble-xacro | 用于简化URDF（机器人描述文件）的宏语言工具，支持定义常量、数学运算和代码复用，提升可维护性。 | | ros-humble-joint-state-publisher | 发布机器人关节状态（如角度、速度）的ROS节点，支持硬件接口与仿真环境的数据同步。 | | ros-humble-joint-state-publisher-gui | 带图形界面的关节状态发布工具，可通过GUI手动调整关节参数，常用于调试和仿真可视化。 | | ros-humble-gazebo-ros | 老版Gazebo Classic | | ros-humble-gazebo-ros-pkgs | 老版Gazebo Classic的一些插件 | | ros-humble-ros-gz | ROS2与新版Gazebo仿真的包，支持ROS 2与Gazebo（如Harmonic版本）的集成，用于传感器模拟和物理引擎交互。 | | ros-humble-diagnostic-updater | 提供机器人系统诊断功能的工具，用于监控硬件状态、频率异常等，并通过ROS主题发布诊断信息。 | | ros-humble-teleop-twist-keyboard | ROS2的键盘控制节点 | | ros-humble-navigation2 | ROS 2的导航框架（Nav2），包含路径规划（全局/局部）、避障、行为树控制等功能，支持动态环境下的自主移动。 | | ros-humble-nav2-bringup | Nav2的启动和配置工具包，提供预定义的启动文件，简化导航系统的部署流程。 | | ros-humble-slam-toolbox | SLAM（同步定位与建图）工具包，支持实时地图构建与优化，兼容激光雷达和深度相机数据，适用于动态环境。 | | ros-humble-cartographer | Google开源的SLAM算法实现，专注于高精度2D/3D建图，适用于大规模环境。 | | ros-humble-cartographer-ros | Cartographer算法的ROS封装包，提供与ROS 2的数据接口和启动配置。 | | ros-humble-asio-cmake-module | 用于集成ASIO（异步I/O库）的CMake模块，支持ROS中网络通信和异步操作的开发。 | | ros-humble-serial-driver | 串口通信驱动包，支持通过串口与硬件设备（如传感器、控制器）进行数据交互。 | | ros-humble-pcl-ros | 点云库（PCL）的ROS接口，提供点云数据处理、滤波、配准等功能，常用于3D感知任务。 | | ros-humble-pointcloud-to-laserscan | 将三维点云转换为二维激光扫描。这对于使Kinect等设备看起来像基于2D算法的激光扫描仪（例如基于激光的SLAM）非常有用。 | | ros-humble-vision-opencv | OpenCV与ROS的集成工具包，支持图像处理、特征提取、相机标定等计算机视觉任务。 |

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 非ROS包（基本都可以apt,dnf安装） | |:—| | 软件包/库名称 | 功能描述 | | libpcl-dev | 点云库（PCL）的开发文件，提供点云数据结构的核心算法（如分割、配准、滤波）。 | | libeigen3-dev | Eigen数学库的开发文件，用于矩阵运算、几何变换和数值计算，是SLAM和运动规划的基础依赖。 | | libpcap-dev | 网络数据包捕获库的开发文件，支持底层网络通信协议解析，常用于传感器数据流的实时捕获。 | | libboost-dev | 这个库是一个巨型库，里面有很多东西，比如Linux的串口，但是由于ROS2的串口库就是基于boost库的，所以该库不安装的话也可以正常跑，ROS2跑的是它自带的boost底层。 | | libogre-1.12-dev | Gazebo的渲染引擎，在debian12中用libogre-dev搜不到，必须用libogre-1.12-dev才能搜到。 |

但是上面这些包可以依赖于其他的包，下面这个仓库里有全部的包的仓库链接：（往后看就知道怎么用了）

https://github.com/ros/rosdistro/tree/master

apt安装方式（Ubuntu强烈建议）

sudo apt update && sudo apt install \
    ros-humble-desktop \
    ros-dev-tools \
    ros-humble-xacro \
    ros-humble-joint-state-publisher \
    ros-humble-joint-state-publisher-gui \
    ros-humble-ros-gz \
    ros-humble-gazebo-ros \
    ros-humble-gazebo-ros-pkgs \
    ros-humble-imu-tools \
    ros-humble-diagnostic-updater \
    ros-humble-teleop-twist-keyboard \
    ros-humble-navigation2 \
    ros-humble-nav2-bringup \
    ros-humble-slam-toolbox \
    ros-humble-cartographer \
    ros-humble-cartographer-ros \
    ros-humble-asio-cmake-module \
    ros-humble-serial-driver \
    ros-humble-pcl-ros \
    ros-humble-vision-opencv \
    ros-humble-pointcloud-to-laserscan \
    libpcl-dev \
    libeigen3-dev \
    libpcap-dev \
    python3-colcon-common-extensions

源码方式（非Ubuntu）

以debian12为例。

黄色的包

以安装ros-jazzy-xacro，ros-jazzy-joint-state-publisher，ros-jazzy-joint-state-publisher-gui这三个包为例，其他的包一样操作。

https://index.ros.org/?search_packages=true

进入上面的网站，在下图中选择对应版本，比如我是JAZZY，然后在搜索框中搜索xacro，joint_state_publisher，joint_state_publisher_gui

比如xacro

找到对应版本的xacro，然后仓库地址是checkout URI，分支是vcs version,你可以使用git clone一个一个克隆下来，但这是巨麻烦的一件事，所以我们选择ROS2提供给我们的vcs命令进行下载包的源码。

首先，进入自己的ros2_jazzy文件并打开一个终端

并创建一个.repos文件，文件名随便起，比如我的叫my_ros2_jazzy_packages.repos

然后用vscode编辑，因为.repos使用的是yaml语言，vscode可以识别语法正确性：

将刚才查询的信息写上去：

repositories:
  xacro:
    type: git
    url: https://github.com/ros/xacro.git
    version: ros2

用同样的方式找到其他包：

上图说明joint_state_publisher和joint_state_publisher_gui其实是一个仓库，所以我们只需要克隆一个仓库。

然后vcs version也就是git的分支是ros2。

repositories:
  xacro:
    type: git
    url: https://github.com/ros/xacro.git
    version: ros2
  joint_state_publisher:
    type: git
    url: https://github.com/ros/joint_state_publisher.git
    version: ros2

其他的包如法炮制。

但我先以这俩包做个例子。

在ros2_jazzy目录下的终端：

vcs import src < my_ros2_jazzy_packages.repos

可以看到下图，克隆成功了

接下来进行补充依赖：

rosdep install --from-paths src --ignore-src -y --skip-keys "fastcdr rti-connext-dds-6.0.1 urdfdom_headers"

提示下图字样，即是成功

再编译安装即可：

colcon build --symlink-install
# 或仅编译特定包
colcon build --packages-select xacro joint_state_publisher joint_state_publisher_gui

其他的包如法炮制。

测试：

# 重新加载环境
source ~/.bashrc
# 敲命令
xacro

如下图xacro提示error即是成功，这样就是安装成功了！

其他的包如法炮制。

我也整了Humble和Jazzy的repos的文件，从官网和distribution.yaml一个个找的，你可以克隆下来直接用我整理的就可以。(但有可能部分仓库分支ROS2官方会做出修改)

https://github.com/tungchiahui/ros-docker/tree/main/repos

蓝色的包

蓝色的包大体的方式是和黄色一样的，只不过蓝色的包在下载完源码后，还需要修改下源码里的文件，再进行编译。

如vision_opencv的cv_bridge:请看下方OpenCV章节的CV_Bridge。

常见问题
libogre-dev找不到

Gazebo的渲染引擎，在debian12中用libogre-dev搜不到，必须用libogre-1.12-dev才能搜到。

nav2_mppi_controller里有个被当成空指针了，警告成错误了。

解决在nav2_mppi_controller的cmakelists里添加上add_compile_options(-Wno-error=null-dereference)即可。

nav2_system_tests里有个内存重叠警告。

解决在nav2_system_tests的cmakelists里添加上下面的几行即可。

if (CMAKE_CXX_COMPILER_ID MATCHES "GNU")
    add_compile_options(-Wno-restrict)
endif()

有的库在Fedora最新版没有，在旧版有。

https://src.fedoraproject.org/projects/rpms/%2A

sudo dnf install python3-pykdl --releasever=41
sudo dnf install orocos-kdl-devel --releasever=41

有些python库在最新版系统上下不了，因为python版本过高

可以使用pip3安装该库

pip3 install flake8-docstrings

但后续编译的话，需要把该库加入rosdep忽略的依赖。比如python3-flake8-docstrings的话就是下面这行：

rosdep install --from-paths src --ignore-src -y --skip-keys "fastcdr rti-connext-dds-6.0.1 urdfdom_headers"
# 改为
rosdep install --from-paths src --ignore-src -y --skip-keys "fastcdr rti-connext-dds-6.0.1 urdfdom_headers python3-flake8-docstrings"

环境进阶
Apt方式

# rosdep换源
export ROSDISTRO_INDEX_URL=https://mirrors.bfsu.edu.cn/rosdistro/index-v4.yaml
# 加载ROS2环境
source /opt/ros/jazzy/setup.bash
# 配置ROS2的分布式
export ROS_DOMAIN_ID=6
# 配置新版Gazebo的资源（这里先注释，等你学到gazebo再打开）
#export GZ_SIM_RESOURCE_PATH=$GZ_SIM_RESOURCE_PATH:/home/tungchiahui/UserFloder/MySource/ROS_WS/gazebo_models:/home/tungchiahui/UserFloder/MySource/ROS_WS/ign_models

注意，Gazebo资源包在不同ROS2版本可能宏的名称不同，如在ROS2Humble里是IGN_GAZEBO_RESOURCE_PATH，在ROS2Jazzy里是GZ_SIM_RESOURCE_PATH。（往后的ROS2版本估计都是GZ_SIM_RESOURCE_PATH）（详见下方Gazebo教程）

源码方式

与apt方式相同，但是需要把加载ROS2环境的那个setup.bash路径设置为你的ros2安装路径。

# 加载ROS2环境
source ~/UserFloder/Applcations/ros2_jazzy/install/setup.bash

测试ROS2
示例1

打开两个终端，并分别输入如下两个红框框里的命令

弹出如下程序，则测试成功。

对着终端按Ctrl+C可以终止程序。

示例2

乌龟作为ROS老测试项目了，必须拉出来溜溜！

同样开两个终端，并分别输入以下命令。

ros2 run turtlesim turtlesim_node
ros2 run turtlesim turtle_teleop_key

鼠标点以下红色部分，然后通过小键盘的方向键可以控制乌龟运行，如果没有小键盘，可以尝试GBVCDERT等按键操控小乌龟。

同样地，对着终端按Ctrl+C可以终止程序。

常见问题
Rviz2闪烁

如果2K+的电脑安装完rviz2有显示问题，可以在.bashrc里加上下面的内容，用来让qt不缩放。

export QT_AUTO_SCREEN_SCALE_FACTOR=0
export QT_SCREEN_SCALE_FACTORS=[1.0]

入门操作
简介

终端环境搭建

如果在上方安装ROS2的时候，已经将该语句添加到~/.bashrc了，则不必再跟着这步操作了。

source /opt/ros/humble/setup.bash  #将ROS2环境变量配置到当前位置
echo " source /opt/ros/humble/setup.bash" >> ~/.bashrc    #每次启动终端都会运行该句

ros2 run demo_nodes_cpp talker

用ctrl+c来进行取消程序运行

ros2 run demo_nodes_py listener

ros2 run turtlesim turtlesim_node
ros2 run turtlesim turtle_teleop_key

命令行操作

mkdir -p 新建文件夹

rm -R 递归删除（删掉文件夹及里面包含的文件夹及文件）

touch 新建文件

rm 删除文件

cd ..退回上级目录（cd 点点）

会弹提示信息，告诉我们后面要跟的参数

ros2 node list会把当前ROS2正在运行的节点列出来

ros2 node info + /节点名可以查看目标节点的详细情况

会弹提示信息，告诉我们后面要跟的参数

可以通过话题来显示机器人运动的状态

ros2 topic pub --rate 1 /turtle1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.8}}"

ros2 service call /spawn turtlesim/srv/Spawn "{x: 2, y: 2, theta: 0.2, name: ''}"

ros2 bag record /turtle1/cmd_vel
ros2 bag play rosbag2_2022_04_11-17_35_40/rosbag2_2022_04_11-17_35_40_0.db3

ros2 bag record + 话题

按Ctrl+C结束，然后录制的数据在当前终端的目录下

如何去复现呢？

ros2 bag play + 文件夹名称

ROS2 HelloWorld(C++)

1.创建功能包

指令就是创建ros2的功能包

ros2 pkg create + 功能包名 + –build-type(构建类型) + ament_cmake / ament_python + –dependencies（依赖） + rclcpp(ROS2的CPP客户端) + –node-name（节点名） + 节点名

ros2 pkg create pkg01_helloworld_cpp --build-type ament_cmake --dependencies rclcpp --node-name helloworld

源文件自动生成了，文件名和我们指定的node name是一致的。

这是自动生成的内容，但是和ROS2没有任何关系。

如果依赖的库不止这一个，则再回车，

xxx

再添加下一个

10行是查找包

12行是添加可执行的

add_executable 的第一个参数是可执行文件的名字（默认和节点名一致，默认和源文件名一致）第二个参数是源文件的名字

17行是为我们的可执行程序添加依赖我们的可执行程序依赖于RCLCPP这个库

22行是要为我们的可执行程序设立一个安装目录，创建在了当前功能包下的lib目录，也就是工作空间名/install/功能包名/lib

编辑配置文件之后编译，用cd..返回ws目录

图标为绿色，是没有警告也没有错误

是黄色的则有警告

是红色的则有致命错误

可执行二进制文件的路径

source install/setup.bash #刷新环境变量

ros2 run pkg01_helloworld_cpp helloworld

ros2 run 功能包名称可执行文件名(默认和节点名一致)

编辑ROS2 C++源文件：

#include "rclcpp/rclcpp.hpp"

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  auto node = rclcpp::Node::make_shared("helloworld_node");

  RCLCPP_INFO(node->get_logger(),"hello world!");

  rclcpp::shutdown();

  return 0;
}

ROS2 HelloWorld(Python)

ros2 pkg create pkg02_helloworld_py --build-type ament_python --dependencies rclpy --node-name helloworld

ros2 pkg create + 功能包名 + –build-type(构建类型) + ament_cmake / ament_python + –dependencies（依赖） + rclpy(ROS2的Python客户端) + –node-name（节点名） + 节点名

与node name和可执行二进制文件同名

默认这里面已经有代码，但是和ROS2无关，这是标准的python代码

二进制可执行文件映射到源文件的main函数

如何编译呢？

先返回上一级，来到ws目录

有个黄色警告，但是不影响我们使用。

source ./install/setup.bash

刷新环境变量

ros2 run pkg02_helloworld_py helloworld

import rclpy

def main():
    rclpy.init()
    node = rclpy.create_node("helloworld_py_node")
    node.get_logger().info("hello world by python!")
    rclpy.shutdown()

if name == '__main__':
    main()

运行优化(bash终端环境)

要使用绝对路径

尽量不要这么干，ROS2有一个bug，就是不同工作空间的功能包可能会调用混乱，所以先不要搞全局的运行优化。

VScode环境搭建

看C/C++，Python，CMake，XML,YAML文件就可以代码高亮显示

在写一些ROS2消息的代码可以提供代码补齐等操作

编写机器人模型所要用的插件，也可以进行代码补齐

ROS2经常生成PDF文件，可以通过这个插件来查看

ROS2插件建议等成熟之后再进行安装

这个官方插件可以尝试安装

人工智能代码补全

MarkDown高亮

虽然报错，但是程序是可以正常运行的。（主要是vscode找不到头文件）

            "includePath": [
                "${default}",
                "${workspaceFolder}/**",
                "/opt/ros/humble/include/**"
            ],

/**代表要包含该文件夹下的所有的子集

按Ctrl + `（ESC底下的按键）把VSCODE终端打开

--node-name也是可选参数，如果不配置，则不会有源文件，也不会有可执行文件到源文件的映射

不需要修改，已经默认生成好了

再返回WS目录进行编译（但是此时编译是编译整个WS目录下的所有功能包）

刷新环境变量并运行

如何在一个功能包里添加多个源文件呢？

新建一个新文件，比如hellovscode2.cpp

但是此时该文件是一个孤零零的文件，他没有做任何的配置，对应的，编译完之后也不会被执行。

我们想编译执行该文件，必须配置相关的配置文件。

选中的这些用不着，可以删掉

运行优化(colcon build)

平常会全编译WS目录下的文件

colcon build --packages-select xxx xxx xxx #可以指向多个包

VScode环境进阶
clangd插件代码提示(可选,但是建议)

https://colcon.readthedocs.io/en/released/index.html

由于C/C++插件在大项目里的表现简直拉胯的一批，所以我们选择使用llvm里的clangd插件来进行代码提示。

但clangd依赖于cmake生成一个编译信息文件，我们需要一些步骤来生成该文件。

由于ROS2没有像ROS1那样的一个总的规范的CMakeLists，所以配置起来没有ROS1那么方便。

配置colcon build参数

方法一：（不建议）每次编译要用该命令：

colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON

等同于在cmake文件里写上（一般不建议改cmakelists）

set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

方法二：全局参数(更加推荐)

mkdir ~/.colcon

vim ~/.colcon/defaults.yaml

按下insert（插入）按键

输入下方内容

build:
  cmake-args:
    - -DCMAKE_EXPORT_COMPILE_COMMANDS=ON

按下ESC，并按下:wq,然后按下Enter(回车)即可成功保存。

在编译的时候正常用colcon build就可以自动启用CMAKE_EXPORT_COMPILE_COMMANDS=ON参数了。

然后再来配置clangd插件
1. 先下载clangd插件
2. 下载clangd文件
按住Ctrl shift P打开搜索框

输入clangd 找到下载语言服务器这一项目，点击安装clangd（请保持良好的网络状况）
1. 接着配置clangd：

禁用C/C++的代码提示功能

如果没有上图的弹窗，可以进行手动关闭，依然是ctrl shift P,输入settings然后找到如下图的选项

找到下图这个选项，改成disabled即可。

"C_Cpp.intelliSenseEngine": "disabled"

重启clangd

然后ctrl shift P搜索clangd找到如下图的选项（重启clangd语言服务器前，要先colcon build）

代码提示就正常啦

安装其他工具

安装terminator(选装，建议，因人而异）

sudo apt install terminator

1，Ctrl+Shift+e 垂直平分窗口

这样你就有了两个独立的工作区见，对于大屏幕的人是个不错的选择。

2，Ctrl+Shift+o 水平平分窗口

如果你分了两个垂直的工作区间之后发现宽度不够了怎么办，没关系，试试把其中一个做水平分割，恩，很符合美学（呲牙笑）。

3，Ctrl+Shift+s 隐藏/恢复滚动条

一般到了这里，我会去掉鸡肋的滚动条，果然清爽了很多，至简至上，如果你想念它了，没关系再按一次滚动条就愉快的滚回来了。

4，F11 进入退出全屏幕

到了这里你会发现，如果屏幕不够大，终端显示都挤在了一起，这时你需要一键切换退出全屏幕，用F11体验一下满眼终端时专注敲代码的感觉吧。

5，Ctrl+Tab 在不同的工作区间循环

经过上面的操作，不出意外的话，我们现在已经有了三个工作区间，在一个工作区间敲完了代码，之后想跑到另一个工作区间执行，又不想用鼠标点击怎么办，利用Ctrl+Tab你可以在不同的工作区间进行循环，以便逐个区间进行操作。

6，Ctrl+l 清空当前窗口

当我们把窗口写满时，就只能在整个窗口的下面敲击命令，这时我们需要这个命令把窗口清空一下，但其实我们并不是把它真正的清空了，只是相当于翻到了一个新页面一样，如果我们用鼠标往上滚动时会发现是可以看到之前的内容的。

7，Ctrl+Shift+w 关闭当前工作区间

我们上面打开了三个工作区间，当我们不需要操作又嫌弃它占用了无用的空间时，我们就可以使用这一快捷方式把它快速关闭。

8，Ctrl+Shift+q

这也是我比较喜欢的命令，当你关掉只剩最后一个窗口时就用这一方式退出终端吧，相信我这比你用鼠标点击退出要要快的多。

还有一些通用的终端复制粘帖方式我就不多说了，当然还有一些其他很有用的命令，但是我在实际的工作中并没有用到，也就是掌握了这几个快捷方式已经完全满足了我日常的工作需求。

咱们的Git入门教程：Vinci机器人队Git入门教程

ROS2体系框架

Client Library就是ROS2的客户端，比如rclcpp，rclpy。

Abstract DDS Layer是DDS抽象层，这样DDS可以实现可插拔，可以随便替换DDS模块。

Intra-process API是进程内通讯API，可以提高通信效率的一类API。

工作空间与功能包
工作空间简介

工作空间里有4个子空间

未来编写的代码和脚本都需要人为的放入src空间里，

编译所形成的中间文件会存放到build空间里，

可执行文件会存放到install空间里，

编译过程以及运行之后各种警告，错误信息等会存放到log空间里。

用pip工具可以很方便的安装各种python的包

国内开发者@鱼香ROS 开发的一个专门处理ROS2依赖的工具

实际上就是扫描了各个功能包里的package.xml里的depend，然后查找本机是否含有该依赖，再决定是否安装。

sudo rosdepc init
rosdepc update
cd ..
rosdepc install -i --from-path src --rosdistro humble -y  #在src文件里看功能包所需依赖并查找安装

后面还需要创建好几个目录，这些目录大多都是和接口文件相关的。

源文件编译

#include "rclcpp/rclcpp.hpp"

class MyNode: public rclcpp::Node
{
  public:
    MyNode():Node("node_name")
    {
      RCLCPP_INFO(this->get_logger(),"hello world!");
    }
};

int main(int argc,char *argv[])
{
  rclcpp::init(argc,argv);
  auto node = std::make_shared<MyNode>();
  rclcpp::shutdown();
  return 0;
}

实例化只能一个进程组织一个节点，

而继承可以组织多个节点。

初始化和资源释放的作用是什么？

可以往context对象里放数据，也可以取数据，类似于FreeRTOS里的消息队列，但也不完全类似，它可以存储之前的数据。

初始化不是仅仅只创建context对象，这是它的其中一个功能，它还有其他比较重要的功能。

先初始化父类构造，并传入一个节点名称。

智能指针忘记的可以看Vinci机器人队C/C++资料

智能指针是一种自动管理内存的指针，它会在不需要对象时自动释放内存。使用智能指针可以避免内存泄漏和空悬指针等问题。

最安全的分配和使用动态内存的方法是调用一个名为 make_shared 的标准库函数。此函数在动态内存中分配一个对象并初始化它，返回指向此对象的 shared_ptr。与智能指针一样，make_shared 也定义在头文件 memory 中。

当要用 make_shared 时，必须指定想要创建的对象的类型。定义方式与模板类相同，在函数名之后跟一个尖括号，在其中给出类型：

// 指向一个值为42的int的shared_ptr
shared_ptr<int> p3 = make_shared<int>(42);

// p4 指向一个值为"9999999999"的string
shared_ptr<string> p4 = make_shared<string>(10,'9');

// p5指向一个未初始化的int
shared_ptr<int> p5 = make_shared<int>();
//当然，我们通常用 auto 定义一个对象来保存 make_shared 的结果，这种方式较为简单：

// p6指向一个动态分配的空vector<string>
auto p6 = make_shared<vector>();

用make_shared可以分配堆区内存。

配置文件

C++

Python

name是功能包名称

version是包的版本

description是描述包的信息，也就是包是干嘛的

email是维护者的邮箱地址

license是我们的功能包使用的软件协议

不建议直接重命名功能包的名字，当修改了文件夹名称，则里面很多文件里的配置内容也需要被修改。建议重新建。

常用操作命令

ament_cmake是cmake的增强版

-h 是查看帮助信息

ros2 pkg executables #输出当前系统可执行的功能包和节点

ros2 pkg executables 功能包名         #是输出当前包下可执行的功能包和节点

ros2 pkg list #是输出当前系统可执行的功能包

ros2 pkg prefix + 功能包名   #是输出该功能包的路径(重要，后面经常要用)

ros2 pkg xml + 功能包名   #是输出该功能包里的packages.xml的内容

核心模块_通信相关

通信模块被封装进了功能包

比如（例子）

会搜到巨多的内容，可以用grep进一步搜索

下载分支用 -b

有个警告，因为有两个功能包都叫humble了

如果允许覆盖需要加参数 –allow-overriding

也就是helloworld那样的

核心模块_工具相关

命令行在某些地方比图形化工具比较好用，在远程登录时只能用命令行

Lanuch文件现在在ROS2里是一个Python文件了。

Camera是摄像头位置，Base_Link是车体的位置，Laser是激光雷达的位置。

雷达检测的距离信息，会转化成车体的位置信息。

功能包

python是解析型语言，不用编译，但是需要setup.py文件，setup.py主要功能是把可执行文件移动到install的。

ROS2技术支持

ROS2_Wiki官网:

http://wiki.ros.org/

ROS2_Wiki中文官网(ROS2维基百科现已支持简体中文):

http://wiki.ros.org/cn

ROS2简体中文社区：

http://wiki.ros.org/cn/community

ROS2插件索引网址:

https://index.ros.org/packages/

ROS2应用方向

许多ROS团队伴随ROS成长到今日，其规模已经发展到足以被认为是独立组织的程度了。在导航、机械臂、无人驾驶、无人机等诸多领域大放异彩，下面列出了其中的一些团队项目，这些项目对我们以后的进阶发展，也提供了指导。

ROS2社区：

https://www.ros.org/blog/community/

NAV2

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

NAV2官网：

https://navigation.ros.org/

OpenCV

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

OpenCV官网：

https://opencv.org/

教程：OpenCV教程

MoveIt

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

https://moveit.ros.org/

The Autoware Foundation

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

TAF官网:

https://www.autoware.org/

F1 Tenth

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

F1 Tenth官网:

https://f1tenth.org/

microROS

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

MicroROS官网:

https://micro.ros.org/

Open Robotics

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

Open Robotics官网:

https://www.openrobotics.org/

PX4

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

PX4官网:

https://px4.io/

ROS-Industrial

ROS-Industrial是一个开源项目，将 ROS 软件的高级功能扩展到工业相关硬件和应用程序。

https://rosindustrial.org/

四大通信
通信机制简介与代码模板

#include "rclcpp/rclcpp.hpp"

class MyNode: public rclcpp::Node
{
  public:
    MyNode():Node("mynode_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"Hello World!");
    }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  rclcpp::spin(std::make_shared<MyNode>());

  rclcpp::shutdown();
  return 0;
}

https://snippet-generator.app/

{
"ROS2节点模板(C++)": {
    "prefix": "ros2_node_cpp",
    "body": [
      "#include \"rclcpp/rclcpp.hpp\"",
      "",
      "class MyNode: public rclcpp::Node",
      "{",
      "  public:",
      "    MyNode():Node(\"mynode_node_cpp\")",
      "    {",
      "      RCLCPP_INFO(this->get_logger(),\"Hello World!\");",
      "    }",
      "};",
      "",
      "int main(int argc, char ** argv)",
      "{",
      "  rclcpp::init(argc,argv);",
      "",
      "  rclcpp::spin(std::make_shared<MyNode>());",
      "",
      "  rclcpp::shutdown();",
      "  return 0;",
      "}"
    ],
    "description": "ROS2节点模板(C++)"
  }
}

第一个窗口是服务端，第二个窗口是客户端。

通信至少要涉及两方，只是一个人算不上通信。

面向接口，话题是一致的，数据载体也是一致的，就可以无缝对接

话题通信：只能单向传输数据

服务通信：双向通信，可以互为客户端和服务端，客户端给服务端发数据，服务端给客户端响应

动作通信：和服务通信很像，有服务端给客户端发的最终响应，但是中间也会连续发送反馈给客户端。

参数服务：参数服务是基于服务通信的，参数客户先发送一个请求，然后从参数服务的数据池里拿走数据。也可以更改数据池里的东西，但是不能删除。

参数通信不用自己定义接口文件，系统会自己弄接口文件，但是开发者是看不到该文件的，该文件被封装了。

我们操作的数据被封装成参数对象了。

ros2 pkg create + 功能包名（可以写在前面或者） + –build-type(构建类型) + ament_cmake / ament_python + –dependencies（依赖） + rclcpp(ROS2的CPP客户端) + –node-name（节点名） + 节点名

在src里创建功能包

话题通信_理论

在ROS1里，node和node之间通信需要经过master，每个传输数据的node都需要在master里注册相关数据，master再将信息进行匹配。

一个publisher发布数据，两个subscriber都会接收到数据。

话题通信_实验1(C++)

ros2 pkg create cpp01_topic --build-type ament_cmake --dependencies rclcpp std_msgs base_interfaces_demo

依赖还需要std_msgs，base_interfaces_demo（这里面存放了我们自己定义的所需的接口）

ros2 pkg create + 功能包名（可以写在前面或者最后面） + –build-type(构建类型) + ament_cmake / ament_python + –dependencies（依赖） + rclcpp(ROS2的CPP客户端) + –node-name（节点名） + 节点名

发布方

定时器是用来控制发送频率的，定时器里还会执行一个回调函数timer_callback。

count_是计数器，每执行一次这个回调函数，count_就++；

spin函数是，程序一旦执行到这里，就返回到上面，返回到上面是为了调用回调函数，如果没有这个spin函数，那么我们这个回调函数是不会被执行的。

以后只要我们创建完节点类对象指针，就要把该指针传入spin()函数里。

#include "rclcpp/rclcpp.hpp"

class Talker: public rclcpp::Node
{
  public:
    Talker():Node("talker_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"发布节点创建！");
    }
};


int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  rclcpp::spin(std::make_shared<Talker>());

  rclcpp::shutdown();
  return 0;
}

此时程序处于挂起状态，会一直运行，因为spin函数。

想结束得按Ctrl+C。

我们想要的类型在std_msgs里，所以要加头文件。

create_publisher()第一个入口参数是话题名称，是一个字符串

create_publisher()第二个入口参数是QOS服务质量有关的，是队列深度是一串数字，暂时可以先填10或者20等。

也就是当网络质量不好的时候，消息发不出去了，我们可以将数据先存到队列里，假设填10，最多就可以存10个数，当网络恢复了，我们就从队列里取数据，将其发出。

其他入口参数有默认值，可以暂时先不管。

返回值是一个publisher的指针。

创建定时器，这个函数有模板，但是模板有默认值可以不设置，

然后三个入口参数，

第一个入口参数是持续时间，也就是周期；

第二个入口参数是回调函数；

第三个入口参数有默认值，先不管。

使用该命名空间的优势是，在第一个入口参数，可以直接填时间+单位。

如果是1s就写1s，是100ms就填100ms。

该函数还有个返回值，返回值是定时器相关的一个指针。

该函数有多个重载，选择适合自己的一个函数。

发布对象得先创建对象。

把count转化成字符串并发送。

因为它是一个std::string类型的，我们要转化成c风格的字符串。

尽量在构造函数的时候，给count赋初值0.

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

using namespace std::chrono_literals;

class Talker: public rclcpp::Node
{
  public:
    Talker():Node("talker_node_cpp")，count(0)
    {
      RCLCPP_INFO(this->get_logger(),"发布节点创建！");
      publisher_ = this->create_publisher<std_msgs::msg::String>("chatter",10);
      timer_ = this->create_wall_timer(1s,std::bind(&Talker::on_timer,this));
    }
  private:
    void on_timer()
    {
      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)
{
  rclcpp::init(argc,argv);

  rclcpp::spin(std::make_shared<Talker>());

  rclcpp::shutdown();
  return 0;
}

但是不一定消息真的发布出去了。

验证方法：

ros2 topic echo /xxx

这样才能确定消息被发到chatter话题上了。

#include "rclcpp/rclcpp.hpp"

class Listener: public rclcpp::Node
{
public:
    Listener():Node("listener_node_cpp")
    {
        RCLCPP_INFO(this->get_logger(),"订阅方创建!");
    }
};


int main(int argc, char * argv[])
{
    rclcpp::init(argc,argv);

    rclcpp::spin(std::make_shared<Listener>());

    rclcpp::shutdown();
    return 0;
}

编译之前别忘了编辑配置文件

依赖包已经自动生成了，不用管。

主要改这三大部分

一共有5个入口参数，后面两个入口参数有默认值。

第一个入口参数是话题名称，要保证和发布方一致；

第二个入口参数是QoS，就是服务质量管理，队列深度，10或者20暂时随便设置，可以看看发布方那边的QoS的解释;

第三个入口参数是回调函数，一旦接收到数据，就触发该回调函数。

返回值是一个订阅对象的指针。

std::placeholders::_1这个是占位符，_1是指一个。这个地方本应该填入消息。

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

class Listener: public rclcpp::Node
{
public:
    Listener():Node("listener_node_cpp")
    {
        RCLCPP_INFO(this->get_logger(),"订阅方创建!");
        subscription_ = this->create_subscription<std_msgs::msg::String>("chatter",10,std::bind(&Listener::do_callback,this,std::placeholders::_1));

    }
private:
    void do_callback(const std_msgs::msg::String &msg)
    {
        RCLCPP_INFO(this->get_logger(),"订阅到的消息是:%s",msg.data.c_str());
    }
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
};


int main(int argc, char * argv[])
{
    rclcpp::init(argc,argv);

    rclcpp::spin(std::make_shared<Listener>());

    rclcpp::shutdown();
    return 0;
}

话题通信_自定义接口信息

构建依赖

执行依赖

当前功能包所属的功能包组

find_package是要把构建依赖传递过来

然后还要指定当前被构建的接口文件的路径（通过这个设置，就可以把.msg转化成对应的C++和Python代码了）

文件名可以自定义，但是首字母必须大写

写完之后test_depend报错了

删掉即可

编译依赖是rosidl开头的，我们通过grep查询一下

ros2 pkg list | grep -i rosidl

我们用的是这一个，直接复制过来

在list里这个所属的功能包组就没有了，需要自己写rosidl_interface_packages

这个依赖要和构建依赖一样

然后我们要为接口文件生成源码

需要使用rosidl_generate_interfaces函数

colcon build --packages-select base_interfaces_demo

会在install空间下生成student.hpp代码

以上.msg生成C++的

然后这个是.msg生成的Python的源码

也可以通过这个方式来检测是否编译正常

interface是接口

话题通信_实验2(C++)

新建完源文件之后，要配置CMakeLists

将最基本的框架直接复制过来

然后替换类的名称

#include "rclcpp/rclcpp.hpp"

class TalkerStu: public rclcpp::Node
{
  public:
    TalkerStu():Node("talkerstu_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"发布节点创建！");
    }
};


int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  rclcpp::spin(std::make_shared<TalkerStu>());

  rclcpp::shutdown();
  return 0;
}

为了编码规范，把talkerstu_node_cpp改成小写

同样的，订阅方也是需要这样操作

#include "rclcpp/rclcpp.hpp"

class ListenerStu: public rclcpp::Node
{
public:
    ListenerStu():Node("listenerstu_node_cpp")
    {
        RCLCPP_INFO(this->get_logger(),"订阅方创建!");
    }
};


int main(int argc, char * argv[])
{
    rclcpp::init(argc,argv);

    rclcpp::spin(std::make_shared<ListenerStu>());

    rclcpp::shutdown();
    return 0;
}

{
  "configurations": [
    {
      "browse": {
        "databaseFilename": "${default}",
        "limitSymbolsToIncludedHeaders": false
      },
      "includePath": [
        "/opt/ros/humble/include/**",
        "/home/tungchiahui/mysource/ros2src/3.ws01_plumbing/src/base_interfaces_demo/include/**",
        "/usr/include/**",
        "${workspaceFolder}/",
**        "${workspaceFolder}/install/base_interfaces_demo/include/**"
      ],
      "name": "ROS",
      "intelliSenseMode": "gcc-x64",
      "compilerPath": "/usr/bin/gcc",
      "cStandard": "gnu11",
      "cppStandard": "c++14"
    }
  ],
  "version": 4
}

不要忘记字符串转成C风格的。

#include "rclcpp/rclcpp.hpp"
#include "base_interfaces_demo/msg/student.hpp"

using namespace std::chrono_literals;

class TalkerStu: public rclcpp::Node
{
public:
    TalkerStu():Node("talkerstu_node_cpp"),age(0)
    {
      RCLCPP_INFO(this->get_logger(),"发布节点创建！");
      publisher_ = this->create_publisher<base_interfaces_demo::msg::Student>("chatter_stu",10);
      timer_ = this->create_wall_timer(500ms,std::bind(&TalkerStu::on_timer_callback,this));
    }
private:
    void on_timer_callback()
    {
        auto stu = base_interfaces_demo::msg::Student();
        stu.name = "葫芦娃";
        stu.age = age;
        stu.height = 2.20f;
        age++;
        publisher_->publish(stu);
        RCLCPP_INFO(this->get_logger(),"发布的消息:(%s,%d,%.2f)",stu.name.c_str(),stu.age,stu.height);
    }
    rclcpp::Publisher<base_interfaces_demo::msg::Student>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
    int32_t age;
};


int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  rclcpp::spin(std::make_shared<TalkerStu>());

  rclcpp::shutdown();
  return 0;
}

虽然发布方可以打印日志，但是不代表信息被正常发出去了。

这样检验才是真能确定数据被发送出去了。

#include "rclcpp/rclcpp.hpp"
#include "base_interfaces_demo/msg/student.hpp"

using base_interfaces_demo::msg::Student;

class ListenerStu: public rclcpp::Node
{
public:
    ListenerStu():Node("listenerstu_node_cpp")
    {
        RCLCPP_INFO(this->get_logger(),"订阅方创建!");
        Subscription_ = this->create_subscription<Student>("chatter_stu",10,std::bind(&ListenerStu::do_callback,this,std::placeholders::1));
    }
private:
    void do_callback(const Student &stu)
    {
        RCLCPP_INFO(this->get_logger(),"订阅的学生信息:name=%s,age=%d,height=%.2f",stu.name.c_str(),stu.age,stu.height);
    }
_    rclcpp::Subscription<Student>::SharedPtr Subscription_;
};


int main(int argc, char * argv[])
{
    rclcpp::init(argc,argv);

    rclcpp::spin(std::make_shared<ListenerStu>());

    rclcpp::shutdown();
    return 0;
}

话题通信_rqt查看计算图

图形化工具RQT

服务通信_理论

只能有一个服务端，可以有多个客户端，每个客户端都可以向服务端发送请求。（当然可以有多个服务端，但是会出很多逻辑问题，这是极其不合理的，禁止使用）

服务通信_实验1_服务端实现(C++)

先开服务端，然后从客户端提交两个整数，到服务端之后，服务端会解析数据，然后求和，并返回给客户端。

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

如果之前用过demo_interfaces_demo，那么一般是不用再配置package.xml了。

记得文件名首字母要大写！

这是另一个验证方式

#include "rclcpp/rclcpp.hpp"

class AddIntsServer: public rclcpp::Node
{
  public:
    AddIntsServer():Node("add_ints_server_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"服务端节点创建！");
    }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  rclcpp::spin(std::make_shared<AddIntsServer>());

  rclcpp::shutdown();
  return 0;
}

服务端是一直要挂起的

客户端是执行完毕就结束返回到终端的

所以客户端不用调用spin函数，直接创建对象即可。

#include "rclcpp/rclcpp.hpp"

class AddIntsClient: public rclcpp::Node
{
  public:
    AddIntsClient():Node("add_ints_client_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"客户端节点创建！");
    }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

//   rclcpp::spin(std::make_shared<AddIntsClient>());
  auto client = std::make_shared<AddIntsClient>();

  rclcpp::shutdown();
  return 0;
}

然后还要编辑配置文件

package.xml现在不用修改

有4个入口参数，但是后两个有默认值，所以我们只用管前2个。

第一个入口参数就是一个话题名称，字符串

第二个入口参数是回调函数

返回值是一个service类型的智能指针

#include "rclcpp/rclcpp.hpp"
#include "base_interfaces_demo/srv/add_ints.hpp"

using base_interfaces_demo::srv::AddInts;

class AddIntsServer: public rclcpp::Node
{
  public:
    AddIntsServer():Node("add_ints_server_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"服务端节点创建！");
      server_ = this->create_service<base_interfaces_demo::srv::AddInts>("add_ints",std::bind(&AddIntsServer::add_callback,this,std::placeholders::_1,std::placeholders::2));
    }
  private:
    void add_callback(const AddInts::Request::SharedPtr req,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<base_interfaces_demo::srv::AddInts>::SharedPtr server_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  rclcpp::spin(std::make_shared<AddIntsServer>());

  rclcpp::shutdown();
  return 0;
}

因为我们的客户端还没写，所以先暂时用ros2 service call这个小工具来查看服务端的情况

ros2 service call /add_ints base_interfaces_demo/srv/AddInts "{'num1': 10,'num2': 30}"

ros2 service call + 话题名 + 接口数据类型 + json代码(也可以理解成yaml格式的)

此json代码(yaml格式)格式: “{‘第一个数的名’: 空格 +对应数值,’第二个数的名’: 空格 +对应数的数值}”

服务通信_实验1_客户端实现(C++)

运行的时候后面跟了两个整形数据，

所以这个argc应该是等于3的。

argv[]:接收编译时的返回的argc的参数

argc是命令行总的参数个数

argv[]是argc个参数，其中第0个参数是程序的全名，以后的参数

必须得保证服务器开着，并且客户端能够连接服务器，如果服务器没开，那么发送的数据会丢失，但是一般使用服务通信的都是比较重要的信息，一定不要丢失了。

客户端发送完数据后，会产生一个响应，这里直接当函数的返回值给返回了。

#include "rclcpp/rclcpp.hpp"

class AddIntsClient: public rclcpp::Node
{
  public:
    AddIntsClient():Node("add_ints_client_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"客户端节点创建！");
    }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

//   rclcpp::spin(std::make_shared<AddIntsClient>());
  auto client = std::make_shared<AddIntsClient>();

  rclcpp::shutdown();
  return 0;
}

这一段就应该放在节点初始化前面，防止多作一些耗资源的操作再进行判断。

因为RCLCPP_INFO在节点创建的前面，无法使用类和实例化方式进行get_logger，也就是无法使用this指针和节点智能指针来获取。

所以我们采用以下方式：

这种方式通过rclcpp里的get_logger，但是需要给日志起个名字，放到入口参数里，我们就叫rclcpp吧。

#include "rclcpp/rclcpp.hpp"

class AddIntsClient: public rclcpp::Node
{
  public:
    AddIntsClient():Node("add_ints_client_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"客户端节点创建！");
    }
};

int main(int argc, char ** argv)
{
  if(argc != 3)
  {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"请提交两个整形数字!");
    return 1;
  }
  rclcpp::init(argc,argv);

  auto client = std::make_shared<AddIntsClient>();

  rclcpp::shutdown();
  return 0;
}

如果我不提交参数，直接回车，然后这是一个异常，主函数返回值不是0

也可以把INFO改成ERROR

#include "rclcpp/rclcpp.hpp"

class AddIntsClient: public rclcpp::Node
{
  public:
    AddIntsClient():Node("add_ints_client_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"客户端节点创建！");
    }
};

int main(int argc, char ** argv)
{
  if(argc != 3)
  {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"请提交两个整形数字!");
    return 1;
  }
  rclcpp::init(argc,argv);

  auto client = std::make_shared<AddIntsClient>();

  rclcpp::shutdown();
  return 0;
}

一共3个入口参数，

第一个入口参数是话题名称，是字符串;

第二个入口参数和第三个入口参数有默认值，先不用管;

返回值是客户端的智能指针。

一旦Ctrl+C关闭，则会疯狂爆INFO，且程序无法停止。

再按Ctrl+Z可以停止程序进行。

解决上面Ctrl+C的bug：

rclcpp::ok()这个是判断当前程序是否正常运行，如果正常运行，则返回true，如果不正常运行则返回false，比如按下Ctrl+C就是不正常运行。

当rclcpp::ok() != true的时候，就是ctrl+c按下了。

这样直接可以让函数结束。

这时按下ctrl+c会爆很多错误

这是因为

this->get_logger()

client->get_logger()

rclcpp::get_logger()

的不同

这个异常和context有关，初始化的时候会创建context对象，相当于是一个容器，可以往容器里放数据，也可以在容器里取数据。

当前，如果我们连接失败的话，打印日志。

按下ctrl+c会结束我们的ROS2程序，要释放资源，比如要关闭context，这时已经关掉了context，这样，我们再从client和this来获取日志，就不行了，所以建议用rclcpp::get_logger()。

因为rclcpp::get_logger()的调用和context没有关系。

这样程序就正常了！

#include "rclcpp/rclcpp.hpp"
#include "base_interfaces_demo/srv/add_ints.hpp"

using base_interfaces_demo::srv::AddInts;
using namespace std::chrono_literals;

class AddIntsClient: public rclcpp::Node
{
  public:
    AddIntsClient():Node("add_ints_client_node_cpp")
    {
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"客户端节点创建！");
      client_ = this->create_client<AddInts>("add_ints");
    }

    bool connect_server()
    {
      while(client_->wait_for_service(1s) != true)
      {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"服务连接中!");

        if (rclcpp::ok() != true)
        {
          RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"强行终止客户端!");
          return false;
        }
      }
      return true;
    }

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

int main(int argc, char ** argv)
{
  if(argc != 3)
  {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"请提交两个整形数字!");
    return 1;
  }
  rclcpp::init(argc,argv);

  auto client = std::make_shared<AddIntsClient>();

  bool flag = client->connect_server();

  if (flag != true)
  {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"服务器连接失败，程序退出!");
    return 0;
  }
  

  rclcpp::shutdown();
  return 0;
}

返回值类型有了，我们就粘贴过去，

因为using base_interfaces_demo::srv::AddInts所以可以省略成AddInts

在主函数里要调用函数。

atoi()是把数据转化成整形

我们还要处理响应，响应有3个

第一个是中断，第二个是成功，第三个是超时;

我们一般只判断成功，其他两种情况都认为是失败。

第一个入口参数是节点的智能指针

第二个入口参数是future

第三个入口参数有默认值，先不用管

#include "rclcpp/rclcpp.hpp"
#include "base_interfaces_demo/srv/add_ints.hpp"

using base_interfaces_demo::srv::AddInts;
using namespace std::chrono_literals;

class AddIntsClient: public rclcpp::Node
{
  public:
    AddIntsClient():Node("add_ints_client_node_cpp")
    {
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"客户端节点创建！");
      client_ = this->create_client<AddInts>("add_ints");
    }

    bool connect_server()
    {
      while(client_->wait_for_service(1s) != true)
      {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"服务连接中!");

        if (rclcpp::ok() != true)
        {
          RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"强行终止客户端!");
          return false;
        }
      }
      return true;
    }

    rclcpp::Client<AddInts>::FutureAndRequestId send_request(int32_t num1,int32_t num2)
    {
      /*
        返回值 rclcpp::Client<base_interfaces_demo::srv::AddInts>::FutureAndRequestId
        入口参数 async_send_request(std::shared_ptr<base_interfaces_demo::srv::AddInts_Request> request)  //其实就相当于AddInts::Request类型
      */
      auto request = std::make_sharedautolinkAddInts::Requestautolink();
      request->num1 = num1;
      request->num2 = num2;
      return client_->async_send_request(request);
    }

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

int main(int argc, char ** argv)
{
  if(argc != 3)
  {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"请提交两个整形数字!");
    return 1;
  }
  rclcpp::init(argc,argv);

  auto client = std::make_shared<AddIntsClient>();

  bool flag = client->connect_server();

  if (flag != true)
  {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"服务器连接失败，程序退出!");
    return 0;
  }
  auto future = client->send_request(atoi(argv[1]),atoi(argv[2]));

  if (rclcpp::spin_until_future_complete(client,future) == rclcpp::FutureReturnCode::SUCCESS)
  {
    RCLCPP_INFO(client->get_logger(),"响应成功! sum = %d",future.get()->sum);
  }
  else
  {
    RCLCPP_INFO(client->get_logger(),"响应失败!");
  }

  rclcpp::shutdown();
  return 0;
}

动作通信_理论

让B一直给A返回当前机器人的状态信息，这样的行为通信更符合我们操控机器人的导航需求。

输入10，会累加1-10的所有数，并且会遍历1-10所有的数，并进行累加，累加是需要耗时的，假设每累加一次，耗时一秒，

然后为了好看出来程序运行情况，在每累加的时候，都发一个INFO，代表当前进度。

可以在进行任务时，把任务取消掉。

第一步，客户端给服务端发目标数据

第二步，服务端评估目标数据，并反馈给客户端这个评估结果(是否能够达到目标)

第三步，客户端再给服务端发最终确定的目标数据

第四步，服务端一直反馈给客户端执行的过程数据

第五步，结束之后，服务端反馈给客户端最终的结果

ros2 pkg create cpp03_action –build-type ament_cmake –dependencies rclcpp rclcpp_action base_interfaces_demo –node-name demo01_action_server

最顶上是请求数据，

中间是最终响应结果的数据，

最底下是连续反馈的数据。

depend是build depend,exe depend,export depend三者的集成。

ros2 interface show base_interfaces_demo/action/Progress

动作通信_实验1_服务端实现(C++)

#include "rclcpp/rclcpp.hpp"

class ProgressActionServer: public rclcpp::Node
{
  public:
    ProgressActionServer():Node("progress_action_server_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"action服务端创建!");
    }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  rclcpp::spin(std::make_shared<ProgressActionServer>());

  rclcpp::shutdown();
  return 0;
}

#include "rclcpp/rclcpp.hpp"

class ProgressActionClient: public rclcpp::Node
{
  public:
    ProgressActionClient():Node("progress_action_server_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"action客户端创建!");
    }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  rclcpp::spin(std::make_shared<ProgressActionClient>());

  rclcpp::shutdown();
  return 0;
}

有俩模板，我们只需要设置action就行了，就是我们的动作接口类型。

第一个参数是node，在class里就用this指针，

第二个参数是话题，字符串，

第三个参数是回调函数用来处理目标值的，

第四个参数是回调函数用来处理取消请求的，

第五个参数是接收目标值之后，该回调函数生成连续反馈，

第六、第七个参数有默认值，先不管，

返回值是action智能指针。

Goal_callback解析：

第一个参数是GoalUUID，

第二个参数是我们动作接口下的Goal，

返回值是goalresponse，用的命名空间是rclcpp_action，底下封装了3个常量，

第一个是接收并马上执行，

第二个是接收并推迟执行，

第三个是拒绝。

报错原因是没加占位符

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces_demo/action/progress.hpp"

using base_interfaces_demo::action::Progress;
using std::placeholders::_1;
using std::placeholders::_2;


class ProgressActionServer: public rclcpp::Node
{
  public:
    ProgressActionServer():Node("progress_action_server_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"action服务端创建!");
      /*
      rclcpp_action::Server<ActionT>::SharedPtr create_server<ActionT,
      NodeT>(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,
      const rcl_action_server_options_t &options = rcl_action_server_get_default_options(),
      rclcpp::CallbackGroup::SharedPtr group = nullptr)
      */
      server_ = rclcpp_action::create_server<Progress>(
        this,
        "get_sum_topic",
        std::bind(&ProgressActionServer::handle_goal_callback,this,_1,_2),
        std::bind(&ProgressActionServer::handle_cancel_callback,this,_1),
        std::bind(&ProgressActionServer::handle_accepted_callback,this,_1)
        );
    }
    //std::function<GoalResponse(const GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)>
    rclcpp_action::GoalResponse handle_goal_callback(const rclcpp_action::GoalUUID &, std::shared_ptr<const Progress::Goal>)
    {

      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    //std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>
    rclcpp_action::CancelResponse handle_cancel_callback(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
    {

      return rclcpp_action::CancelResponse::ACCEPT;
    }

    //std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>
    void handle_accepted_callback(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
    {
    }

  private:
    rclcpp_action::Server<Progress>::SharedPtr server_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  rclcpp::spin(std::make_shared<ProgressActionServer>());

  rclcpp::shutdown();
  return 0;
}

ros2 action send_goal /get_sum_topic base_interfaces_demo/action/Progress -f "{'num': 10}"

ros2 action send_goal /话题名称 + 接口类型 + -f + 参数

-f是连续反馈，就是可以获取连续反馈。

发送目标值为10

然后为我们客户端设置了一个ID，因为可能有多个客户端都访问这个服务端，所以我们要给每个客户端都设置一个唯一的ID

然后结果是0

因为我们程序暂时啥都还没写。

uuid就是客户端ID，在此时没有使用，那就用(void)uuid，因为如果不用，编译器可能报警告。

我们的这个任务是可以正常被取消的，所以直接return accept就可以，根据不同任务需求来在函数里做不同的事。

因为我们的连续反馈和最终响应的生成是耗时操作，为了避免主逻辑出现阻塞，建议单独再开一个线程。

生成连续反馈的API，需要传参，传入的参数就是Progress里的Feedback对象。

get_goal()可以获取目标值

因为这是个耗时操作，为了看出效果来，所以咱们每次循环的时候都给设置一下休眠。

1.0是指1Hz，也就是每隔1秒执行一次。

因为1Hz，所以我们这个rate.sleep()每次都会休眠1秒钟;

生成最终结果的API，需要传参，传入的参数就是Progress里的Result对象。

ok()函数的返回值：如果正常运行，则返回true，如果不正常运行则返回false

bug：当我们终止客户端之后，服务端没有停止运行。服务端要一直执行到程序结束。

bug解决思路：

接收到取消请求后，就是中断我的主逻辑，也就是execute_callback被关闭，

这个函数返回值是布尔值，如果接收到了取消请求就返回true，否则返回false，

我们取消之后，其实仍然可以向客户端反应最终的结果，用canceled函数，

要把定义result放在前面。

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces_demo/action/progress.hpp"

using base_interfaces_demo::action::Progress;
using std::placeholders::_1;
using std::placeholders::_2;


class ProgressActionServer: public rclcpp::Node
{
  public:
    ProgressActionServer():Node("progress_action_server_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"action服务端创建!");
      /*
      rclcpp_action::Server<ActionT>::SharedPtr create_server<ActionT,
      NodeT>(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,
      const rcl_action_server_options_t &options = rcl_action_server_get_default_options(),
      rclcpp::CallbackGroup::SharedPtr group = nullptr)
      */
      server_ = rclcpp_action::create_server<Progress>(
        this,
        "get_sum_topic",
        std::bind(&ProgressActionServer::handle_goal_callback,this,_1,_2),
        std::bind(&ProgressActionServer::handle_cancel_callback,this,_1),
        std::bind(&ProgressActionServer::handle_accepted_callback,this,_1)
        );
    }

    //std::function<GoalResponse(const GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)>
    rclcpp_action::GoalResponse handle_goal_callback(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const Progress::Goal> goal)
    {
      (void)uuid;
      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;
    }

    //std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>
    rclcpp_action::CancelResponse handle_cancel_callback(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
    {
      (void)goal_handle;
      RCLCPP_INFO(this->get_logger(),"接收到任务取消请求!");
      return rclcpp_action::CancelResponse::ACCEPT;
    }


    //std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>
    void execute_callback(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
    {
      //void publish_feedback(std::shared_ptr<base_interfaces_demo::action::Progress_Feedback> feedback_msg)
      //goal_handle->publish_feedback();
      int num = goal_handle->get_goal()->num;
      int sum = 0;
      auto feedback = std::make_sharedautolinkProgress::Feedbackautolink();
      auto result = std::make_sharedautolinkProgress::Resultautolink();
      rclcpp::Rate rate(1.0);
      for (int32_t i = 1; i <= num; i++)
      {
        sum += i;
        double progress = i / (double)num;
        feedback->progress = progress;

        goal_handle->publish_feedback(feedback);
        RCLCPP_INFO(this->get_logger(),"连续反馈中，当前进度为:%.2f",progress);

        if(goal_handle->is_canceling() == true)
        {
          result->sum = sum;
          goal_handle->canceled(result);
          RCLCPP_INFO(this->get_logger(),"任务被取消了!");
          return;
        }

        rate.sleep();
      }
      

      //void succeed(std::shared_ptr<base_interfaces_demo::action::Progress_Result> result_msg)
      //goal_handle->succeed();

      if(rclcpp::ok() == true)
      {
        result->sum = sum;
        goal_handle->succeed(result);
        RCLCPP_INFO(this->get_logger(),"最终响应结果为:%d",sum);
      }

    }

    void handle_accepted_callback(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
    {
      std::thread(std::bind(&ProgressActionServer::execute_callback,this,goal_handle)).detach();
    }

  private:
    rclcpp_action::Server<Progress>::SharedPtr server_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  rclcpp::spin(std::make_shared<ProgressActionServer>());

  rclcpp::shutdown();
  return 0;
}

动作通信_实验1_客户端实现(C++)

这条红色的线是在action_client帮我们封装好的，所以可以不用管。

#include "rclcpp/rclcpp.hpp"

class ProgressActionClient: public rclcpp::Node
{
  public:
    ProgressActionClient():Node("progress_action_server_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"action客户端创建!");
    }
};

int main(int argc, char ** argv)
{
  if(argc != 2)
  {
    RCLCPP_INFO(rclcpp::get_logger("rclcppp"),"请输入一个整形数字!");
    return 1;
  }
  rclcpp::init(argc,argv);

  rclcpp::spin(std::make_shared<ProgressActionClient>());

  rclcpp::shutdown();
  return 0;
}

第一个入口参数是node

第二个入口参数是话题名称，字符串

第三个入口参数和第四个入口参数都有默认值

返回值是客户端智能指针。

需要把一步分成两步。

再把send_goal函数调用一下。

async_send_goal是异步发送目标值

第一个入口参数是我们接口文件里的目标值

第二个入口参数是发送目标选项对象，我们可以设置这个目标发送过去之后，我们需要处理的一些回调函数

返回值是

先不管其他的，先把函数随便定义上，什么返回值，入口参数都是void，先不让程序报错。

得知，

GoalResponseCallback返回值是void，入口参数是goalhandle，goalhandle在本图的上面，是clientgoalhandle，然后这个clientgoalhandle属于rclcpp_action工作空间

返回值是void

第一个入口参数是clientgoalhandle

第二个入口参数是feedback

发送一个数值给服务端，服务端要先拿到目标值进行判断，判断该目标值是否可以被接收，或者被拒绝，再把处理结果响应给客户端。

如果说这个目标值可处理，那么goal_handle里是有内容的;

如果不可以被处理，那么goal_handle是一个nullptr空指针。

接下来处理反馈数据：

如果我们只是解析反馈的数据，那么goal_handle是用不上的。

需要用俩%来转译%，如上图是打印百分比数据的案例。

假设progress_int是50，则会输出50%。

如果只想打印一个%，那就需要%%来转译。

回调函数是可能会数据丢失的。这是正常现象。

最终响应：

这个result最终结果的状态是不一定的，有可能任务被取消了，或被终止了，也有可能任务正常运行了。

所以我们要通过状态码来判断状态。

第一个是被强行终止

第二个是取消

第三个是成功

第四个是未知

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces_demo/action/progress.hpp"

using base_interfaces_demo::action::Progress;
using namespace std::chrono_literals;
using std::placeholders::_1;
using std::placeholders::_2;

class ProgressActionClient: public rclcpp::Node
{
  public:
    ProgressActionClient():Node("progress_action_server_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"action客户端创建!");
      /*
      rclcpp_action::Client<ActionT>::SharedPtr create_client<ActionT,
      NodeT>(NodeT node, const std::string &name,
      rclcpp::CallbackGroup::SharedPtr group = nullptr,
      const rcl_action_client_options_t &options = rcl_action_client_get_default_options())
      */
      client_ = rclcpp_action::create_client<Progress>(this,"get_sum_topic");
    }

    void send_goal(int32_t num)
    {
      if(client_->wait_for_action_server(1s) != true)
      {
        RCLCPP_ERROR(this->get_logger(),"服务连接失败!");
        return;
      }

      /*
      std::shared_future<rclcpp_action::ClientGoalHandle<base_interfaces_demo::action::Progress>::SharedPtr>
      async_send_goal(const base_interfaces_demo::action::Progress::Goal &goal,
      const rclcpp_action::Client<base_interfaces_demo::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 goal_response_callback(rclcpp_action::ClientGoalHandle<Progress>::SharedPtr goal_handle)
  {
    if(goal_handle == nullptr)
    {
      RCLCPP_INFO(this->get_logger(),"目标请求被服务端拒绝!");
    }
    else
    {
      RCLCPP_INFO(this->get_logger(),"目标处理中!");
    }
  }

  void feedback_callback(rclcpp_action::ClientGoalHandle<Progress>::SharedPtr goal_handle,std::shared_ptr<const Progress::Feedback> feedback)
  {
    (void)goal_handle;
    double progress = feedback->progress;
    int progress_int = (int) (progress * 100);
    RCLCPP_INFO(this->get_logger(),"当前进度为:%d%%",progress_int);
  }

  void result_callback(const rclcpp_action::ClientGoalHandle<Progress>::WrappedResult & result)
  {
    if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
    {
      RCLCPP_INFO(this->get_logger(),"最终结果为:%d",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(),"未知异常!");
    }
  }

  private:
    rclcpp_action::Client<Progress>::SharedPtr client_;
};

int main(int argc, char ** argv)
{
  if(argc != 2)
  {
    RCLCPP_ERROR(rclcpp::get_logger("rclcppp"),"请输入一个整形数字!");
    return 1;
  }
  rclcpp::init(argc,argv);

  auto node = std::make_shared<ProgressActionClient>();

  node->send_goal(atoi(argv[1]));

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

此时我们取消客户端，反而服务端还在运行，这里的ctrl+c只是结束了我们的客户端，而不是指挥我们的客户端去下发取消请求指令，我们只有去捕获一下我们的键盘才能完成取消请求指令的发送。

修复bug:

还没修复好

// 发送取消请求auto future_cancel = client_->async_cancel_goal(goal_handle);
rclcpp::spin_until_future_complete(this->get_node_base_interface(), future_cancel);
if (future_cancel.wait_for(1s) == std::future_status::ready)
{
  RCLCPP_INFO(this->get_logger(), "终止请求已发送!");
}
else
{
  RCLCPP_ERROR(this->get_logger(), "无法发送终止请求...");
}

参数服务_理论与API介绍(C++)

当然还有特殊情况，比如把参数设置为私有的。

其他通信需要自己弄接口文件，但是参数服务不用自己弄接口文件，ROS2已经封装好了API，所以我们只需要调用API即可。

只是想展示一下API，所以先创建参数功能包展示下API，先不创建客户端和服务端。

rclcpp::parameter 对象(键,值);

该函数是给parameter的值赋值的，有18个重载，各种类型。

其中空是说不给赋值，这样只有键，没有值。

参数服务_实验1_服务端(C++)

这里有差异，在Node里传入了第二个参数，这句是专门用来允许删除参数的。undeclared解除声明。

只有查询和修改的接口API，并没有新增和删除的API，这是ROS2根据安全考虑的。

#include "rclcpp/rclcpp.hpp"

class ParamServer: public rclcpp::Node
{
  public:
    ParamServer():Node("param_server_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"参数服务端搭建!");
    }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  auto node = std::make_shared<ParamServer>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}
#include "rclcpp/rclcpp.hpp"

class ParamClient: public rclcpp::Node
{
  public:
    ParamClient():Node("param_client_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"参数客户端搭建!");
    }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  auto node = std::make_shared<ParamClient>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

链式编程。

一个普通的节点就可以当参数服务端，不需要另行创建参数服务端。

#include "rclcpp/rclcpp.hpp"

class ParamServer: public rclcpp::Node
{
  public:
    ParamServer():Node("param_server_node_cpp",rclcpp::NodeOptions().allow_undeclared_parameters(true))
    {
      RCLCPP_INFO(this->get_logger(),"参数服务端搭建!");
    }

    void create_param()
    {
      RCLCPP_INFO(this->get_logger(),"-------------增操作--------------");
    }

    void get_param()
    {
      RCLCPP_INFO(this->get_logger(),"-------------查操作--------------");
    }

    void update_param()
    {
      RCLCPP_INFO(this->get_logger(),"-------------改操作--------------");
    }

    void delete_param()
    {
      RCLCPP_INFO(this->get_logger(),"-------------删操作--------------");
    }


};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  auto node = std::make_shared<ParamServer>();

  node->create_param();
  node->get_param();
  node->update_param();
  node->delete_param();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

ros2 param list

查询所有节点里的所有参数

ros2 param get /节点名称参数键名来查看参数的值

可以通过键来查询参数的值

带复数形式的函数可以通过由键组成的容器来获取一些参数对象。

来判断是否有该参数的，入口参数也是键，返回值是布尔值。

需要传入parameter对象。

我们覆盖掉旧值即可。

通过set_parameter也可以创建参数,但是必须声明allow_undeclared_parameters(true)。

这种声明的参数不可以被删除，只能删除未声明但设置的。

#include "rclcpp/rclcpp.hpp"

class ParamServer: public rclcpp::Node
{
  public:
    ParamServer():Node("param_server_node_cpp",rclcpp::NodeOptions().allow_undeclared_parameters(true))
    {
      RCLCPP_INFO(this->get_logger(),"参数服务端搭建!");
    }

    void create_param()
    {
      RCLCPP_INFO(this->get_logger(),"-------------增操作--------------");

      this->declare_parameter("car_name","ER");
      this->declare_parameter("width",1.55);
      this->declare_parameter("wheels",5);

      this->set_parameter(rclcpp::Parameter("height",2.00));
    }

    void get_param()
    {
      RCLCPP_INFO(this->get_logger(),"-------------查操作--------------");

      auto car = this->get_parameter("car_name");
      RCLCPP_INFO(this->get_logger(),"key = %s,value = %s",car.get_name().c_str(),car.as_string().c_str()); 

      auto params = this->get_parameters({"car_name","width","wheels"});
      for(auto &¶m : params)
      {
        RCLCPP_INFO(this->get_logger(),"key = %s,value = %s",param.get_name().c_str(),param.value_to_string().c_str());
      }
      bool car_name_flag = this->has_parameter("car_name");
      bool height_flag = this->has_parameter("height");
      RCLCPP_INFO(this->get_logger(),"是否包含car_name? 答案:%d",car_name_flag);
      RCLCPP_INFO(this->get_logger(),"是否包含height? 答案:%d",height_flag);
    }

    void update_param()
    {
      RCLCPP_INFO(this->get_logger(),"-------------改操作--------------");
      this->set_parameter(rclcpp::Parameter("width",1.85));
      RCLCPP_INFO(this->get_logger(),"width = %.2f",this->get_parameter("width").as_double());
    }

    void delete_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("height");
      RCLCPP_INFO(this->get_logger(),"是否包含height? 答案:%d",this->has_parameter("height"));
    }


};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  auto node = std::make_shared<ParamServer>();

  node->create_param();
  node->get_param();
  node->update_param();
  node->delete_param();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

参数服务_实验1_客户端(C++)

第一个入口参数是客户端节点对象，

第二个入口参数是需要连接的服务端的节点名称。

如果1秒钟之内连接上了就返回true，如果超时1s没连接上就返回false。

#include "rclcpp/rclcpp.hpp"

using namespace std::chrono_literals;

class ParamClient: public rclcpp::Node
{
  public:
    ParamClient():Node("param_client_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"参数客户端搭建!");
      param_client_ = std::make_sharedautolinkrclcpp::SyncParametersClientautolink(this,"param_server_node_cpp");
    }

    bool connect_server()
    {
      while(param_client_->wait_for_service(1s) != true)
      {
        if(rclcpp::ok() != true)
        {
          return false;
        }
        RCLCPP_INFO(this->get_logger(),"服务连接中!");
      }
      return true;
    }

    void get_param()
    {
      RCLCPP_INFO(this->get_logger(),"-----------参数查询操作-------------");
    }

    void update_param()
    {
      RCLCPP_INFO(this->get_logger(),"-----------参数更新操作-------------");
    }

  private:
    rclcpp::SyncParametersClient::SharedPtr param_client_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  auto client = std::make_shared<ParamClient>();

  bool flag = client->connect_server();

  if(!flag)
  {
    return 0;
  }

  client->get_param();
  client->update_param();
  client->get_param();

  // rclcpp::spin(client);

  rclcpp::shutdown();
  return 0;
}

这些话题都是我们此节点名称下的。

要用高级for

入口参数填参数对象的容器。

我们不仅可以修改值，也可以创建新的参数，但是要保证服务端那边调用过undeclared……

ROS2其他通信机制
概述

不同设备之间的通信，是通过分布式来实现的。

镜像王八

分布式搭建

场景

在许多机器人相关的应用场景中都涉及到多台ROS2设备协作，比如：无人车编队、无人机编队、远程控制等等，那么不同的ROS2设备之间是如何实现通信的呢？

概念

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

ROS2本身是一个分布式通信框架，可以很方便的实现不同设备之间的通信，ROS2所基于的中间件是DDS，当处于同一网络中时，通过DDS的域ID机制(ROS_DOMAIN_ID)可以实现分布式通信，大致流程是：在启动节点之前，可以设置域ID的值，不同节点如果域ID相同，那么可以自由发现并通信，反之，如果域ID值不同，则不能实现。默认情况下，所有节点启动时所使用的域ID为0，换言之，只要保证在同一网络，你不需要做任何配置，不同ROS2设备上的不同节点即可实现分布式通信。

作用

分布式通信的应用场景是较为广泛的，如上所述：机器人编队时，机器人可能需要获取周边机器人的速度、位置、运行轨迹的相关信息，远程控制时，则可能需要控制端获取机器人采集的环境信息并下发控制指令…… 这些数据的交互都依赖于分布式通信。

实现

多机通信时，可以通过域ID对节点进行分组，组内的节点之间可以自由通信，不同组之间的节点则不可通信。如果所有节点都属于同一组，那么直接使用默认域ID即可，如果要将不同节点划分为多个组，那么可以在终端中启动节点前设置该节点的域ID(比如设置为6)，具体执行命令为：

export ROS_DOMAIN_ID=6

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

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

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

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

默认域ID是0，域ID不一样，无法互相通信。

ID不一样就无法正常通信，和在ROS1内需要指定ROS Master一样。

注意

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

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

DDS 域 ID 值的计算规则

域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个。

上述计算规则了解即可。

域 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

Domain ID是指域ID

Participant ID是参与组ID，是指该域内的第几个节点

Discovery Multicast Port主发现端口，DDS规定，端口应从7400开始

User Multicast Port用户广播端口，

Discovery Unicast Port单播发现端口，7410是第一个节点开始使用的Discovery Unicast端口，因为DDS规定的，第11个端口才是第一个节点的Discovery Unicast端口。

User Unicast Port单播用户端口，7411才是第一个节点开始使用的User Unicast端口，因为DDS规定的，第12个端口才是第一个节点的User Unicast端口。

Discovery Unicast Port和User Unicast Port是第一个节点所占用的端口，所以一共占用了俩端口。

如果Domain ID不变还为0，Participant ID变成1的话，那么下一个节点的Discovery Unicast Port为7412，User Unicast Port为7413。

以此类推。

一个Domain ID占用250个端口，所以当Domain ID为1的时候，Discovery Unicast Port应该是7650。

这是第三个节点，所以是7664和7665。

实践：在树莓派5上跑一个ROS2 Jazzy开启键盘控制节点，然后在电脑实体机Linux(或者在Docker里跑也行，但要设置好网络)里跑一个ROS2 Humble，并打开乌龟显示节点。

会发现是可以正常通信的。

工作空间覆盖

没什么用，建议不要使用

这个是和你的bashrc文件里，加载bash文件的顺序有关，谁最后加载，谁就会运行，也就是最高优先级。除了ROS2本身自带的bash，这个bash是不论在哪加载，都是最低优先级。

元功能包

distro就是发行版的意思

--build-type默认C++，

--dependent默认无，

--node-name本身是虚包，所以也无需设置。

CmakeLists无需修改

先把12，13行删除

<exec_depend>xxxxxx</exec_depend>

所要依赖的功能包名

以后可能需要的元功能包：

这个功能包是导航相关的

这个就是个元功能包

只有配置文件，没有其他实质性实现

元功能包的作用就是方便安装，把自己的东西打包，可以共享到ROS2社区。也方便安装别人的东西。

节点重名

而且节点名称都是一致的，图中有个<2>，这是操作系统给的标号，其他的操作系统是没有这个标号的。

rqt_graph

他只显示一个turtlesim，实际上我们是使用了两个turtlesim的。

这样虽然都显示了，但是是一模一样的名字，容易混淆，而且上面也给重名警告了。

要么起别名：王大宝，王小宝

要么加命名空间：毛驴子家的王宝，李二狗家的王宝

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

ros2 run 功能包名节点名 –ros-args –remap __ns:=/命名空间

ros2 run turtlesim turtlesim_node --ros-args --remap __name:=turtlesim2

ros2 run 功能包名节点名 –ros-args –remap __name:=别名

ros2 run 功能包名节点名 –ros-args –remap __node:=别名

ros2 pkg create cpp05_names --build-type ament_cmake --node-name demo01_names

节点名可以不设置，这里设置主要是为以后学习做铺垫。

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

先导俩包

这个LaunchDescription对象里面呢是个列表，这个列表就是存储要启动的若干个节点。

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(package="turtlesim",executable="turtlesim_node",name="t1")
    ])

package功能包名

executable节点名

name别名

ros2 launch cpp05_names demo01_names_launch.py

根标签是Launch

子集标签是node

<launch>
    <node pkg="turtlesim" exec="turtlesim_node" name="t1" />
</launch>

ros2 launch cpp05_names demo02_names_launch.xml 

yaml的根标签也是launch

launch:
node:
  pkg: "turtlesim"
  exec: "turtlesim_node"
  name: "t1"

ros2 launch cpp05_names demo03_names_launch.yaml 

name别名

namespace命名空间

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")
    ])

<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>

launch:
node:
  pkg: "turtlesim"
  exec: "turtlesim_node"
  name: "turtle"
node:
  pkg: "turtlesim"
  exec: "turtlesim_node"
  namespace: "t1"
node:
  pkg: "turtlesim"
  exec: "turtlesim_node"
  namespace: "t1"
  name: "turtle"

可指定命名空间

#include "rclcpp/rclcpp.hpp"

class MyNode: public rclcpp::Node
{
  public:
    MyNode():Node("mynode_node_cpp","t1_ns")
    {
      RCLCPP_INFO(this->get_logger(),"Hello World!");
    }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  auto node = std::make_shared<MyNode>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

话题重名

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

ros2 run 功能包名节点名 –ros-args –remap __ns:=/命名空间

这种加命名空间的方式，不仅对节点重名生效，当然对话题名称依然生效。

ros2 run teleop_twist_keyboard teleop_twist_keyboard 

这个是打开控制机器人运动的节点

ros2 run teleop_twist_keyboard teleop_twist_keyboard

其所对应的话题名称是这个。

但此时控制乌龟运动无效，是因为话题命名空间不同。

乌龟接收的话题是/turtle1/cmd_vel，命名空间是/turtle1

而控制乌龟运动的是/cmd_vel，命名空间不同，

所以我们要把两者命名空间弄成一样的。

随便改即可，只要改成一样的，就可以正常通信了。

remappings可以实现话题的重映射，该参数是一个列表，然后里面是元组，每一个元组都可以对一个话题进行重映射，元组里第一个参数是原话题名称，第二个参数是重映射后的话题名称。

namespace可以实现命名空间。

remp也是可以设置重映射，from是原话题名称，to是重映射的名称。

<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" /> -->

    <node pkg ="turtlesim" exec ="turtlesim_node" namespace ="t1" />
    <node pkg ="turtlesim" exec ="turtlesim_node" >
        <remap from= "/turtle1/cmd_vel" to="/cmd_vel" />
    </node>
</launch>

launch:

- node:
pkg: "turtlesim"
exec: "turtlesim_node"
name: "turtle"
- node:
pkg: "turtlesim"
exec: "turtlesim_node"
namespace: "t1"
- node:
pkg: "turtlesim"
exec: "turtlesim_node"
namespace: "t1"
name: "turtle"


node:
  pkg: "turtlesim"
  exec: "turtlesim_node"
  namespace: "t1"
node:
  pkg: "turtlesim"
  exec: "turtlesim_node"
  remap:
  -
      from: "/turtle1/cmd_vel"
      to: "/cmd_vel"

命名空间可以有好几级。

全局话题是和节点命名空间平级，也就是挂载在根下的。

相对话题是挂载在命名空间下的。

私有话题是节点名称的子级。

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

using std_msgs::msg::String;

class MyNode: public rclcpp::Node
{
  public:
    MyNode():Node("mynode_node_name","t1_namespace")
    {
      RCLCPP_INFO(this->get_logger(),"Hello World!");
      publisher_ = this->create_publisher<String>("/global_topics",10);
    }
  private:
    rclcpp::Publisher<String>::SharedPtr publisher_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  auto node = std::make_shared<MyNode>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

全局话题

相对话题

私有话题

时间相关API

发消息可以有消息头，消息头里有时间戳，接收方解析消息头，并把消息时间和当前时间进行比对，看是否延迟过高。

Rate是频率

Time是时刻

Duration是持续时间

这个类的构造函数有两个重载，第一个是周期，第二个是频率。

#include "rclcpp/rclcpp.hpp"

using namespace std::chrono_literals;

class MyNode: public rclcpp::Node
{
  public:
    MyNode():Node("time_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"Hello World!");
      demo_rate();
    }
  private:
    void demo_rate()
    {
      rclcpp::Rate rate1(500ms);
      rclcpp::Rate rate2(1.0);
      // while(rclcpp::ok())
      // {
      //   RCLCPP_INFO(this->get_logger(),"休眠500ms");
      //   rate1.sleep();
      // }
      while(rclcpp::ok())
      {
        RCLCPP_INFO(this->get_logger(),"休眠1000ms");
        rate2.sleep();
      }
    }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  auto node = std::make_shared<MyNode>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

可以传入一个纳秒

也可以传入一个秒和一个纳秒

因为是int64_t类型的，所以我们后面加个L，这是5亿纳秒，也就是0.5秒。

这样time2代表2.5秒。

获取当前时刻有两种方式，

一个是this->get_clock()->now()，

另一个是this->now();

#include "rclcpp/rclcpp.hpp"

using namespace std::chrono_literals;

class MyNode: public rclcpp::Node
{
  public:
    MyNode():Node("time_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"Hello World!");
      // demo_rate();
      demo_time();
    }
  private:
    void demo_rate()
    {
      rclcpp::Rate rate1(500ms);
      rclcpp::Rate rate2(1.0);
      // while(rclcpp::ok())
      // {
      //   RCLCPP_INFO(this->get_logger(),"休眠500ms");
      //   rate1.sleep();
      // }
      while(rclcpp::ok())
      {
        RCLCPP_INFO(this->get_logger(),"休眠1000ms");
        rate2.sleep();
      }
    }

    void demo_time()
    {
      rclcpp::Time time1(500000000L);
      rclcpp::Time time2(2,500000000L);
      rclcpp::Time right_now_1 = this->get_clock()->now();
      rclcpp::Time right_now_2 = this->now();

      RCLCPP_INFO(this->get_logger(),"s = %.2f , ns = %ld",time1.seconds(),time1.nanoseconds());
      RCLCPP_INFO(this->get_logger(),"s = %.2f , ns = %ld",time2.seconds(),time2.nanoseconds());
      RCLCPP_INFO(this->get_logger(),"s = %.2f , ns = %ld",right_now_1.seconds(),right_now_1.nanoseconds());
      RCLCPP_INFO(this->get_logger(),"s = %.2f , ns = %ld",right_now_2.seconds(),right_now_2.nanoseconds());
    }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  auto node = std::make_shared<MyNode>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

和Time类似

但是不完全相同，这个duration用到了chrono。

t2和t1可以进行相减，结果是一个duration类型的，但是不能相加。

time也可以和duration相加相减，结果是一个time。

通信机制工具
命令行

ros2 doctor是来检测系统网络状态、版本兼容性等状态的。

是通过了的，但是有几个警告：版本过低，不影响正常使用。

参数服务端的本质还是服务端，所以也会列在Service Servers里。

如果查看list发现有一些接口文件没显示，那说明你的这个终端的环境变量没刷新。

proto比show更精简一些。

pose是发送位姿的，会一直发数据。

想输出延时，消息必须有消息头。

输出实时位姿

find和type是相反着来的。

消息发布频率是不断变动的，消息频率是可以通过定时器来控制，但是定时器是有误差的，并不是特别特别精准。当然还有网络也是一大影响因素。

ros2 topic pub -r 发布消息的频率话题名称消息具体的指令（要用json格式）

clear是清除乌龟轨迹

kill是杀乌龟

reset是将乌龟位置重置

spawn是产卵，生成新乌龟

可以按Tab补齐

type和find是相反的

empty就是空，所以我们后面内容啥都不用写。

可以加上-f或者-feedback打开连续反馈，这个连续反馈是航向角弧度

删除不是所有参数都能删除，这里提示不能删除静态类型参数。

最大值255，最小取值0

步长1

可以显示在终端上，

也可以写入磁盘

当然也可以修改

也可以用ROS2 RUN来修改，–ros-args -p 后面跟键:=值

ROS2 RUN也可以直接读取磁盘文件

Rqt工具箱

这个是显示两个节点之间关系的。

这个是用来发布消息的。

点击添加按钮

可以设置线速度和角速度，频率等

设置好参数后勾选

call /clear这个是清除轨迹的。

也可以产卵，设置好参数

这个可以直接修改参数

我们不能用rqt代替命令行，虽然rqt更方便，但是因为我们在工作中是远程控制机器人的，我们是通过terminal来远程控制机器人，所以，命令行很重要，这样不能使用rqt。

Launch
概述

rosbag2的作用是来序列化储存数据的，是一个数据库。

ros2 launch 功能包名 launch文件名

ros2 pkg create cpp01_launch –build-type ament_cmake –dependencies rclcpp

ros2 pkg create cpp01_launch --build-type ament_cmake --dependencies rclcpp

from launch import LaunchDescription
from launch_ros.actions import Node

封装终端指令相关类
from launch.actions import ExecuteProcess
from launch.substitutions import FindExecutable
参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
文件包含相关
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
分组相关
from launch_ros.actions import PushRosNamespace
from launch.actions import GroupAction
事件相关
from launch.event_handlers import OnProcessStart,OnProcessExit
from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    return LaunchDescription([])

launch基本使用流程(C++)

配置完这个，不管我launch目录下有多少launch文件，只需要配置这一次就行了。

这样说明我们cmake配置对了

from launch import LaunchDescription
from launch_ros.actions import Node
封装终端指令相关类
from launch.actions import ExecuteProcess
from launch.substitutions import FindExecutable
参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
文件包含相关
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
分组相关
from launch_ros.actions import PushRosNamespace
from launch.actions import GroupAction
事件相关
from launch.event_handlers import OnProcessStart,OnProcessExit
from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    return LaunchDescription([])

<launch>
    <node pkg = "turtlesim" exec = "turtlesim_node" name = "t1" />
    <node pkg = "turtlesim" exec = "turtlesim_node" name = "t2" />
</launch>

launch:
node:
  pkg: "turtlesim"
  exec: "turtlesim_node"
  name: "t1"
node:
  pkg: "turtlesim"
  exec: "turtlesim_node"
  name: "t2"

最好添加一个依赖

Launch_Python_Node

这个是标签exec_name

这俩参数传参的区别在于–ros-args的区别

在launch里写更简单

这样是等价的

{}里是yaml格式的。

有另一种更常用的方法，就是上来读取yaml文件，把数据都存在yaml里，用到的时候直接读取。

把yaml文件放到config里

ros2 param dump haha --output-dir src/cpp01_launch/config/

还需要再配置cmakelists

我们要读就读install目录下的。

直接复制路径

可以进一步优化代码

这个就是来获取某个功能包的share目录路径

from launch import LaunchDescription
from launch_ros.actions import Node

封装终端指令相关类
from launch.actions import ExecuteProcess
from launch.substitutions import FindExecutable
参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
文件包含相关
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
分组相关
from launch_ros.actions import PushRosNamespace
from launch.actions import GroupAction
事件相关
from launch.event_handlers import OnProcessStart,OnProcessExit
from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

import os

def generate_launch_description():
    # turtle1 = Node(
    #     package="turtlesim",
    #     executable="turtlesim_node",
    #     exec_name="my_label",
    #     ros_arguments=["--remap","__ns:=/t2"]
    #     )
    turtle2 = Node(
        package="turtlesim",
        executable="turtlesim_node",
        name="haha",
        parameters=[os.path.join(get_package_share_directory("cpp01_launch"),"config","haha.yaml")]
        )
    return LaunchDescription([turtle2])

建议采用第三种动态获取路径

respawn是自动重启的意思，这样运行后的节点，你用鼠标关闭小乌龟窗口后，也会自动重启节点，再把小乌龟窗口打开一个新的。

按Ctrl+C可以终止。

from launch import LaunchDescription
from launch_ros.actions import Node

封装终端指令相关类
from launch.actions import ExecuteProcess
from launch.substitutions import FindExecutable
参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
文件包含相关
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
分组相关
from launch_ros.actions import PushRosNamespace
from launch.actions import GroupAction
事件相关
from launch.event_handlers import OnProcessStart,OnProcessExit
from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

import os

def generate_launch_description():
    # turtle1 = Node(
    #     package="turtlesim",
    #     executable="turtlesim_node",
    #     exec_name="my_label",
    #     ros_arguments=["--remap","__ns:=/t2"]
    #     )
    turtle2 = Node(
        package="turtlesim",
        executable="turtlesim_node",
        name="haha",
        # parameters=[{"background_r": 255,"background_g": 0,"background_b": 0}],
        # parameters=["/home/tungchiahui/mysource/ros2src/4.ws02_tools/install/cpp01_launch/share/cpp01_launch/config/haha.yaml"],
        parameters=[os.path.join(get_package_share_directory("cpp01_launch"),"config","haha.yaml")],
        respawn=True
        )
    return LaunchDescription([turtle2])

第一个是原话题名称，第二个是新话题名称

Launch_Python_执行命令

这是个列表，列表里面写我们的指令

这样是把日志既输出到终端也输出到日志，如果不写则只会输出到日志。

这样就是把命令当成终端指令来执行

from launch import LaunchDescription
from launch_ros.actions import Node

封装终端指令相关类
from launch.actions import ExecuteProcess
from launch.substitutions import FindExecutable
参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
文件包含相关
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
分组相关
from launch_ros.actions import PushRosNamespace
from launch.actions import GroupAction
事件相关
from launch.event_handlers import OnProcessStart,OnProcessExit
from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    turtle = Node(
        package="turtlesim",
        executable="turtlesim_node"
    )
    cmd = ExecuteProcess(
        cmd = ["ros2 topic echo /turtle1/pose"],
        output = "both",
        shell = True
    )

    return LaunchDescription([turtle,cmd])

如果指令过长，可以分到多个字符串里运行。

这样也是可以运行的。

from launch import LaunchDescription
from launch_ros.actions import Node

封装终端指令相关类
from launch.actions import ExecuteProcess
from launch.substitutions import FindExecutable
参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
文件包含相关
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
分组相关
from launch_ros.actions import PushRosNamespace
from launch.actions import GroupAction
事件相关
from launch.event_handlers import OnProcessStart,OnProcessExit
from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    turtle = Node(
        package="turtlesim",
        executable="turtlesim_node"
    )
    cmd = ExecuteProcess(
        cmd = [FindExecutable(name="ros2"),"topic","echo","/turtle1/pose"],
        output = "both",
        shell = True
    )

    return LaunchDescription([turtle,cmd])

Launch_Python_参数设置

没传值的话，乌龟红色是满的，那就是粉红色背景

也可以传值，比如传backg_r:=0

这样背景色就变的更偏蓝绿了。

from launch import LaunchDescription
from launch_ros.actions import Node

封装终端指令相关类
from launch.actions import ExecuteProcess
from launch.substitutions import FindExecutable
参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
文件包含相关
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
分组相关
from launch_ros.actions import PushRosNamespace
from launch.actions import GroupAction
事件相关
from launch.event_handlers import OnProcessStart,OnProcessExit
from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    bg_r = DeclareLaunchArgument(name="backg_r",default_value="255")
    bg_g = DeclareLaunchArgument(name="backg_g",default_value="255")
    bg_b = DeclareLaunchArgument(name="backg_b",default_value="255")
    turtle = Node(
        package="turtlesim",
        executable="turtlesim_node",
        parameters=[{"background_r" : LaunchConfiguration("backg_r"),"background_g" : LaunchConfiguration("backg_g"),"background_b" : LaunchConfiguration("backg_b")}]
    )
    return LaunchDescription([bg_r,bg_g,bg_b,turtle])

Launch_Python_文件包含

假设我要编写一个机器人启动相关的launch文件，在这个launch文件中，我可能要启动雷达，启动IMU，启动底盘等等，我们需要把这些launch文件都包含进机器人启动的launch文件中。

这个值是由被包含的launch文件封装而来的对象。

这个对象对应的类就是PythonLaunchDescriptionSource，

类里面还得设置一个参数。

这个参数是launch_file_path就是文件路径。

建议用这个来获取路径。

也可以为其传参

from launch import LaunchDescription
from launch_ros.actions import Node

封装终端指令相关类
from launch.actions import ExecuteProcess
from launch.substitutions import FindExecutable
参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
文件包含相关
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
分组相关
from launch_ros.actions import PushRosNamespace
from launch.actions import GroupAction
事件相关
from launch.event_handlers import OnProcessStart,OnProcessExit
from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

import os

def generate_launch_description():
    include = IncludeLaunchDescription(
        launch_description_source=PythonLaunchDescriptionSource(
            launch_file_path=os.path.join(
                get_package_share_directory("cpp01_launch"),
                "launch/py",
                "py04_args_launch.py"
            )
        ),
        launch_arguments=[("backg_r","80"),("backg_g","10"),("backg_b","200")]
    )
    return LaunchDescription([include])

Launch_Python_分组设置

from launch import LaunchDescription
from launch_ros.actions import Node

封装终端指令相关类
from launch.actions import ExecuteProcess
from launch.substitutions import FindExecutable
参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
文件包含相关
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
分组相关
from launch_ros.actions import PushRosNamespace
from launch.actions import GroupAction
事件相关
from launch.event_handlers import OnProcessStart,OnProcessExit
from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    turtle1 = Node(
    package="turtlesim",
    executable="turtlesim_node",
    name="t1"
    )
    turtle2 = Node(
    package="turtlesim",
    executable="turtlesim_node",
    name="t2"
    )
    turtle3 = Node(
    package="turtlesim",
    executable="turtlesim_node",
    name="t3"
    )

    group1 = GroupAction(actions=[PushRosNamespace("g1"),turtle1,turtle2])
    group2 = GroupAction(actions=[PushRosNamespace("g2"),turtle3])

    return LaunchDescription([group1,group2])

Launch_Python_事件设置

第一个主要用于注册事件，第二个是开始事件，第三个是节点退出。

这个是在终端中生成新小乌龟的命令

第一个参数是针对哪个事件进行注册。

target_action是事件源，你要为哪个节点注册事件。

on_start是等到事件被触发，你要做哪些操作？

from launch import LaunchDescription
from launch_ros.actions import Node

封装终端指令相关类
from launch.actions import ExecuteProcess
from launch.substitutions import FindExecutable
参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
文件包含相关
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
分组相关
from launch_ros.actions import PushRosNamespace
from launch.actions import GroupAction
事件相关
from launch.event_handlers import OnProcessStart,OnProcessExit
from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    turtle = Node(
    package="turtlesim",
    executable="turtlesim_node",
    )
    spawn = ExecuteProcess(
        cmd=["ros2 service call /spawn turtlesim/srv/Spawn \"{'x': 8.0,'y': 3.0}\""],
        output="both",
        shell=True
    )

    event_start = RegisterEventHandler(
        event_handler=OnProcessStart(
            target_action=turtle,
            on_start=spawn
        )
    )

    event_exit = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=turtle,
            on_exit=[LogInfo(msg="turtlesim_node:退出！")]
        )
    )
    return LaunchDescription([turtle,event_start,event_exit])

on_exit可以创建一个对象，也可以放在列表里。

可以只学Python版本的Launch，XML和Yaml版本的Launch可以了解就行，自己写就写Python版本的，当你要用到别人开源的功能包用的Launch是Xml或者yaml版本一般不影响正常使用。

Launch_XML_YAML_Node
Launch_XML_YAML_执行命令
Launch_XML_YAML_参数设置
Launch_XML_YAML_分组设置
Launch_XML_YAML_文件包含
回溯rosbag2
概述

方式1一边采集数据，一边生成地图信息。

方式2是将采集数据和生成地图信息分割开来了，方式2做到了解耦合，所以更灵活一些。用同一套数据，可能用不同的算法处理，总之，非常灵活。

留存的过程咱们也可以叫做序列化。（转化为磁盘文件）

留存一般叫录制（序列化）。

读取一般叫回放(反序列化)。

留存时也可以将文件进行分卷，也就是每个文件最大能占多大的大小，如果超过该大小，就新建一个文件继续留存。（类似于压缩文件的分卷）

这样的话，存的数据太大，我们一次性打开太慢，就可以分段打开。

需要依赖于rosbag2_cpp或者rosbag2_py

然后还要依赖于geometry_msgs，这个是因为我们要序列化的数据是这个包下的速度指令。

ros2 pkg create cpp02_rosbag --build-type ament_cmake --dependencies rclcpp rosbag2_cpp geometry_msgs --node-name cpp01_writer

rosbag2的命令工具

一般ROSBAG2的使用有命令行工具和编码两种使用方式，命令行工具功能比较齐全，够用。

查看帮助文档ros2 bag -h

主要有6个指令。

可以看record的详细用法。

record是用来将消息序列化的。(录制)

play是用来反序列化消息的。(回放)

info是用来输出bag文件的相关信息的。比如有多少条消息，录制起始时间和终止时间以及持续时间。

reindex是重建bag文件，可以修改bag源数据文件。

list是输出rosbag2中可用的插件(高阶应用)。

convert我们可以用这个给bag文件修改扩展名，也可以把多个bag文件合并成一个文件。

打开小乌龟节点与键盘控制小乌龟节点。

我们用键盘控制小乌龟，然后把速度指令通过rosbag2给序列化。

然后我们关掉两个节点，再重启小乌龟节点，然后这次不通过键盘控制，而是通过play bag文件让小乌龟运动。

record指令后面要跟一个话题组成的列表，但是在咱们下面的操作中，只用到了一个话题。

然后再用output，把序列化后的文件写出到一个磁盘目录中去。

cd进想保存bag的目录，然输入record指令，后面跟话题名称，然后-o +bag文件名，这里也可以不重新命名bag文件，这样会用默认的名字，默认的名称是年月日命名的。

这样就已经开始录制了。

按Ctrl + C进行结束，结束有个提示，说正在将消息写入bag，需要一段时间。

这个yaml文件是源数据文件。

db3是SQLite数据库，这个是移动端(比如手机)常用的数据库。

这个数据库就存储了录制的数据。

rosbag2 C++案例分析及框架搭建

add_executable(demo01_writer src/demo01_writer.cpp)
ament_target_dependencies(
  demo01_writer
  "rclcpp"
  "rosbag2_cpp"
  "geometry_msgs"
)

add_executable(demo02_reader src/demo02_reader.cpp)
ament_target_dependencies(
  demo02_reader
  "rclcpp"
  "rosbag2_cpp"
  "geometry_msgs"
)

install(TARGETS 
  demo01_writer
  demo02_reader
  DESTINATION lib/${PROJECT_NAME})

rosbag2 C++ 录制数据

/* 
  需求：录制 turtle_teleop_key 节点发布的速度指令。
  步骤：
    1.包含头文件；
    2.初始化 ROS 客户端；
    3.定义节点类；
      3-1.创建写出对象指针；
      3-2.设置写出的目标文件；
      3-3.写出消息。
    4.调用 spin 函数，并传入对象指针；
    5.释放资源。

 */
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "rosbag2_cpp/writer.hpp"
#include "geometry_msgs/msg/twist.hpp"

using std::placeholders::_1;

// 3.定义节点类；
class SimpleBagRecorder : public rclcpp::Node
{
public:
  SimpleBagRecorder()
  : Node("simple_bag_recorder")
  {
    // 3-1.创建写出对象指针；
    writer_ = std::make_unique<rosbag2_cpp::Writer>();
    // 3-2.设置写出的目标文件；(目录为ws目录)
    writer_->open("src/cpp02_rosbag/my_bag");
    subscription_ = create_subscription<geometry_msgs::msg::Twist>(
      "/turtle1/cmd_vel", 10, std::bind(&SimpleBagRecorder::topic_callback, this, _1));
  }

private:
  void topic_callback(std::shared_ptr<rclcpp::SerializedMessage> msg) const
  {
    rclcpp::Time time_stamp = this->now();
    // 3-3.写出消息。
    RCLCPP_INFO(this->get_logger(),"数据写出... ...");
    writer_->write(msg, "/turtle1/cmd_vel", "geometry_msgs/msg/Twist", time_stamp);
  }

  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscription_;
  std::unique_ptr<rosbag2_cpp::Writer> writer_;
};

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

这是一个相对目录，目录位置是工作空间目录。

写数据之前，要先创建一个订阅方，订阅方要建立一个回调函数。

回调函数入口参数，消息类型用write函数的入口参数。

这里要用斜杠代替冒号，因为入口是string类型。

创建乌龟节点。

启动writer节点

运行小乌龟

运行一会儿后，关掉所有节点。

成功生成了文件

验证录制文件是否成功。

在验证前，先创建一个乌龟节点，不用创建控制节点。

ros2 bag play src/cpp02_rosbag/my_bag

可以看到乌龟正常走了。回放成功！

如果已经生成了一遍my_bag，再想生成新的会显示不能覆盖。

解决方案，可以把my_bag设置为动态的，加个时间戳或者直接按功能命名。

rosbag2 C++ 读取数据

/* 
  需求：读取 bag 文件数据。
  步骤：
    1.包含头文件；
    2.初始化 ROS 客户端；
    3.定义节点类；
      3-1.创建读取对象指针；
      3-2.设置读取的目标文件；
      3-3.读消息；
      3-4.关闭文件。
    4.调用 spin 函数，并传入对象指针；
    5.释放资源。

 */
 // 1.包含头文件；
 #include "rclcpp/rclcpp.hpp"
 #include "rosbag2_cpp/reader.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 // 3.定义节点类；
class SimpleBagPlayer: public rclcpp::Node {
public:
    SimpleBagPlayer():Node("simple_bag_player"){
        // 3-1.创建读取对象指针；
        reader_ = std::make_unique<rosbag2_cpp::Reader>();
        // 3-2.设置读取的目标文件；
        reader_->open("src/cpp02_rosbag/my_bag");
        // 3-3.读消息；
        while (reader_->has_next())
        {
            auto twist = reader_->read_next<geometry_msgs::msg::Twist>();
            RCLCPP_INFO(this->get_logger(),"线速度:%.2f, 角速度: %.2f",twist.linear.x, twist.angular.z);
        }
        // 3-4.关闭文件。
        reader_->close();
    }
private:
    std::unique_ptr<rosbag2_cpp::Reader> reader_;

};

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

编译

这显示能读出来几条信息。能读出来8条。

正好8条

坐标变换TF
坐标变换
引言与应用场景

里程计ODOM

惯性计IMU

激光雷达Laser

摄像头Camera

场景1：现有一移动式机器人底盘，在底盘上安装了一雷达，雷达相对于底盘的偏移量已知，现雷达检测到一障碍物信息，获取到坐标分别为(x,y,z)，该坐标是以雷达为参考系的，如何将这个坐标转换成以小车为参考系的坐标呢？

激光雷达与小车的中心或边缘相差的横纵距离，以及激光雷达与墙的距离及小车与墙的距离。

场景2:现有一带机械臂的机器人(比如:PR2)需要夹取目标物，当前机器人头部摄像头可以探测到目标物的坐标(x,y,z)，不过该坐标是以摄像头为参考系的，而实际操作目标物的是机械臂的夹具，当前我们需要将该坐标转换成相对于机械臂夹具的坐标，这个过程如何实现？

以上通过TF即可算

概念与作用

TF实行右手坐标系

重要的就是相对位置和时间，在某个时间某个物体位于某个位置。（时间差太大，数据会被废弃）

案例安装以及运行

安装乌龟案列：

# Humble版本安装
sudo apt-get install ros-humble-turtle-tf2-py ros-humble-tf2-tools ros-humble-tf-transformations
# Jazzy版本安装
sudo apt-get install ros-jazzy-turtle-tf2-py ros-jazzy-tf2-tools ros-jazzy-tf-transformations

此外，还需要安装一个名为 transforms3d 的 Python 包，它为 tf_transformations包提供四元数和欧拉角变换功能，安装命令如下：

# 方式一（不推荐）
sudo apt intall python3-pip
pip3 install transforms3d

#方式二（推荐）
sudo apt install python3-transforms3d

启动两个终端，终端1输入如下命令：

ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py

该命令会启动 turtlesim_node 节点，turtlesim_node 节点中自带一只小乌龟 turtle1，除此之外还会新生成一只乌龟 turtle2，turtle2 会运行至 turtle1 的位置。

终端2输入如下命令：

ros2 run turtlesim turtle_teleop_key

该终端下可以通过键盘控制 turtle1 运动，并且 turtle2 会跟随 turtle1 运动（参考引言部分的 案例1 ）。

龟男🐢🚹会跟随前面的乌龟🐢运动

坐标变换相关消息

坐标变换的实现其本质是基于话题通信的发布订阅模型的，发布方可以发布坐标系之间的相对关系，订阅方则可以监听这些消息，并实现不同坐标系之间的变换。显然的根据之前介绍，在话题通信中，接口消息作为数据载体在整个通信模型中是比较重要的一部分，本节将会介绍坐标变换中常用的两种接口消息：geometry_msgs/msg/TransformStamped和geometry_msgs/msg/PointStamped。

前者用于描述某一时刻两个坐标系之间相对关系的接口，后者用于描述某一时刻坐标系内某个坐标点的位置的接口。在坐标变换中，会经常性的使用到坐标系相对关系以及坐标点信息。

geometry_msgs/msg/TransformStamped

通过如下命令查看接口定义：

ros2 interface show geometry_msgs/msg/TransformStamped

接口定义解释：

std_msgs/Header header
    builtin_interfaces/Time stamp     # 时间戳
        int32 sec         #秒
        uint32 nanosec    #纳秒
    string frame_id                   # 父级坐标系
string child_frame_id                 # 子级坐标系
Transform transform                   # 子级坐标系相对于父级坐标系的位姿
    Vector3 translation               # 三维偏移量
        float64 x
        float64 y
        float64 z
    Quaternion rotation               # 四元数
        float64 x 0
        float64 y 0
        float64 z 0
        float64 w 1

描述一个物体运动一般有6个自由度：X，Y，Z，Yaw，Pitch，Roll。

三个平动，三个旋转：

Vector3 translation代表3个平移

Quaternion rotation四元数可以转化为三个欧拉角(yaw，pitch，roll)

（Q：为何不用欧拉角而用四元数？A：因为用欧拉角计算会出现死锁现象，所以选择用四元数，而不用欧拉角，以便避免欧拉角的缺陷。）

3个平移以米meter为单位

3个旋转以弧度rad为单位

四元数类似于欧拉角用于表示坐标系的相对姿态，

具体转化详见大疆开发板C型嵌入式软件教程文档.pdf的18.3节

具体转化算法（Mahony算法）（ROS2的TF2库中也有具体的转化算法）：

https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

按右手坐标系来看，N2相对于N1沿X轴平移了1m(一格代表1米）。

旋转

geometry_msgs/msg/PointStamped

通过如下命令查看接口定义：

ros2 interface show geometry_msgs/msg/PointStamped

接口定义解释：

std_msgs/Header header
    builtin_interfaces/Time stamp    # 时间戳
        int32 sec   #秒
        uint32 nanosec   #纳秒
    string frame_id                  # 参考系
Point point                          # 三维坐标
    float64 x
    float64 y
    float64 z

在三维中的坐标点

坐标变换广播
引言与案例及分析

坐标系相对关系主要有两种： 静态坐标系相对关系 与 动态坐标系相对关系 。

所谓静态坐标系相对关系是指两个坐标系之间的相对位置是固定不变的，比如：车辆上的雷达、摄像头等组件一般是固定式的，那么雷达坐标系相对于车辆底盘坐标系或摄像头坐标系相对于车辆底盘坐标系就是一种静态关系。

所谓动态坐标系相对关系是指两个坐标系之间的相对位置关系是动态改变的，比如：车辆上机械臂的关节或夹爪、多车编队中不同车辆等都是可以运动的，那么机械臂的关节或夹爪坐标系相对车辆底盘坐标系或不同车辆坐标系的相对关系就是一种动态关系。

本节会主要介绍如何实现静态坐标变换广播与动态坐标变换广播。另外，本节还会演示如何发布坐标点消息。

激光雷达Laser和色相头与底盘位置是静态的，而机械臂的末端执行器与机器人的位置是动态的。

1.案例需求

案例1： 现有一无人车，在无人车底盘上装有固定式的雷达与摄像头，已知车辆底盘、雷达与摄像头各对应一坐标系，各坐标系的原点取其几何中心。现又已知雷达坐标系相对于底盘坐标系的三维平移量分别为：x方向0.4米，y方向0米，z方向0.2米，无旋转。摄像头坐标系相对于底盘坐标系的三维平移量分别为：x方向-0.5米，y方向0米，z方向0.4米，无旋转。请广播雷达与底盘的坐标系相对关系，摄像头与底盘的坐标系相对关系，并在 rviz2 中查看广播的结果。

案例2： 启动 turtlesim_node，设该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，乌龟可以通过键盘控制运动，请动态发布乌龟坐标系与世界坐标系的相对关系。

2.案例分析

在上述案例中，案例1需要使用到静态坐标变换，案例2则需要使用动态坐标变换，不论无论何种实现关注的要素都有两个：

如何广播坐标系相对关系；
如何使用 rviz2 显示坐标系相对关系。

3.流程简介

以编码的实现实现静态或动态坐标变换的流程类似，主要步骤如下：

编写广播实现；
编辑配置文件；
编译；
执行；
在 rviz2 中查看坐标系关系。

案例我们会采用 C++ 和 Python 分别实现，二者都遵循上述实现流程。

另外：需要说明的是，静态广播器除了可以以编码的方式实现外，在 tf2 中还内置了相关工具，可以无需编码，直接执行节点并传入表示坐标系相对关系的参数，即可实现静态坐标系关系的发布（即命令行，有命令行可以优先用命令行）。而动态广播器没有提供类似的工具。（即必须敲代码）

4.准备工作

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

ros2 pkg create cpp03_tf_broadcaster --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs turtlesim

tf2功能包内包含了四元数与欧拉角的转换算法。

tf2_ros功能包内包含了广播对象。

geometry_msgs功能包消息载体

turtlesim功能包是获取乌龟🐢位姿

静态广播器_命令行实现

1.静态广播器工具

在 tf2_ros功能包中提供了一个名为static_transform_publisher的可执行文件，通过该文件可以直接广播静态坐标系关系，其使用语法如下。

格式1：

使用以米为单位的 x/y/z 偏移量和以弧度为单位的roll/pitch/yaw（可直译为滚动/俯仰/偏航，分别指的是围绕 x/y/z 轴的旋转）向 tf2 发布静态坐标变换。

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --yaw yaw --pitch pitch --roll roll --frame-id frame_id --child-frame-id child_frame_id

父级坐标系和子级坐标系是必须要写的。

如果这些可选参数不选的话，默认父级坐标系和子级坐标系重合，也就是偏移量和旋转度都是0。

偏移量：X,Y,Z

旋转度：QX,QY,QZ,QW 或者 ROLL,PITCH,YAW（单位是弧度）

时间戳：不用设置，会以发布的时间为起点

格式2：

使用以米为单位的 x/y/z 偏移量和 qx/qy/qz/qw 四元数向 tf2 发布静态坐标变换。

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --qx qx --qy qy --qz qz --qw qw --frame-id frame_id --child-frame-id child_frame_id

注意：在上述两种格式中除了用于表示父级坐标系的--frame-id和用于表示子级坐标系的--child-frame-id之外，其他参数都是可选的，如果未指定特定选项，那么将直接使用默认值。

2.静态广播器工具使用

打开两个终端，终端1输入如下命令发布雷达（laser）相对于底盘（base_link）的静态坐标变换（重合）：

ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id laser

base_link是父级参考系

laser是子级参考系

一般选父系参考系

3.rviz2 查看坐标系关系

新建终端，通过命令rviz2打开 rviz2 并配置相关插件查看坐标变换消息：

将 Global Options 中的 Fixed Frame 设置为 base_link；
点击 add 按钮添加 TF 插件；
勾选 TF 插件中的 show names。

右侧 Grid 中将以图形化的方式显示坐标变换关系。

如图，两个参考系重合。

打开两个终端，终端1输入如下命令发布雷达（laser）相对于底盘（base_link）的静态坐标变换：

ros2 run tf2_ros static_transform_publisher --x 0.4 --y 0 --z 0.2 --yaw 0.5 --roll 0 --pitch 0 --frame-id base_link --child-frame-id laser

终端2输入如下命令发布摄像头（camera）相对于底盘（base_link）的静态坐标变换：

ros2 run tf2_ros static_transform_publisher --x -0.5 --y 0 --z 0.4 --yaw 0 --roll 0 --pitch 0 --frame-id base_link --child-frame-id camera

静态广播器_C++实现
框架搭建

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(turtlesim REQUIRED)

add_executable(demo01_static_tf_broadcaster src/demo01_static_tf_broadcaster.cpp)
ament_target_dependencies(
  demo01_static_tf_broadcaster
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "turtlesim"
)

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

广播实现

/*  
  需求：编写静态坐标变换程序，执行时传入两个坐标系的相对位姿关系以及父子级坐标系id，
       程序运行发布静态坐标变换。
  步骤：
    1.包含头文件；
    2.判断终端传入的参数是否合法；
    3.初始化 ROS 客户端；
    4.定义节点类；
      4-1.创建静态坐标变换发布方；
      4-2.组织并发布消息。
    5.调用 spin 函数，并传入对象指针；
    6.释放资源。

*/

// 1.包含头文件；
#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>

using std::placeholders::_1;

// 4.定义节点类；
class MinimalStaticFrameBroadcaster : public rclcpp::Node
{
public:
  explicit MinimalStaticFrameBroadcaster(char * transformation[]): Node("minimal_static_frame_broadcaster")
  {
    // 4-1.创建静态坐标变换发布方；
    tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

    this->make_transforms(transformation);
  }

private:
  // 4-2.组织并发布消息。
  void make_transforms(char * transformation[])
  {
    // 组织消息
    geometry_msgs::msg::TransformStamped t;

    rclcpp::Time now = this->get_clock()->now();
    t.header.stamp = now;
    t.header.frame_id = transformation[7];
    t.child_frame_id = transformation[8];

    t.transform.translation.x = atof(transformation[1]);
    t.transform.translation.y = atof(transformation[2]);
    t.transform.translation.z = atof(transformation[3]);
    tf2::Quaternion q;
    q.setRPY(
      atof(transformation[4]),
      atof(transformation[5]),
      atof(transformation[6]));
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    // 发布消息
    tf_publisher_->sendTransform(t);
  }
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_publisher_;
};

int main(int argc, char * argv[])
{
  // 2.判断终端传入的参数是否合法；
  auto logger = rclcpp::get_logger("logger");

  if (argc != 9) {
    RCLCPP_INFO(
      logger, "运行程序时请按照：x y z roll pitch yaw frame_id child_frame_id 的格式传入参数");
    return 1;
  }

  // 3.初始化 ROS 客户端；
  rclcpp::init(argc, argv);
  // 5.调用 spin 函数，并传入对象指针；
  rclcpp::spin(std::make_shared<MinimalStaticFrameBroadcaster>(argv));
  // 6.释放资源。
  rclcpp::shutdown();
  return 0;
}

必须传参正确，否则抛错。

创建组织并发布数据的函数

用最简单的重载。

这样就可以发送了，接下来编辑发送的内容。

stamp时间戳设置为当前时间，now()函数是设置为当前时间。

atof()是转化为float浮点类型

x()和getx()都是获取四元数x。（y，z，w以此类推）

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

colcon build --packages-select cpp03_tf_broadcaster

当前工作空间下，启动两个终端，终端1输入如下命令发布雷达（laser）相对于底盘（base_link）的静态坐标变换：

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster 0.4 0 0.2 0 0 0 base_link laser

参考 静态广播器（命令） 内容启动并配置 rviz2，最终执行结果与案例1类似。

本质就是话题通信，但这个话题通信的topic是啥呢？

通过查看该类源码，就得知，话题为/tf_static

发布方有俩，订阅方有一个。

这俩是发布方

这个是订阅方

动态广播器_C++实现
框架搭建

CMakeLists.txt 文件需要添加如下内容：

add_executable(demo02_dynamic_tf_broadcaster src/demo02_dynamic_tf_broadcaster.cpp)
ament_target_dependencies(
  demo02_dynamic_tf_broadcaster
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "turtlesim"
)

文件中 install 修改为如下内容：

install(TARGETS demo01_static_tf_broadcaster
  demo02_dynamic_tf_broadcaster
  DESTINATION lib/${PROJECT_NAME})

广播实现

/*   
  需求：编写动态坐标变换程序，启动 turtlesim_node 以及 turtle_teleop_key 后，该程序可以发布
       乌龟坐标系到窗口坐标系的坐标变换，并且键盘控制乌龟运动时，乌龟坐标系与窗口坐标系的相对关系
       也会实时更新。

  步骤：
    1.包含头文件；
    2.初始化 ROS 客户端；
    3.定义节点类；
      3-1.创建动态坐标变换发布方；
      3-2.创建乌龟位姿订阅方；
      3-3.根据订阅到的乌龟位姿生成坐标帧并广播。
    4.调用 spin 函数，并传入对象指针；
    5.释放资源。

*/
// 1.包含头文件；
#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <turtlesim/msg/pose.hpp>

using std::placeholders::_1;

// 3.定义节点类；
class MinimalDynamicFrameBroadcaster : public rclcpp::Node
{
public:
  MinimalDynamicFrameBroadcaster(): Node("minimal_dynamic_frame_broadcaster")
  {
    // 3-1.创建动态坐标变换发布方；
    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    std::string topic_name = "/turtle1/pose";

    // 3-2.创建乌龟位姿订阅方；
    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
      topic_name, 10,
      std::bind(&MinimalDynamicFrameBroadcaster::handle_turtle_pose, this, _1));
  }

private:
  // 3-3.根据订阅到的乌龟位姿生成坐标帧并广播。   
  void handle_turtle_pose(const turtlesim::msg::Pose & msg)
  {
    // 组织消息
    geometry_msgs::msg::TransformStamped t;
    rclcpp::Time now = this->get_clock()->now();

    t.header.stamp = now;
    t.header.frame_id = "world";   //窗体坐标系
    t.child_frame_id = "turtle1";  //乌龟坐标系

    t.transform.translation.x = msg.x;
    t.transform.translation.y = msg.y;
    t.transform.translation.z = 0.0;      //乌龟在平面内运动

    //从欧拉角转换为四元数
    tf2::Quaternion q;
    q.setRPY(0, 0, msg.theta);        //乌龟只有Yaw
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();
    // 发布消息
    tf_broadcaster_->sendTransform(t);
  }
  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

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

入口参数：

参数1话题名称

参数2QoS

参数3回调函数

操控乌龟运动，会使乌龟在Rviz2里也运动

坐标点发布_C++实现
案例与分析

案例需求

案例： 无人车上安装有激光雷达，现激光雷达扫描到一点状障碍物并且可以定位障碍物的坐标，请在雷达坐标系下发布障碍物坐标点数据，并在 rviz2 中查看发布结果。

案例分析

上述案例，是一个简单的话题发布程序，在了解坐标点geometry_msgs/msg/PointStamped接口消息之后，直接通过话题发布方按照一定逻辑发布消息即可。

流程简介

程序实现主要步骤如下：

编写话题发布实现；
编辑配置文件；
编译；
执行；
在 rviz2 中查看运行结果。

案例我们会采用 C++ 和 Python 分别实现，二者都遵循上述实现流程。

CMakeLists.txt 文件需要添加如下内容：

add_executable(demo03_point_publisher src/demo03_point_publisher.cpp)
ament_target_dependencies(
  demo03_point_publisher
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "turtlesim"
)

文件中 install 修改为如下内容：

install(TARGETS demo01_static_tf_broadcaster
  demo02_dynamic_tf_broadcaster
  demo03_point_publisher
  DESTINATION lib/${PROJECT_NAME})

实现

/*  
    需求：发布雷达坐标系中某个坐标点相对于雷达（laser）坐标系的位姿。
    步骤：
        1.包含头文件；
        2.初始化 ROS 客户端；
        3.定义节点类；
            3-1.创建坐标点发布方；
            3-2.创建定时器；
            3-3.组织并发布坐标点消息。
        4.调用 spin 函数，并传入对象指针；
        5.释放资源。

*/
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"

using namespace std::chrono_literals;

// 3.定义节点类；
class MinimalPointPublisher: public rclcpp::Node {
public:
    MinimalPointPublisher(): Node("minimal_point_publisher"),x(0.1){
        // 3-1.创建坐标点发布方；
        point_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>("point",10);
        // 3-2.创建定时器；
        timer_ = this->create_wall_timer(0.1s,std::bind(&MinimalPointPublisher::on_timer, this));
    }
private:
    void on_timer(){
        // 3-3.组织并发布坐标点消息。
        geometry_msgs::msg::PointStamped point;
        point.header.frame_id = "laser";
        point.header.stamp = this->now();
        x += 0.004;
        point.point.x = x;
        point.point.y = 0.0;
        point.point.z = 0.1;        
        point_pub_->publish(point);
    }
    rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr point_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    double_t x;
};

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

创建定时器用create_wall_time()函数，要填时间间隔和对应的回调函数。

执行

当前工作空间下，启动两个终端，终端1输入如下命令发布雷达（laser）相对于底盘（base_link）的静态坐标变换：

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster 0.4 0 0.2 0 0 0 base_link laser

终端2输入如下命令发布障碍物相对于雷达（laser）的坐标点：

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo03_point_publisher

rviz2 查看坐标系关系

参考 5.3.2 静态广播器（命令） 内容启动并配置 rviz2，显示坐标变换后，再添加 PointStamped 插件并将其话题设置为 /point，最终显示结果与案例演示类似。

通过这里可以改球的透明度和大小

小结

静态广播只发布一次。

而动态广播和坐标点广播都是发布多次。

但实质上就是话题通信。

坐标变换监听
案例与分析

案例1： 在 5.3 坐标变换广播 中发布了laser相对于base_link和camra相对于base_link的坐标系关系，请求解laser相对于camera的坐标系关系。

案例2： 在 5.3 坐标变换广播 中发布了laser相对于base_link的坐标系关系且发布了laser坐标系下的障碍物的坐标点数据，请求解base_link坐标系下该障碍物的坐标。

案例分析

在上述案例中，案例1是多坐标系的场景下实现不同坐标系之间的变换，案例2则是要实现同一坐标点在不同坐标系下的变换，虽然需求不同，但是相关算法都被封装好了，我们只需要调用相关 API 即可。

流程简介

两个案例的实现流程类似，主要步骤如下：

编写坐标变换程序实现；
编辑配置文件；
编译；
执行。

案例我们会采用 C++ 和 Python 分别实现，二者都遵循上述实现流程。

准备工作

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

ros2 pkg create cpp04_tf_listener --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs --node-name demo01_tf_listener

坐标系变换监听_C++
实例分析

与之前实现不太一样，要保存到buffer中。

因为之前的广播是发一条订阅一条。

但是在坐标变换中，是多对一实现的。

多个广播发布的消息组成一个坐标树，要从坐标树中获取不同坐标帧的变换。

把多条广播方的消息组成坐标树，就要使用buffer，把消息全部存到缓存buffer中。

这里为何要做异常处理呢？

因为进程间的通信开销比较大，是有延迟的，可能程序要开始做变换了，可惜消息还没订阅到。

消息都没有，就会抛异常。直到buffer里有数据，坐标也转化成功，才不会抛异常。

实现

/*  
  需求：订阅 laser 到 base_link 以及 camera 到 base_link 的坐标系关系，
       并生成 laser 到 camera 的坐标变换。
  步骤：
    1.包含头文件；
    2.初始化 ROS 客户端；
    3.定义节点类；
      3-1.创建tf缓存对象指针；融合多个坐标系相对关系为一棵坐标树。
      3-2.创建tf监听器；指定缓存对象，会将所有广播器广播的数据写入缓存。
      3-3.编写定时器，循环实现转换；按照条件查找符合条件的坐标系并生成变换后的坐标帧。
    4.调用 spin 函数，并传入对象指针；
    5.释放资源。

*/
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2/LinearMath/Quaternion.h"

using namespace std::chrono_literals;

// 3.定义节点类；
class MinimalFrameListener : public rclcpp::Node {
public:
  MinimalFrameListener():Node("minimal_frame_listener"){
    tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
    transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
    timer_ = this->create_wall_timer(1s, std::bind(&MinimalFrameListener::on_timer,this));
  }

private:
  void on_timer(){
    try
    {
      auto transformStamped = tf_buffer_->lookupTransform("camera","laser",tf2::TimePointZero);
      RCLCPP_INFO(this->get_logger(),"----------------------转换结果----------------------");
      RCLCPP_INFO(this->get_logger(),"frame_id:%s",transformStamped.header.frame_id.c_str());
      RCLCPP_INFO(this->get_logger(),"child_frame_id:%s",transformStamped.child_frame_id.c_str());
      RCLCPP_INFO(this->get_logger(),"坐标:(%.2f,%.2f,%.2f)",
                transformStamped.transform.translation.x,
                transformStamped.transform.translation.y,
                transformStamped.transform.translation.z);

    }
    catch(const tf2::LookupException& e)
    {
      RCLCPP_INFO(this->get_logger(),"坐标变换异常：%s",e.what());
    }

  }
  rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
};

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

要填回调函数

实现坐标系转换的核心函数就是lookuptransform()

target_frame就是父级

source_frame就是子级

time是时间，一般都写最新时刻的

如果buffer没捕获到，抛异常的函数实现

这样就成功转换坐标了，可以打印转换的坐标。

除了用try catch处理转换异常，还可以用buffer底下的函数转换。

比如这个cantransform可以判断是否可以正常转换。

这种报错要改为C风格字符串

启动三个终端，终端1输入如下命令发布雷达（laser）相对于底盘（base_link）的静态坐标变换：

ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id camera --x -0.5 --z -0.4

终端2输入如下命令发布摄像头（camera）相对于底盘（base_link）的静态坐标变换：

ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id laser--x 0.4 --z 0.2

终端3输入如下命令执行坐标系变换：

. install/setup.bash 
ros2 run cpp04_tf_listener demo01_tf_listener

终端3将输出 laser 相对于 camera 的坐标，具体结果请参考案例1。

当只运行一个广播，他会抛错。

当所有广播都跑起来时，才会正常转换。

坐标点变换监听_C++
实现框架

求laser测得的坐标点到base_link的距离

laser到point的坐标用的是point话题，与监听器用的话题不一样，所以不好订阅，要创建一个新的订阅对象。

框架搭建

package.xml在创建功能包时，所依赖的部分功能包已经自动配置了，不过为了实现坐标点变换，还需要添加依赖包tf2_geometry_msgs和

message_filters，修改后的配置内容如下：

<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>geometry_msgs</depend>
<depend>tf2_geometry_msgs</depend>
<depend>message_filters</depend>

CMakeLists.txt 文件修改后的内容如下：

find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(message_filters REQUIRED)

add_executable(demo01_tf_listener src/demo01_tf_listener.cpp)
ament_target_dependencies(
  demo01_tf_listener
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
)
add_executable(demo02_message_filter src/demo02_message_filter.cpp)
ament_target_dependencies(
  demo02_message_filter
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "tf2_geometry_msgs"
  "message_filters"
)
install(TARGETS demo01_tf_listener
  demo02_message_filter
  DESTINATION lib/${PROJECT_NAME})

实现

/*  
  需求：将雷达感知到的障碍物的坐标点数据（相对于 laser 坐标系），
       转换成相对于底盘坐标系（base_link）的坐标点。

  步骤：
    1.包含头文件；
    2.初始化 ROS 客户端；
    3.定义节点类；
      3-1.创建tf缓存对象指针；
      3-2.创建tf监听器；
      3-3.创建坐标点订阅方并订阅指定话题；
      3-4.创建消息过滤器过滤被处理的数据；
      3-5.为消息过滤器注册转换坐标点数据的回调函数。
    4.调用 spin 函数，并传入对象指针；
    5.释放资源。

*/// 1.包含头文件；#include <geometry_msgs/msg/point_stamped.hpp>#include <message_filters/subscriber.h>#include <rclcpp/rclcpp.hpp>#include <tf2_ros/buffer.h>#include <tf2_ros/create_timer_ros.h>#include <tf2_ros/message_filter.h>#include <tf2_ros/transform_listener.h>// #ifdef TF2_CPP_HEADERS//   #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>// #else//   #include <tf2_geometry_msgs/tf2_geometry_msgs.h>// #endif#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>using namespace std::chrono_literals;

// 3.定义节点类；class MessageFilterPointListener : public rclcpp::Node
{
public:
  MessageFilterPointListener(): Node("message_filter_point_listener")
  {

    target_frame_ = "base_link";

    typedef std::chrono::duration<int> seconds_type;
    seconds_type buffer_timeout(1);
    // 3-1.创建tf缓存对象指针；
    tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
      this->get_node_base_interface(),
      this->get_node_timers_interface());
    tf2_buffer_->setCreateTimerInterface(timer_interface);
    // 3-2.创建tf监听器；
    tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);

    // 3-3.创建坐标点订阅方并订阅指定话题；
    point_sub_.subscribe(this, "point");
    // 3-4.创建消息过滤器过滤被处理的数据；
    tf2_filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(
      point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(),
      this->get_node_clock_interface(), buffer_timeout);
    // 3-5.为消息过滤器注册转换坐标点数据的回调函数。
    tf2_filter_->registerCallback(&MessageFilterPointListener::msgCallback, this);
  }

private:
  void msgCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_ptr){
    geometry_msgs::msg::PointStamped point_out;
    try {
      tf2_buffer_->transform(*point_ptr, point_out, target_frame_);
      RCLCPP_INFO(
        this->get_logger(), "坐标点相对于base_link的坐标:(%.2f,%.2f,%.2f)",
        point_out.point.x,
        point_out.point.y,
        point_out.point.z);
    } catch (tf2::TransformException & ex) {
      RCLCPP_WARN(
        // Print exception which was caughtthis->get_logger(), "Failure %s\n", ex.what());
    }
  }
  std::string target_frame_;
  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
  message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub_;
  std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> tf2_filter_;
};

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

把上面此参数设置一下

进行数据解析

用第一个重载。

转换过程会抛异常，可以不管他。转化失败会在终端上自动抛异常。

在当前工作空间下，启动三个终端，终端1输入如下命令发布雷达（laser）相对于底盘（base_link）的静态坐标变换：

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster 0.4 0 0.2 0 0 0 base_link laser

终端2输入如下命令发布障碍物相对于雷达（laser）的坐标点：

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo03_point_publisher

终端3输入如下命令执行坐标点变换：

. install/setup.bash 
ros2 run cpp04_tf_listener demo02_message_filter

终端3将输出坐标点相对于 base_link 的坐标，具体结果请参考案例2。

按顺序依次发布

坐标变换工具

在 ROS2 的 TF 框架中除了封装了坐标广播与订阅功能外，还提供了一些工具，可以帮助我们提高开发、调试效率。本节主要介绍这些工具的使用，这些工具主要涉及到两个功能包：tf2_ros和tf2_tools。

tf2_ros包中提供的常用节点如下：

static_transform_publisher：该节点用于广播静态坐标变换；（用过很多次了）
tf2_monitor：该节点用于打印所有或特定坐标系的发布频率与网络延迟；
tf2_echo：该节点用于打印特定坐标系的平移旋转关系。

tf2_tools包中提供的节点如下：

view_frames：该节点可以生成显示坐标系关系的 pdf 文件，该文件包含树形结构的坐标系图谱。

上述诸多工具中，功能包tf2_ros中的static_transform_publisher节点在 静态广播器（命令） 一节中已有详细说明，本节不再介绍。

准备工作：

为了更好的演示工具的使用，请先启动若干坐标系广播节点，比如：可以按照 静态广播器（命令） 和 动态广播器（C++） 广播一些坐标系消息。

第一个终端：

ros2 run tf2_ros static_transform_publisher --x 0.4 --y 0 --z 0.2 --yaw 0.5 --roll 0 --pitch 0 --frame-id base_link --child-frame-id laser

第二个终端：

ros2 run tf2_ros static_transform_publisher --x -0.5 --y 0 --z 0.4 --yaw 0 --roll 0 --pitch 0 --frame-id base_link --child-frame-id camera

第三个终端：

ros2 run turtlesim turtlesim_node

第四个终端：

ros2 run turtlesim turtle_teleop_key

第五个终端：

source install/setup.bash
ros2 run cpp03_tf_broadcaster demo02_dynamic_tf_broadcaster

以上是准备工作

1.tf2_monitor

1.1打印所有坐标系的发布频率与网络延迟

终端执行命令：

ros2 run tf2_ros tf2_monitor

运行结果：

该命令有10s的阻塞时间，因为要在一个区间内测频率。

静态的没有变化，而动态的发布频率是有一些变化的。

该函数每隔10s会再发布一次。

1.2打印指定坐标系的发布频率与网络延迟

终端执行命令：

ros2 run tf2_ros tf2_monitor camera laser

运行结果：

刚开始在缓存里拿不到数据抛错很正常。

2.tf2_echo

打印两个坐标系的平移旋转关系。

终端执行命令：

ros2 run tf2_ros tf2_echo world turtle1

运行结果：

会出平移量和旋转量

会以好几种方式表现，如平移距离，四元数，弧度欧拉角，角度欧拉角，矩阵等。

当龟男🐢🚹动弹的时候，数值也会发生改变。

3.view_frames（常用）

以图形化的方式显示坐标系关系。

终端执行命令：

ros2 run tf2_tools view_frames

运行结果：将会在当前目录下生成 frames_xxxx.gv 与 frames_xxxx.pdf 文件，其中 xxxx 为时间戳。打开 pdf 文件显示如下内容：

此节点会监听我们5秒钟，并生成对应的文件。

上图中有两棵坐标树，但是实际上的项目中，咱们只能设计一棵树，而不能设计两个坐标树。

练习
乌龟跟随
乌龟护航
小结
可视化平台RVIZ2与URDF建模语言
可视化简介

坐标相关、激光雷达相关、摄像头相关的rviz2插件

rviz2基本使用

以sudo apt install ros-[ROS_DISTRO]-desktop格式安装ROS2时，RViz已经默认被安装了。

sudo apt install ros-[ROS_DISTRO]-rviz2

备注： 命令中的 [ROS_DISTRO] 指代ROS2版本。

方式1：rviz2；

方式2：ros2 run rviz2 rviz2。

rviz2 启动之后，默认界面如下：

上部为工具栏：包括视角控制、预估位姿设置、目标设置等，还可以添加自定义插件；
左侧为插件显示区：包括添加、删除、复制、重命名插件，显示插件，以及设置插件属性等功能；
中间为3D试图显示区：以可视化的方式显示添加的插件信息；
右侧为观测视角设置区：可以设置不同的观测视角；
下侧为时间显示区：包括系统时间和ROS时间。

左侧插件显示区默认有两个插件：

Global Options：该插件用于设置全局显示相关的参数，一般情况下，需要自行设置的是 Fixed Frame 选项，该选项是其他所有数据在可视化显示时所参考的全局坐标系；
Global Status：该插件用于显示在 Global Options 设置完毕 Fixed Frame 之后，所有的坐标变换是否正常。

最上面是菜单区，左侧是插件显示区，中间是3D调试区，右侧是视角切换区域，最下方是时间区ROS Time是ROS2时间，Wall Time是系统时间，Elapsed是Rviz2运行的时间。

可以保存rviz2的配置，也可以打开配置

设置显示面板

可以设置平面网格的个数

可以设置竖直方向网格的个数

可以设置网格边长是多大

改变sRGB的4个通道

可以改变视角

设置偏移量，如果Z是-1，那么网格会相对于坐标系下沉1个单位

Fixed name一般是根坐标系的名称

background color就是背景色

frame rate是坐标系的发布频率

全局状态，当fixed name设置对后，就无警告了

视角切换(一般默认)

可以翻转Z轴

常用插件：

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 序号 | 名称 | 功能 | 消息类型 | |:—|:—|:—|:—| | 1 | Axes | 显示 rviz2 默认的坐标系。 | | | 2 | Camera | 显示相机图像，必须需要使用消息：CameraInfo。 | sensor_msgs/msg/Image，sensor_msgs/msg/CameraInfo | | 3 | Grid | 显示以参考坐标系原点为中心的网格。 | | | 4 | Grid Cells | 从网格中绘制单元格，通常是导航堆栈中成本地图中的障碍物。 | nav_msgs/msg/GridCells | | 5 | Image | 显示相机图像，但是和Camera插件不同，它不需要使用 CameraInfo 消息。 | sensor_msgs/msg/Image | | 6 | InteractiveMarker | 显示来自一个或多个交互式标记服务器的 3D 对象，并允许与它们进行鼠标交互。 | visualization_msgs/msg/InteractiveMarker | | 7 | Laser Scan | 显示激光雷达数据。 | sensor_msgs/msg/LaserScan | | 8 | Map | 显示地图数据。 | nav_msgs/msg/OccupancyGrid | | 9 | Markers | 允许开发者通过主题显示任意原始形状的几何体。 | visualization_msgs/msg/Marker，visualization_msgs/msg/MarkerArray | | 10 | Path | 显示机器人导航中的路径相关数据。 | nav_msgs/msg/Path | | 11 | PointStamped | 以小球的形式绘制一个点。 | geometry_msgs/msg/PointStamped | | 12 | Pose | 以箭头或坐标轴的方式绘制位姿。 | geometry_msgs/msg/PoseStamped | | 13 | Pose Array | 绘制一组 Pose。 | geometry_msgs/msg/PoseArray | | 14 | Point Cloud2 | 绘制点云数据。 | sensor_msgs/msg/PointCloud，sensor_msgs/msg/PointCloud2 | | 15 | Polygon | 将多边形的轮廓绘制为线。 | geometry_msgs/msg/Polygon | | 16 | Odometry | 显示随着时间推移累积的里程计消息。 | nav_msgs/msg/Odometry | | 17 | Range | 显示表示来自声纳或红外距离传感器的距离测量值的圆锥。 | sensor_msgs/msg/Range | | 18 | RobotModel | 显示机器人模型。 | | | 19 | TF | 显示 tf 变换层次结构。 | | | 20 | Wrench | 将geometry_msgs /WrenchStamped消息显示为表示力的箭头和表示扭矩的箭头加圆圈。 | geometry_msgs/msg/WrenchStamped | | 21 | Oculus | 将 RViz 场景渲染到 Oculus 头戴设备。 | |

上述每一种插件又包含了诸多属性，可以通过设置插件属性来控制插件的最终显示效果。

Image是摄像头数据插件

LaserScan是激光雷达数据插件

TF是坐标变换插件

RobotModel是机器人模型插件

ros2 run rviz2 rviz2 -d xxx.rviz
#可以读取自己保存的rviz配置

rviz2集成URDF基本流程
1. 案例分析

请调用如下命令，安装案例所需的两个功能包(可以控制机器人关节运动)：

sudo apt install ros-humble-joint-state-publisher
sudo apt install ros-humble-joint-state-publisher-gui

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

ros2 pkg create cpp06_urdf --build-type ament_cmake

功能包下新建 urdf、rviz、launch、meshes目录以备用，其中 urdf 目录下再新建子目录 urdf 与 xacro，分别用于存储 urdf 文件和 xacro 文件。

launch存放launch文件

urdf文件里面存放urdf三维模型文件

meshes存放stl模型

xacro可以简化urdf文件，并且增强其灵活性

rviz存放rviz2的配置

框架搭建

<!-- 
    需求：显示一盒状机器人
 -->
 <robot> name="hello_world"
   <link> name="base_link"
     <visual>
       <geometry>
         <box size="0.5 0.2 0.1"/>
       </geometry>
     </visual>
   </link>
 </robot>

标准的XML文件

from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command,LaunchConfiguration
from launch.actions import DeclareLaunchArgument

#示例：ros2 launch cpp06_urdf display.launch.py model:=`ros2 pkg prefix --share cpp06_urdf`/urdf/urdf/demo01_helloworld.urdf
def generate_launch_description():

    cpp06_urdf_dir = get_package_share_directory("cpp06_urdf")
    default_model_path = os.path.join(cpp06_urdf_dir,"urdf/urdf","demo01_helloworld.urdf")
    default_rviz_path = os.path.join(cpp06_urdf_dir,"rviz","display.rviz")
    model = DeclareLaunchArgument(name="model", default_value=default_model_path)

    # 加载机器人模型
    # 1.启动 robot_state_publisher 节点并以参数方式加载 urdf 文件
    robot_description = ParameterValue(Command(["xacro ",LaunchConfiguration("model")]))
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"robot_description": robot_description}]
    )
    # 2.启动 joint_state_publisher 节点发布非固定关节状态
    joint_state_publisher = Node(
        package="joint_state_publisher",
        executable="joint_state_publisher"
    )
    # rviz2 节点
    rviz2 = Node(
        package="rviz2",
        executable="rviz2"
        # arguments=["-d", default_rviz_path]
    )
    return LaunchDescription([
        model,
        robot_state_publisher,
        joint_state_publisher,
        rviz2
    ])

<exec_depend>rviz2</exec_depend>
<exec_depend>xacro</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>ros2launch</exec_depend>

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

colcon build --packages-select cpp06_urdf

source install/setup.bash
ros2 launch cpp06_urdf display.launch.py

小提示：

在本章的后续案例中，所有实现都遵循上述步骤，在后续案例中我们只需要关注 urdf 实现即可，launch 文件和配置文件无需修改。

urdf文件

按ctrl+\生成注释

因为安装过urdf插件，所以有提示，需要创建robot根标签

第一个属性为机器人名字

第二个有个xml namespace，指向xacro

第三个有个xml namespace，指向xacro，然后还有一个机器人名字

(暂时用最简单的，也就是第一个)

link标签叫连杆，也需要起个名字，连杆一般指刚体部分

link有个子集标签，叫visual

visual标签下要写机器人形状

然后该标签下又有一个子集标签叫geometry(几何形状)

然后又有子集标签叫box(矩形体状)后面的size后面对应长宽高

<!-- 
    需求：显示一盒状机器人
    长:1.0m
    宽:0.5m
    高:0.1m
 -->
 <!-- 根标签 -->
 <robot name="boxrobot"> 
  <!-- 连杆 -->
    <link name="base_link"> 
      <!-- 可视化 -->
      <visual>
        <!-- 几何图形 -->
        <geometry>
          <!-- 长方体 -->
          <box size="1.0 0.5 0.1"/>
        </geometry>
      </visual>
    </link>
  </robot>

xacro工具(将磁盘文件加载到ROS2中的工具)

搜索是否安装过xacro

ros2 pkg list | grep -i xacro

如果打印了xacro说明安装了，如果没打印，则要手动安装

sudo apt-get update
sudo apt-get install ros-humble-xacro

使用xacro读取文件

文件里的内容被输出到了终端，咱们一般集成到launch文件中。咱们在终端里是只能查看内容，但是用launch就可以把文件弄到节点里，也就是集成到ROS2里。

launch核心实现

核心实现就三步，加载机器人模型，节点发布非固定关节的状态，启动rviz2节点

创建rviz2节点很简单，就声明下包名，声明下executable。

加载机器人模型比较复杂，加载机器人模型，也要创建一个节点，

然后有参数，参数里有个键叫robot_description，然后这个键对应一个值

值是ParameterValue对象，这个对象里执行了一个指令，叫xacro，然后后面又有一个Launch配置，其实就是urdf文件的路径。

这个值，其实就是URDF文件里的内容，但是内容太长了，所以我们把它封装成一个对象。

命令行，不能直接当对象参数值，所以还要封装

Comand是专门封装终端指令执行的

记得xacro后面要有空格，这里填路径

此时已经定位到cpp06_urdf的share路径下的cpp06_urdf路径了，返回的也就是该路径的字符串

from launch import LaunchDescription
from launch_ros.actions import Node

# 封装终端指令相关类
# from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
# from launch.actions import DeclareLaunchArgument
# from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command

p_value = ParameterValue(Command(["xacro ",get_package_share_directory("cpp06_urdf") + "/urdf/urdf/demo01_boxrobot.urdf"]))
robot_state_pub = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    parameters=[{"robot_description":p_value}]
)

rviz2 = Node(
    package="rviz2",
    executable="rviz2"
    )

def generate_launch_description():
    return LaunchDescription([robot_state_pub,rviz2])

点击左下角Add添加RobotModel插件

新建一个坐标系插件，长沿着X，宽沿着Y，高沿着Z

launch优化说明与实现

我们还需要优化三个点，第一个是打开关节节点，第二是设置rviz2默认配置文件，第三是Launch文件中我们将读取的urdf文件写死了，所以要优化结构。

还要启动这个节点，来控制关节运动，可以改成joint_state_publisher_gui，出现图形化界面。

保存一下rviz2的配置

正常的指令是

ros2 run rviz2 rviz2 -d rviz2配置的路径

创建一个参数叫model，值是后面那一长串。

LaunchConfiguration是解析参数

记得要把model放在最前面，放在后面是不可以的，现在已经把路径封装完毕了。

现在启动是正常启动默认的urdf路径。

解析非默认值的urdf，在终端里也有类似于get_package_share_directory，以下就是(这里参数model少写了个L)要把参数值用反引号(ESC与TAB中间的按键)框起来。

ros2 pkg prefix --share cpp06_urdf

ros2 run launch cpp06_urdf display.launch.py model:=`ros2 pkg prefix --share cpp06_urdf`/urdf/urdf/hahah.urdf

from launch import LaunchDescription
from launch_ros.actions import Node

# 封装终端指令相关类
# from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command

model = DeclareLaunchArgument(name="model",default_value=get_package_share_directory("cpp06_urdf") + "/urdf/urdf/demo01_boxrobot.urdf")

p_value = ParameterValue(Command(["xacro ",LaunchConfiguration("model")]))
robot_state_pub = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    parameters=[{"robot_description":p_value}]
)


joint_state_pub = Node(
    package="joint_state_publisher",
    executable="joint_state_publisher"
    )


rviz2 = Node(
    package="rviz2",
    executable="rviz2",
    arguments=["-d",get_package_share_directory("cpp06_urdf") + "/rviz/urdf.rviz"]
    )

def generate_launch_description():
    return LaunchDescription([model,robot_state_pub,joint_state_pub,rviz2])

继续优化最终的代码为：

from launch import LaunchDescription
from launch_ros.actions import Node

# 封装终端指令相关类
# from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command

import os

cpp06_urdf_dir = get_package_share_directory("cpp06_urdf")

default_model_path = os.path.join(cpp06_urdf_dir,"urdf/urdf","demo01_boxrobot.urdf")
default_rviz_path = os.path.join(cpp06_urdf_dir,"rviz","urdf.rviz")

model = DeclareLaunchArgument(name="model",default_value=default_model_path)

p_value = ParameterValue(Command(["xacro ",LaunchConfiguration("model")]))
robot_state_pub = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    parameters=[{"robot_description":p_value}]
)

# 关节信息节点
# joint_state_pub = Node(
#     package="joint_state_publisher",
#     executable="joint_state_publisher"
# )

# 关节信息节点图形界面(建议)
joint_state_pub = Node(
    package="joint_state_publisher_gui",
    executable="joint_state_publisher_gui"
)

rviz2 = Node(
    package="rviz2",
    executable="rviz2",
#    arguments=["-d",get_package_share_directory("cpp06_urdf") + "/rviz/urdf.rviz"]
    arguments=["-d",default_rviz_path]

    )

def generate_launch_description():
    return LaunchDescription([model,robot_state_pub,joint_state_pub,rviz2])

URDF语法
简介

robot根标签

对机器人进行分割，分割成几个子集，比如一个子集描述头，一个子集描述身子，最后再合成合集，成机器人

虽然我的子文件和主文件在逻辑上是有包含关系的，但是其实，他们都是单独的urdf文件。主文件中，robot标签的name属性必须写，子文件可以不写，如果写，那么子文件name的值与主文件的必须相同！

link标签
简介

每一个link都是刚体，都是独立部件

Link是通过joint进行拼接的

Link主要包含三部分，Visual，Collision和Inertial

没加尖括号的是属性，加了的是标签

**** （可选）：用于描述link的可视化属性，可以设置link的形状（立方体、球体、圆柱等）。

name （可选）：指定link名称，此名称会映射为同名坐标系，还可以通过引用该值定位定位link。
**** （必填）：用于设置link的形状，比如：立方体、球体或圆柱。
- **** ：立方体标签，通过size属性设置立方体的边长，原点为其几何中心。
- **** ：圆柱标签，通过radius属性设置圆柱半径，通过length属性设置圆柱高度，原点为其几何中心。
- **** ：球体标签，通过radius属性设置球体半径，原点为其几何中心。
- **** ：通过属性filename引用“皮肤”文件，为link设置外观，该文件必须是本地文件。使用 package:///为文件名添加前缀。
- **** （可选）：用于设置link的相对偏移量以及旋转角度，如未指定则使用默认值（无偏移且无旋转）。
  - xyz ：表示x、y、z三个维度上的偏移量（以米为单位），不同数值之间使用空格分隔，如未指定则使用默认值（三个维度无偏移）。
  - rpy ：表示翻滚、俯仰与偏航的角度（以弧度为单位），不同数值之间使用空格分隔，如未指定则使用默认值（三个维度无旋转）。
- **** （可选）：视觉元素的材质。也可以在根标签robot中定义material标签，然后，可以在link中按名称进行引用。
  - name （可选）：为material指定名称，可以通过该值进行引用。
  - **** （可选）：rgba 材质的颜色，由代表red/green/blue/alpha 的四个数字组成，每个数字的范围为 \[0,1\]。
  - **** （可选）：材质的纹理，可以由属性filename设置。

当有多个Visual的时候，需要给Visual设置name，所以是个可选项。

Collision与仿真有关系，我们可以给我们的机器人的刚体设置一个碰撞区间，只要障碍物进入了区间，那么就发生了碰撞，一般碰撞区间要比实际大小要大。

**** （可选）：link的碰撞属性。可以与link的视觉属性一致，也可以不同，比如：我们会通常使用更简单的碰撞模型来减少计算时间，或者设置的值大于link的视觉属性，以尽量避免碰撞。另外，同一链接可以存在多个标签实例，多个几何图形组合表示link的碰撞属性。

name （可选）：为collision设置名称。
**** （必须）：请参考visual标签的geometry使用规则。
**** （可选）：请参考visual标签的origin使用规则。

Inertial是设置惯性矩阵的，也是和仿真有关系的。比如说机器人刹车，会出现前倾的情况，比如说惯性矩阵的重心高一些，那么急刹车就会出现翻车的情况了。

**** （可选）：用于设置link的质量、质心位置和中心惯性特性，如果未指定，则默认为质量为0、惯性为0。

**** （可选）：该位姿（平移、旋转）描述了链接的质心框架 C 相对于链接框架 L 的位置和方向。
- xyz ：表示从 Lo（链接框架原点）到 Co（链接的质心）的位置向量为 x L̂x + y L̂y + z L̂z，其中 L̂x、L̂y、L̂z 是链接框架 L 的正交单位向量。
- rpy ：将 C 的单位向量 Ĉx、Ĉy、Ĉz 相对于链接框架 L 的方向表示为以弧度为单位的欧拉旋转序列 (r p y)。注意：Ĉx、Ĉy、Ĉz 不需要与连杆的惯性主轴对齐。
**** （必填）：通过其value属性设置link的质量。
**** （必填）：对于固定在质心坐标系 C 中的单位向量 Ĉx、Ĉy、Ĉz，该连杆的惯性矩 ixx、iyy、izz 以及关于 Co（连杆的质心）的惯性 ixy、ixz、iyz 的乘积。

注意： 和在仿真环境下才需要使用到，如果只是在 rviz2 中集成 urdf，那么不必须为 link 定义这两个标签。

使用

<robot name="link_demo">
  <!-- 定义颜色 -->
  <material name="yellow">
    <color rgba="0.7 0.7 0 0.8" />
  </material>
  <link name="base_link">
    <visual>
        <!-- 形状 -->
        <geometry>
            <!-- 长方体的长宽高 -->
            <box size="0.5 0.3 0.1" />
            <!-- 圆柱，半径和长度 -->
            <!-- <cylinder radius="0.5" length="1.0" /> -->
            <!-- 球体，半径-->
            <!-- <sphere radius="0.3" /> -->

        </geometry>
        <!-- xyz坐标 rpy翻滚俯仰与偏航角度(3.14=180度) -->
        <origin xyz="0 0 0" rpy="0 0 0" />
        <!-- 调用已定义的颜色 -->
        <material name="yellow"/>
    </visual>
  </link>
</robot>

ros2 launch cpp06_urdf display.launch.py model:=`ros2 pkg prefix --share cpp06_urdf`/urdf/urdf/demo02_link.urdf

矩形体，球形，圆柱体

平移量，X平移1，Y和Z平移0，rpy旋转度设置为0(旋转度分别是欧拉角里的翻滚角Roll(绕X)，俯仰角Pitch(绕Y)，航向角Yaw(绕Z))

发现长方体在X上偏移了1

让航向角Yaw(绕Z运动)为0.5 rad

设置翻滚角Roll(绕X运动)为0.5rad

设置俯仰角Pitch(绕Y运动)为0.5rad

sRGB是R,G,B,Alpha(浮点模型，所以范围是0-1.0)

如果一个颜色要被用好几次，可以封装成一个类似于全局变量的东西，然后在其他link中调用时，直接用（这里是个闭环，注意）

<robot name="link_demo">
    <!-- 定义颜色 -->
    <material name="yellow">
      <color rgba="0.7 0.7 0 0.8" />
    </material>
    <link name="base_link">
      <visual>
          <!-- 形状 -->
          <geometry>
              <!-- 长方体的长宽高 -->
              <box size="0.5 0.3 0.1" />
              <!-- 圆柱，半径和长度 -->
              <!-- <cylinder radius="0.5" length="1.0" /> -->
              <!-- 球体，半径-->
              <!-- <sphere radius="0.3" /> -->
  
          </geometry>
          <!-- xyz坐标 rpy翻滚俯仰与偏航角度(3.14=180度) -->
          <origin xyz="0 0 0" rpy="0 0 0" />
          <!-- 调用已定义的颜色 -->
          <material name="yellow"/>
      </visual>
    </link>
  </robot>

使用补充

mesh标签是引用皮肤文件，一般是stl文件，可以用SolidWorks导出，可以看文档后面的SW2URDF

如果不会使用solidworks，可以学，这东西2天半就能学会，只是一个工具，只会画图没有啥水平，最重要的还是机械设计比较难。想学的可以看兄弟社团机械学会微信公众号的视频进行学习。

https://mp.weixin.qq.com/mp/homepage?__biz=MzI4MjkyMDgyMA==&hid=7&sn=1efc3d3cee0142970227785f767cc7c8&scene=18

当然，没时间学习可以直接用别人画好的机器人，在Github上搜索turtlebot3

这里有已经导出的模型，bases里是外观的模型，sensors是传感器的模型，wheels是轮子的模型。

我们克隆下仓库，注意要克隆branch是ros2的分支。

我们引用一个就行了，看看效果即可，真正的应用还是要用SolidWorks通过SW2URDF插件进行导出

把项目里的meshes/bases里的burger_base.stl拷贝到我们WS里的meshes目录

filename是写刚才的皮肤文件，package://就是协议名，后面跟包名，也就是cpp06_urdf。然后跟功能包下的文件路径，也就是meshes/burger_base.stl（其实也就是share下的路径）

因为是个三维模型，所以在填scale大小缩放时，需要填3个比例，咱们都填1.0即可。

为什么显示的模型会这么大呢，因为rviz2以米为单位，而stl是以mm为单位，注意。在机械上，默认不说单位就都是mm，不要乱改单位，一般都要以mm为单位，咱们是做机器人的，要专业一些。

joint标签
简介

urdf 中的 joint 标签用于描述机器人关节的运动学和动力学属性，还可以指定关节运动的安全极限，机器人的两个部件(分别称之为 parent link 与 child link)以”关节“的形式相连接，不同的关节有不同的运动形式: 旋转、滑动、固定、旋转速度、旋转角度限制….,比如:安装在底座上的轮子可以360度旋转，而摄像头则可能是完全固定在底座上。

name （必填）：为关节命名，名称需要唯一。
type （必填）：设置关节类型，可用类型如下：
- continuous：旋转关节，可以绕单轴无限旋转。
- revolute：旋转关节，类似于 continues，但是有旋转角度限制。
- prismatic：滑动关节，沿某一轴线移动的关节，有位置极限。
- planer：平面关节，允许在平面正交方向上平移或旋转。
- floating：浮动关节，允许进行平移、旋转运动。
- fixed：固定关节，不允许运动的特殊关节。

以下是子级标签

**** （必填）：指定父级link。
- link （必填）：父级link的名字，是这个link在机器人结构树中的名字。
**** （必填）：指定子级link。
- link （必填）：子级link的名字，是这个link在机器人结构树中的名字。
**** （可选）：这是从父link到子link的转换，关节位于子link的原点。
- xyz ：各轴线上的偏移量。
- rpy ：各轴线上的偏移弧度。
<axis> （可选）：如不设置，默认值为（1，0，0）。
- xyz ：用于设置围绕哪个关节轴运动。
**** （可选）：关节的参考位置，用于校准关节的绝对位置。
- rising （可选）：当关节向正方向移动时，该参考位置将触发上升沿。
- falling （可选）：当关节向正方向移动时，该参考位置将触发下降沿。
**** （可选）：指定接头物理特性的元素。这些值用于指定关节的建模属性，对仿真较为有用。
- damping （可选）：关节的物理阻尼值，默认为0。
- friction （可选）：关节的物理静摩擦值，默认为0。
**** （关节类型是revolute或prismatic时为必须的）：
- lower （可选）：指定关节下限的属性（旋转关节以弧度为单位，棱柱关节以米为单位）。如果关节是连续的，则省略。
- upper （可选）：指定关节上限的属性（旋转关节以弧度为单位，棱柱关节以米为单位）。如果关节是连续的，则省略。
- effort （必填）：指定关节可受力的最大值。
- velocity （必填）：用于设置最大关节速度（旋转关节以弧度每秒 [rad/s] 为单位，棱柱关节以米每秒 [m/s] 为单位）。
**** （可选）：此标签用于指定定义的关节模仿另一个现有关节。该关节的值可以计算为*value = multiplier \* other\_joint\_value + offset*。
- joint （必填）：指定要模拟的关节的名称。
- multiplier （可选）：指定上述公式中的乘法因子。
- offset （可选）：指定要在上述公式中添加的偏移量，默认为 0（旋转关节的单位是弧度，棱柱关节的单位是米）。
<safety_controller> （可选）：安全控制器。
- soft_lower_limit （可选）：指定安全控制器开始限制关节位置的下关节边界，此限制需要大于joint下限。
- soft_upper_limit （可选）：指定安全控制器开始限制关节位置的关节上边界的属性，此限制需要小于joint上限。
- k_position （可选）：指定位置和速度限制之间的关系。
- k_velocity （必填）：指定力和速度限制之间的关系。

关节名称是必填的，且是唯一的。

咱们最常用的是有限位的revolute类型的关节，continuous可无限旋转的关节，fixed固定关节，这个根据具体的关节类型来填。

revolute一般用于工业机器人机械臂的关节，continuous比如舵轮结构的“关节”，fixed就是一些固定的不能运动的结构关节。

子集标签很多，咱们用的最常用的就几个，记住常用的即可。

parent标签，link属性指定父级link的名字。

child标签类似。

这个轴，默认是1，0，0，以X轴进行旋转，但是咱们一般是需要设置的，可通过SolidWorks设置基准轴进行设置。

剩下的标签，都与关节类型有关，比如limit，如果关节类型是revolute，而且不设置limit，那么在joint_state_publisher_gui里是无法调关节的角度的。

其他标签用到的时候再进行介绍。

练习

先把俩关节，单独实现，然后再通过joint关节进行连接。

黄色的话，红和绿要多一些。

这是红色。

底盘Link就创建完毕了，

摄像头Link也创建完毕了，

关节名字设置为camera2base_link，也就是摄像头连接底座的joint，然后类型是360度都可以转的continuous。

填入子级Link与父级Link

这样两个Link就通过该Joint连接到一起了。

还需要设置这俩选项，咱们先不设置，先看默认是什么状态。

colcon build --packages-select cpp06_urdf
source install/setup.bash
ros2 launch cpp06_urdf display.launch.py model:=`ros2 pkg prefix --share cpp06_urdf`/urdf/urdf/demo03_joint.urdf

可以打开TF看看坐标系，勾上Show Names，发现是重合的。所以显示效果不满足咱们的逻辑业务。（默认状态下），所以需要设置偏移量。

如果咱们想把摄像头移动到车头，如图所示，在X上有偏移量，Y没有，但是Z有。然后roll，pitch，yaw上没有。

Z的高度就是1/2的底盘高度+1/2的摄像头高度

咱们要绕Yaw旋转，所以也就是绕Z轴旋转，也就是001，注意一定得是整形，不能是浮点型。

想要看是否能旋转，要打开joint_state_publisher_gui，可以从launch中打开，也可以从终端中直接打开。

ros2 run joint_state_publisher_gui joint_state_publisher_gui

拖拽滚动条可改变摄像头的Yaw

Randomize是随机数，Center是回中(复位)

<!-- 
    需求：创建机器人模型，底盘为长方体，
         在长方体的前面添加一摄像头，
         摄像头可以沿着 Z 轴 360 度旋转

 -->
 <robot name="joint_demo">
  <!-- 定义颜色 -->
  <material name="yellow">
    <color rgba="0.7 0.7 0 0.8" />
  </material>
  <material name="red">
    <color rgba="0.8 0.1 0.1 0.8" />
  </material>
  <link name="base_link">
    <visual>
        <!-- 形状 -->
        <geometry>
            <box size="0.5 0.3 0.1" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="yellow"/>
    </visual>
  </link>

  <!-- 摄像头 -->
  <link name="camera">
      <visual>
          <geometry>
              <box size="0.02 0.05 0.05" />
          </geometry>
          <origin xyz="0 0 0" rpy="0 0 0" />
          <material name="red" />
      </visual>
  </link>

  <!-- 关节 -->
  <joint name="camera2baselink" type="continuous">
      <parent link="base_link"/>
      <child link="camera" />
      <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
      <origin xyz="0.2 0 0.075" rpy="0 0 0" />
      <axis xyz="0 0 1" />
  </joint>

</robot>

joint_state_publisher

bug:Yaw不稳定，会一直回中。

其实是因为launch里启动的和终端里启动的冲突了。

解决方案：只启动其中一个。

解决方案：直接在Launch里启动GUI版本的，这样即可解决。(但是不建议)

建议方案：用非GUI版本的，因为以后，我们控制关节是用程序控制，而不是GUI控制。

如果只是想展示模型，用GUI，

如果想用程序控制，用普通版。

base_footprint

bug:机器人底盘半沉入地下

  <link name="base_footprint">
    <visual>
      <geometry>
          <sphere radius="0.001"/>
      </geometry>
    </visual>
  </link>

    <joint name="baselink2basefootprint" type="fixed">
      <parent link="base_footprint"/>
      <child link="base_link"/>
      <origin xyz="0.0 0.0 0.05"/>
    </joint>

Z的偏移量要填下沉底盘的距离，也就是整车底盘的一半。

修改参考坐标系

其实这个优化，可以不做，影响不大。但是建议用有basefootprint版本的。

<!-- 
    需求：为机器人模型添加 base_footprint

 -->
 <robot name="base_footprint_demo">
  <!-- 定义颜色 -->
  <material name="yellow">
    <color rgba="0.7 0.7 0 0.8" />
  </material>
  <material name="red">
    <color rgba="0.8 0.1 0.1 0.8" />
  </material>

  <link name="base_footprint">
    <visual>
      <geometry>
          <sphere radius="0.001"/>
      </geometry>
    </visual>
  </link>

  <link name="base_link">
    <visual>
        <!-- 形状 -->
        <geometry>
            <box size="0.5 0.3 0.1" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="yellow"/>
    </visual>
  </link>

  <joint name="baselink2basefootprint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0.0 0.0 0.05"/>
  </joint>

  <!-- 摄像头 -->
  <link name="camera">
      <visual>
          <geometry>
              <box size="0.02 0.05 0.05" />
          </geometry>
          <origin xyz="0 0 0" rpy="0 0 0" />
          <material name="red" />
      </visual>
  </link>

  <!-- 关节 -->
  <joint name="camera2baselink" type="fixed">
      <parent link="base_link"/>
      <child link="camera" />
      <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
      <origin xyz="0.2 0 0.075" rpy="0 0 0" />
      <axis xyz="0 0 1" />
  </joint>

</robot>

练习

没必要太深入练习，咱们可以直接用SolidWorks建模，更为友好。

<!-- 
    练习：编写四轮差速机器人的底盘模型

    参数：
        长 0.2m
        宽 0.12m
        高 0.07m
        离地 0.015m
        车轮半径：0.025m
        车轮厚度：0.02m

 -->
 <robot name="exercise_demo">
  <!-- 定义颜色 -->
  <material name="yellow">
    <color rgba="0.7 0.7 0 0.8" />
  </material>
  <material name="red">
    <color rgba="0.8 0.1 0.1 0.8" />
  </material>
  <material name="gray">
    <color rgba="0.2 0.2 0.2 0.8" />
  </material>

  <link name="base_footprint">
    <visual>
      <geometry>
          <sphere radius="0.001"/>
      </geometry>
    </visual>
  </link>
  <!-- 车体 -->
  <link name="base_link">
    <visual>
        <!-- 形状 -->
        <geometry>
            <box size="0.2 0.12 0.07" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="yellow"/>
    </visual>
  </link>

  <joint name="baselink2basefootprint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0.0 0.0 0.05"/>
  </joint>

  <!-- ==============车轮================= -->
  <!-- 左前轮 -->
  <link name="front_left_wheel">
      <visual>
          <geometry>
              <cylinder radius="0.025" length="0.02"/>
          </geometry>
          <origin xyz="0 0 0" rpy="1.57 0 0" />
          <material name="gray" />
      </visual>
  </link>

  <!-- 左前轮关节 -->
  <joint name="frontleftwheel2baselink" type="continuous">
      <parent link="base_link"/>
      <child link="front_left_wheel" />
      <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
      <origin xyz="0.075 0.06 -0.025" rpy="0 0 0" />
      <axis xyz="0 1 0" />
  </joint>

  <!-- 右前轮 -->
  <link name="front_right_wheel">
    <visual>
        <geometry>
            <cylinder radius="0.025" length="0.02"/>
        </geometry>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <material name="gray" />
    </visual>
  </link>

  <!-- 右前轮关节 -->
  <joint name="frontrightwheel2baselink" type="continuous">
      <parent link="base_link"/>
      <child link="front_right_wheel" />
      <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
      <origin xyz="0.075 -0.06 -0.025" rpy="0 0 0" />
      <axis xyz="0 1 0" />
  </joint>

  <!-- 左后轮 -->
  <link name="back_left_wheel">
    <visual>
        <geometry>
            <cylinder radius="0.025" length="0.02"/>
        </geometry>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <material name="gray" />
    </visual>
  </link>

  <!-- 左后轮关节 -->
  <joint name="backleftwheel2baselink" type="continuous">
    <parent link="base_link"/>
    <child link="back_left_wheel" />
    <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
    <origin xyz="-0.075 0.06 -0.025" rpy="0 0 0" />
    <axis xyz="0 1 0" />
  </joint>

  <!-- 右后轮 -->
  <link name="back_right_wheel">
    <visual>
        <geometry>
            <cylinder radius="0.025" length="0.02"/>
        </geometry>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <material name="gray" />
    </visual>
  </link>

  <!-- 右后轮关节 -->
  <joint name="backrightwheel2baselink" type="continuous">
    <parent link="base_link"/>
    <child link="back_right_wheel" />
    <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
    <origin xyz="-0.075 -0.06 -0.025" rpy="0 0 0" />
    <axis xyz="0 1 0" />
  </joint>


</robot>

终端运行

ros2 launch cpp06_urdf display.launch.py model:=`ros2 pkg prefix --share cpp06_urdf`/urdf/urdf/demo05_exercise.urdf

URDF工具

在 ROS2 中，提供了一些URDF文件相关的工具，比如:

check_urdf命令可以检查复杂的 urdf 文件是否存在语法问题；
urdf_to_graphviz命令可以查看 urdf 模型结构，显示不同 link 的层级关系。

当然，要使用工具之前，请先安装，安装命令：sudo apt install liburdfdom-tools。

check_urdf 语法检查

进入urdf文件所属目录，调用：check_urdf urdf文件，如果不抛出异常，说明文件合法，否则非法。

示例，终端下进入功能包 cpp06_urdf 的 urdf/urdf 目录，执行如下命令：

check_urdf demo05_exercise.urdf

urdf 文件如无异常，将显示urdf中link的层级关系，如下图所示：

否则将会给出错误提示。

演示错误，

urdf_to_graphviz 结构查看

进入urdf文件所属目录，调用:urdf_to_graphviz urdf文件，当前目录下会生成 pdf 文件。

示例，终端下进入功能包 cpp06_urdf 的 urdf/urdf 目录，执行如下命令：

urdf_to_graphviz demo05_exercise.urdf

当前目录下，将生成以urdf中robot名称命名的.pdf和.gv文件，打开pdf文件会显示如下图内容：

在上图中会以树形结构显示link与joint的关系。

注意： 该工具以前名为urdf_to_graphiz现建议使用urdf_to_graphviz替代。

urdf_to_graphiz是历史版本，已经被废弃，建议用urdf_to_graphviz。

黑色方框代表Link，蓝色代表Joint，也会展示平移量和旋转度等信息。

SW2URDF
solidworks简介

SolidWorks是一种计算机辅助设计（CAD）和计算机辅助制造（CAM）软件，由Dassault Systèmes SolidWorks Corp.开发。它主要用于工程设计和制造，可用于创建3D三维模型、进行装配设计、进行工程分析和绘图等。SolidWorks具有直观的用户界面和强大的功能，使工程师和设计师能够快速而精确地设计复杂的零部件和装配体。该软件广泛应用于机械、航空航天、汽车、医疗设备等行业，是工程设计领域的重要工具之一。

solidworks插件sw2urdf介绍

sw2urdf插件维基百科：

https://wiki.ros.org/sw_urdf_exporter

github下载链接：

https://github.com/ros/solidworks_urdf_exporter/releases

虽然github链接上写着只支持到SW2021，但是目前发现最新版SW(2022、2024经测试)也是可以正常用的。

安装sw2urdf
去github上下载sw2urdf插件（这个SW版本不用管，实测在后续的SW版本依然可用）

查看SW的安装路径

比如我的路径”C:\Program Files\SOLIDWORKS Corp\SOLIDWORKS\”

打开插件安装器，默认情况下会自动找到你的SW路径进行安装，如果没有自动找到路径安装，那么需要手动选取SW安装路径。

查看插件是否安装成功

如图这样就是安装成功了。

导出URDF与Meshes
如图是一个标准的SW装配体图(大家也可以尝试自己手撸一个，里面没有传动装置也可以)

这个过程看不太懂的话，可以参考一下古月居老师的视频，结合本教程一起学习。【SolidWorks模型导出urdf（古月居老师）-哔哩哔哩】https://www.bilibili.com/video/BV1Tx411o7rH

对joint关节建立基准轴

选择关节的圆柱面或者其他面，对转轴进行标定

如果建完基准轴，发现不能够正常展示，那么就打开该选项。

建完所有系后

打开export as URDF选项

需要先选择base_link基底刚体

这里的Global Origin可能需要自己选择，就选择Origin_Global即可，但是一般选择automatically Generate即可。

因为他的base_link通过一个joint连接了一个child_link，所以，这个选项要填1。

咱们把base_link连接的关节叫joint1，把joint1连接的更上面的刚体叫做link1,

然后参考基准轴选择刚才咱们在这个关节处建立的基准轴1，然后joint type关节类型选择revolute（有限位的关节，只有这样，机器人关节才可以正常运动）

这里的参考坐标系系统可以选择automatically Generate，也可以选择Origin_Joint1。

然后再在link1上加一，创建link2。

后面的link3，link4添加步骤是一样的，不再详细展示。

点击preview and export

对关节进行限位设置

coordinates是坐标系，axis是轴，如果这里Axis坐标显示错误，那么说明Coordinates或者Axis选择错了，请手动选择正确的坐标系或者基准轴。

如果转轴生成错误，比如都是000，可以自己填一下。

比如这里是X轴，可以把左轮全填-100,右轮100（这里，仅供参考，chatgpt提供的解决方案）

查看矩阵，并点导出urdf与meshes

这样就成功生成ROS1的WS了，将WS移动至Linux系统。

将ROS1的WS转化为ROS2的WS
先将ROS1的WS移动到Linux系统上

新建一个ROS2的WS

mkdir -p ros2_4axisrobot_ws/src
cd ros2_4axisrobot_ws/
colcon build
cd src
ros2 pkg create cpp01_urdf --build-type ament_cmake
cd ..
code .

在./src/cpp01_urdf路径下创建launch、meshes、rviz、urdf等等文件夹，在urdf文件夹下再新建urdf和xacro文件夹

复制ROS1的WS里的URDF和Meshes到ROS2的WS中

对CMake和package进行配置

在CMake中请添加

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

在Package.xml中请添加

  <exec_depend>rviz2</exec_depend>
  <exec_depend>xacro</exec_depend>
  <exec_depend>robot_state_publisher</exec_depend>
  <exec_depend>joint_state_publisher</exec_depend>
  <exec_depend>ros2launch</exec_depend>

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(cpp01_urdf)

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)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

install(
  DIRECTORY launch urdf rviz meshes
  DESTINATION share/${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()

package.xml

<?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>cpp01_urdf</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="tungchiahui@gmail.com">tungchiahui</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <exec_depend>rviz2</exec_depend>
  <exec_depend>xacro</exec_depend>
  <exec_depend>robot_state_publisher</exec_depend>
  <exec_depend>joint_state_publisher</exec_depend>
  <exec_depend>ros2launch</exec_depend>

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

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

对launch文件进行编写

详细过程请看URDF有关Launch的核心优化那一节

from launch import LaunchDescription
from launch_ros.actions import Node

# 封装终端指令相关类
# from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command

import os

cpp01_urdf_dir = get_package_share_directory("cpp01_urdf")

default_model_path = os.path.join(cpp01_urdf_dir,"urdf/urdf","4axisrobot.urdf")
default_rviz_path = os.path.join(cpp01_urdf_dir,"rviz","urdf.rviz")

model = DeclareLaunchArgument(name="model",default_value=default_model_path)

p_value = ParameterValue(Command(["xacro ",LaunchConfiguration("model")]))
robot_state_pub = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    parameters=[{"robot_description":p_value}]
)

# 关节信息节点(建议)
# joint_state_pub = Node(
#     package="joint_state_publisher",
#     executable="joint_state_publisher"
# )

# 关节信息节点图形界面
joint_state_pub = Node(
    package="joint_state_publisher_gui",
    executable="joint_state_publisher_gui"
)

rviz2 = Node(
    package="rviz2",
    executable="rviz2",
#    arguments=["-d",get_package_share_directory("cpp06_urdf") + "/rviz/urdf.rviz"]
    arguments=["-d",default_rviz_path]

    )

def generate_launch_description():
    return LaunchDescription([model,robot_state_pub,joint_state_pub,rviz2])

对URDF文件进行修改

修改的内容主要有两项，一个是meshes的路径，一个是删掉易引起报错的注释。

主要由于ROS1的WS和ROS2的WS路径不同，所以，我们只需要修改有关路径的内容，比如说Meshes的路径

package://一般是share目录，所以我们需要从cpp01_urdf这一级开始写（share目录需要编译后才会显示）

然后按Ctrl+H

然后还需要删除掉urdf文件刚开始的注释，否则也会报错

如下是替换完毕的URDF文件：

<?xml version="1.0" encoding="utf-8"?>
<robot
  name="4axisrobot">
  <link
    name="base_link">
    <inertial>
      <origin
        xyz="-0.0819771145953119 -0.0394328123681598 -0.0605612328464761"
        rpy="0 0 0" />
      <mass
        value="7.43807257011133" />
      <inertia
        ixx="0.33173827700719"
        ixy="-1.91350798633049E-05"
        ixz="-2.38586363043925E-05"
        iyy="0.510937907009661"
        iyz="1.14706407748075E-05"
        izz="0.66107272632796" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/base_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>
  <link
    name="link1">
    <inertial>
      <origin
        xyz="0.19141 -0.050676 -0.030057"
        rpy="0 0 0" />
      <mass
        value="21.822" />
      <inertia
        ixx="0.50464"
        ixy="0.13295"
        ixz="0.087353"
        iyy="0.59136"
        iyz="-0.066053"
        izz="0.64081" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link1.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.79216 0.81961 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link1.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint1"
    type="revolute">
    <origin
      xyz="-0.081947 -0.039447 0.11191"
      rpy="0.40394 -1.5708 0" />
    <parent
      link="base_link" />
    <child
      link="link1" />
    <axis
      xyz="1 0 0" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="100"
      velocity="1" />
  </joint>
  <link
    name="link2">
    <inertial>
      <origin
        xyz="0.00711598521454873 0.430460941882332 -0.181074256126024"
        rpy="0 0 0" />
      <mass
        value="30.6115349647897" />
      <inertia
        ixx="2.84056101478768"
        ixy="0.00176700425270318"
        ixz="-0.000605507690039326"
        iyy="0.803420593303961"
        iyz="0.881921487314161"
        izz="2.25142021571439" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link2.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link2.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint2"
    type="revolute">
    <origin
      xyz="0.35141 -0.28119 0.13918"
      rpy="-2.0566 1.1982 1.5708" />
    <parent
      link="link1" />
    <child
      link="link2" />
    <axis
      xyz="-1 0 0" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="100"
      velocity="1" />
  </joint>
  <link
    name="link3">
    <inertial>
      <origin
        xyz="0.00112887115421612 0.0366346915274407 0.31720330934553"
        rpy="0 0 0" />
      <mass
        value="32.192726667578" />
      <inertia
        ixx="4.01515983126431"
        ixy="0.0013193752174692"
        ixz="0.0113529938102155"
        iyy="3.86196311673823"
        iyz="-0.336440022491883"
        izz="0.399977905540088" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link3.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link3.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint3"
    type="revolute">
    <origin
      xyz="0.3765 0.82481 -0.49591"
      rpy="2.5802 0 0" />
    <parent
      link="link2" />
    <child
      link="link3" />
    <axis
      xyz="1 0 0" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="100"
      velocity="1" />
  </joint>
  <link
    name="link4">
    <inertial>
      <origin
        xyz="0.162516620557178 -0.213844847423599 3.38133504529381E-07"
        rpy="0 0 0" />
      <mass
        value="12.1646007734167" />
      <inertia
        ixx="0.288208127188405"
        ixy="0.00080326659836716"
        ixz="8.72489392803044E-07"
        iyy="0.107000706575729"
        iyz="-1.48935044230573E-07"
        izz="0.295147863994059" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link4.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link4.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint4"
    type="revolute">
    <origin
      xyz="0 0.070837 0.99728"
      rpy="0.82315 -1.5708 0" />
    <parent
      link="link3" />
    <child
      link="link4" />
    <axis
      xyz="1 0 0" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="100"
      velocity="1" />
  </joint>
</robot>

编译

colcon build --packages-select cpp01_urdf

更新终端环境

source install/setup.bash

运行Launch

ros2 launch cpp01_urdf display.launch.py

在Rviz2中添加插件以及基本配置

使用joint_state_publisher_gui可以调关节的角度

保存rviz2的配置到rviz文件夹中

首先点击rviz2的菜单栏上的File，然后选择Save Config as

可以去本节简介看效果视频

注意事项（仅供参考，ChatGPT的解决方案）
坐标系1

轮子要用continuous的关节，并且尽量自己选坐标系，要求从后面看车的话，左是Y，前是X，上是Z。所有的坐标系都要求。

可以先让他帮你自动生成一个，你再去修改坐标系，要简单一些。

坐标系让车躺着

如果你的车在rviz2里是侧着睡觉的，那么：

哈哈，没错，这种“整车侧着躺尸”的情况 90%就是 STL 的坐标轴方向搞错了 。很多 CAD 工具导出的 STL 模型默认是：

Z 轴朝前（例如 SolidWorks：Z朝前，Y朝上，用右手坐标系推X坐标系位置，从车屁股后面看是朝左。）
或者 Y 轴朝上（例如 Blender）

而 ROS/URDF 的坐标系统是：

X 向前（前进方向）
Y 向左（平移方向）
Z 向上（重力方向）

车直走变旋转，旋转变直走

那说明你的转轴有问题。

如果向前走成了左转，那么说明左轮的转轴错了，少了个符号，加上就行了。

比如这个010,你应该改成0-10

xacro
场景、作用与概念

场景

前面 URDF 文件构建机器人模型的过程中，存在若干问题。

问题1：在设计关节的位置时，需要按照一定的规则计算，规则是固定的，但是在 URDF 中依赖于人工计算，存在不便，容易计算失误，且当某些参数发生改变时，还需要重新计算。

问题2：URDF中的部分内容是高度重复的，比如车轮的设计实现，不同轮子只是部分参数不同，形状、颜色、翻转量都是一致的，在实际应用中，构建复杂的机器人模型时，更是易于出现高度重复的设计，按照一般的编程思想涉及到重复代码应该考虑封装、复用，但是在之前的URDF文件中并没有相关操作。

……

如果在一般编程语言中遇到类似问题，我们可以通过变量结合函数解决。对应的，在 ROS 中也给出了类似编程的优化方案，该方案称之为： Xacro（可以理解为urdf2.0） 。

概念

Xacro 是 XML Macros 的缩写，Xacro 是一种 XML 宏语言，是可编程的 XML。

Xacro 可以声明变量，可以通过数学运算求解；可以使用流程控制控制执行顺序；还可以通过宏封装(可以想成函数)、复用功能，从而提高代码复用率以及程序的安全性。

作用

较之于纯粹的 URDF 实现，可以编写更安全、精简、易读性更强的机器人模型文件，且可以提高编写效率。

快速体验

先安装xacro

#humble版本
sudo apt install ros-humble-xacro
#jazzy版本
sudo apt install ros-jazzy-xacro

1.需求

使用xacro优化 6.4.4 URDF练习 中的小车底盘实现，需要使用变量封装车辆参数，并使用 xacro 宏封装轮子重复的代码并调用宏创建四个轮子(注意: 在此，演示 xacro 的基本使用，不必要生成合法的 URDF )。

2.实现

功能包cpp06_urdf的urdf/xacro目录下，新建xacro文件demo01_helloworld.urdf.xacro，并编辑文件，输入如下内容：

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 属性封装 -->
    <xacro:property name="wheel_radius" value="0.025" />
    <xacro:property name="wheel_length" value="0.02" />
    <xacro:property name="PI" value="3.1415927" />

    <!-- 宏 -->
    <xacro:macro name="wheel_func" params="wheel_name" >
        <link name="${wheel_name}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>

                <origin xyz="0 0 0" rpy="${PI / 2} 0 0" />

                <material name="wheel_color">
                    <color rgba="0 0 0 0.3" />
                </material>
            </visual>
        </link>
    </xacro:macro>
    <xacro:wheel_func wheel_name="left_front"/>
    <xacro:wheel_func wheel_name="left_back"/>
    <xacro:wheel_func wheel_name="right_front"/>
    <xacro:wheel_func wheel_name="right_back"/>
</robot>

宏类似函数

params类似入口参数

子标签类似函数体

终端下进入当前文件所述目录，输入如下指令：

cd src/cpp06_urdf/urdf/xacro/
xacro demo01_helloworld.urdf.xacro
#或者
ros2 run xacro xacro demo01_helloworld.urdf.xacro

终端将会输出如下内容（以下内容是纯urdf）：

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from demo01_helloworld.urdf.xacro   | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="mycar">
  <link name="left_front_wheel">
    <visual>
      <geometry>
        <cylinder length="0.02" radius="0.025"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="wheel_color">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
  </link>
  <link name="left_back_wheel">
    <visual>
      <geometry>
        <cylinder length="0.02" radius="0.025"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="wheel_color">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
  </link>
  <link name="right_front_wheel">
    <visual>
      <geometry>
        <cylinder length="0.02" radius="0.025"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="wheel_color">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
  </link>
  <link name="right_back_wheel">
    <visual>
      <geometry>
        <cylinder length="0.02" radius="0.025"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="wheel_color">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
  </link>
</robot>

显然的，通过xacro我们方便的实现了代码复用。

语法

简介

xacro 提供了可编程接口，类似于计算机语言，包括变量声明调用、函数声明与调用等语法实现。在使用 xacro 生成 urdf 时，根标签robot中必须包含命名空间声明:xmlns:xacro="http://wiki.ros.org/xacro"。

变量

变量用于封装 URDF 中的一些字段，比如: PAI 值，小车的尺寸，轮子半径 ….，变量的基本使用语法包括变量定义、变量调用、变量运算等。

1.1变量定义

语法格式：

<xacro:property name="变量名" value="变量值" />

示例：

<xacro:property name="PI" value="3.1416"/>
<xacro:property name="wheel_radius" value="0.025"/>
<xacro:property name="wheel_length" value="0.02"/>

1.2变量调用

语法格式：

${变量名}

示例：

<geometry>
    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
</geometry>

1.3变量运算

语法格式：

${数学表达式}

示例：

<origin xyz="0 0 0" rpy="${PI / 2} 0 0" />

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="demo2_pro">
<!-- 变量声明 -->
<xacro:property name="num1" value="10"/>
<xacro:property name="num2" value="20"/>
<!-- 变量调用 -->
<car length="${num1}" width="${num2}"/>
<!-- 变量运算 -->
<sum value="${num1 + num2}"/>
</robot>

宏

类似于函数实现，提高代码复用率，优化代码结构，提高安全性。宏的基本使用语法包括宏的定义与调用。

2.1宏定义

语法格式：

<xacro:macro name="宏名称" params="参数列表(多参数之间使用空格分隔)">
    .....
    参数调用格式: ${参数名}
</xacro:macro>

示例：

<xacro:macro name="wheel_func" params="wheel_name" >
    <link name="${wheel_name}_wheel">
        <visual>
            <geometry>
                <cylinder radius="${wheel_radius}" length="${wheel_length}" />
            </geometry>
    
            <origin xyz="0 0 0" rpy="${PI / 2} 0 0" />
    
            <material name="wheel_color">
                <color rgba="0 0 0 0.3" />
            </material>
        </visual>
    </link>
</xacro:macro>

2.2宏调用

语法格式：

<xacro:宏名称 参数1=xxx 参数2=xxx/>

示例：

<xacro:wheel_func wheel_name="left_front"/>
<xacro:wheel_func wheel_name="left_back"/>
<xacro:wheel_func wheel_name="right_front"/>
<xacro:wheel_func wheel_name="right_back"/>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="demo3_func">
    <!-- 宏定义 -->
    <xacro:macro name="get_sum" params="num1 num2">
        <sum value="${num1 + num2}"/>
    </xacro:macro>
    
    <!-- 宏调用 -->
    <xacro:get_sum num1="20" num2="30"/>
    <xacro:get_sum num1="70" num2="30"/>
    
</robot>

文件

机器人由多部件组成，不同部件可能封装为单独的 xacro 文件，最后再将不同的文件集成，组合为完整机器人，可以使用文件包含实现。

语法格式：

<xacro:include filename="其他xacro文件" />

示例：

<robot name="car" xmlns:xacro="http://wiki.ros.org/xacro">
      <xacro:include filename="car_base.xacro" />
      <xacro:include filename="car_camera.xacro" />
      <xacro:include filename="car_laser.xacro" />
</robot>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="demo4_include">
    <xacro:include filename="demo02_base_pro.urdf.xacro"/>
    <xacro:include filename="demo03_base_func.urdf.xacro"/>
</robot>

但不建议这样，建议父级xacro和子级xacro使用同样的name。

练习
框架

1.需求

使用xacro创建一个四轮机器人模型，该模型底盘可以参考 6.4.4 URDF练习 中的实现，并且在底盘之上添加了相机与激光雷达。相机与激光雷达的尺寸参数、安装位置可自定义。

2.实现分析

需求中的机器人模型是由底盘、摄像头和雷达三部分组成的，那么可以将每一部分都封装进一个xacro文件，最后再通过xacro文件包含组织成一个完整的机器人模型。

3.实现

功能包cpp06_urdf的urdf/xacro目录下，新建多个xacro文件，分别为：

car.urdf.xacro：用于包含不同机器人部件对应的xacro文件；
car_base.urdf.xacro：描述机器人底盘的xacro文件；
car_camera.urdf.xacro：描述摄像头的xacro文件；
car_laser.urdf.xacro：描述雷达的xacro文件。

编辑car.urdf.xacro文件，输入如下内容：

<robot name="car" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="car_base.urdf.xacro"/>
    <xacro:include filename="car_camera.urdf.xacro"/>
    <xacro:include filename="car_laser.urdf.xacro"/>
</robot>

车体

编辑car_base.urdf.xacro文件，输入如下内容：

<robot xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- PI 值 -->
    <xacro:property name="PI" value="3.1416"/>
    <!-- 定义车辆参数 -->
    <!-- 车体长宽高 -->
    <xacro:property name="base_link_x" value="0.2"/>
    <xacro:property name="base_link_y" value="0.12"/>
    <xacro:property name="base_link_z" value="0.07"/>
    <!-- 离地间距 -->
    <xacro:property name="distance" value="0.015"/>
    <!-- 车轮半径 宽度 -->
    <xacro:property name="wheel_radius" value="0.025"/>
    <xacro:property name="wheel_length" value="0.02"/>

    <!-- 定义颜色 -->
    <material name="yellow">
        <color rgba="0.7 0.7 0 0.8" />
    </material>
    <material name="red">
        <color rgba="0.8 0.1 0.1 0.8" />
    </material>
    <material name="gray">
        <color rgba="0.2 0.2 0.2 0.95" />
      </material>
    <!-- 定义 base_footprint -->
    <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="0.001"/>
            </geometry>
        </visual>
    </link>

    <!-- 定义 base_link -->
    <link name="base_link">
        <visual>
            <!-- 形状 -->
            <geometry>
                <box size="${base_link_x} ${base_link_y} ${base_link_z}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="yellow"/>
        </visual>
    </link>
    <joint name="baselink2basefootprint" type="fixed">
        <parent link="base_footprint"/>
        <child link="base_link"/>
        <origin xyz="0.0 0.0 ${distance + base_link_z / 2}"/>
    </joint>
    <!-- 车轮宏定义 -->
    <xacro:macro name="wheel_func" params="wheel_name is_front is_left" >
        <link name="${wheel_name}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>
                <origin xyz="0 0 0" rpy="${PI / 2} 0 0" />
                <material name="gray"/>
            </visual>
        </link>
        <joint name="${wheel_name}2baselink" type="continuous">
            <parent link="base_link"  />
            <child link="${wheel_name}_wheel" />
            <origin xyz="${(base_link_x / 2 - wheel_radius) * is_front} ${base_link_y / 2 * is_left} ${(base_link_z / 2 + distance - wheel_radius) * -1}" rpy="0 0 0" />
            <axis xyz="0 1 0" />
        </joint>
    </xacro:macro>
    <!-- 车轮宏调用 -->
    <xacro:wheel_func wheel_name="left_front" is_front="1" is_left="1" />
    <xacro:wheel_func wheel_name="left_back" is_front="-1" is_left="1" />
    <xacro:wheel_func wheel_name="right_front" is_front="1" is_left="-1" />
    <xacro:wheel_func wheel_name="right_back" is_front="-1" is_left="-1" />
</robot>

添加摄像头

编辑car_camera.urdf.xacro文件，输入如下内容：

<!-- 摄像头相关的 xacro 文件 -->
<robot xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 摄像头属性 -->
    <xacro:property name="camera_x" value="0.012" /> <!-- 摄像头长度(x) -->
    <xacro:property name="camera_y" value="0.05" /> <!-- 摄像头宽度(y) -->
    <xacro:property name="camera_z" value="0.01" /> <!-- 摄像头高度(z) -->
    <xacro:property name="camera_joint_x" value="${base_link_x / 2 - camera_x / 2}" /> <!-- 摄像头安装的x坐标 -->
    <xacro:property name="camera_joint_y" value="0.0" /> <!-- 摄像头安装的y坐标 -->
    <xacro:property name="camera_joint_z" value="${base_link_z / 2 + camera_z / 2}" /> <!-- 摄像头安装的z坐标:底盘高度 / 2 + 摄像头高度 / 2  -->

    <!-- 摄像头关节以及link -->
    <link name="camera">
        <visual>
            <geometry>
                <box size="${camera_x} ${camera_y} ${camera_z}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="red" />
        </visual>
    </link>

    <joint name="camera2baselink" type="fixed">
        <parent link="base_link" />
        <child link="camera" />
        <origin xyz="${camera_joint_x} ${camera_joint_y} ${camera_joint_z}" />
    </joint>
</robot>

添加雷达

编辑car_laser.urdf.xacro文件，输入如下内容：

<!--
    小车底盘添加雷达
-->
<robot xmlns:xacro="http://wiki.ros.org/xacro">

    <material name="blue">
        <color rgba="0.0 0.0 0.4 0.95" />
    </material>

    <!-- 雷达属性 -->
    <xacro:property name="laser_length" value="0.03" /> <!-- 雷达长度 -->
    <xacro:property name="laser_radius" value="0.03" /> <!-- 雷达半径 -->
    <xacro:property name="laser_joint_x" value="0.0" /> <!-- 雷达安装的x坐标 -->
    <xacro:property name="laser_joint_y" value="0.0" /> <!-- 雷达安装的y坐标 -->
    <xacro:property name="laser_joint_z" value="${base_link_z / 2 + laser_length / 2}" /> <!-- 雷达安装的z坐标:车体高度 / 2 + 雷达高度 / 2  -->

    <!-- 雷达关节以及link -->
    <link name="laser">
        <visual>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="blue" />
        </visual>
    </link>

    <joint name="laser2baselink" type="fixed">
        <parent link="base_link" />
        <child link="laser" />
        <origin xyz="${laser_joint_x} ${laser_joint_y} ${laser_joint_z}" />
    </joint>
</robot>

执行

编译后，工作空间终端下调用如下命令执行：

# ROS Humble
ros2 launch cpp06_urdf display.launch.py model:=ros2 pkg prefix --share cpp06_urdf/urdf/xacro/car.urdf.xacro
#ROS Jazzy
colcon build
source install/setup.bash
ros2 run xacro xacro $(ros2 pkg prefix cpp06_urdf)/share/cpp06_urdf/urdf/xacro/car.urdf.xacro -o ./src/cpp06_urdf/urdf/urdf/car.urdf
ros2 launch cpp06_urdf display.launch.py model:=./src/cpp06_urdf/urdf/urdf/car.urdf

命令执行后，rviz2 中可以显示与需求类似的机器人模型。

小结

目前只是空壳，激光雷达和摄像头以及轮子还都是空壳，到进阶联系中，才可以实现作用。

Stage_Ros2仿真平台

stage_ros2是基于Stage模拟器的ROS2功能包，用于在虚拟环境中模拟和控制机器人的行为。它提供了强大的工具和通信接口，可用于开发、测试和验证机器人系统。通过stage_ros2，用户可以轻松创建虚拟环境，并使用ROS 2发送指令控制机器人的运动，同时接收传感器数据。这个功能包可以帮助用户以更低的成本和风险进行机器人开发，提供了高效且灵活的机器人仿真环境。

stage使用手册：

http://rtv.github.io/Stage/modules.html

（ 可以不学 ，连ROS2官方都已经移除掉了Stage， 用更牛X的Gazebo更好 ，但学一学也问题不大）

stage_ros2安装与运行

1.安装

因为ROS2官方已经移除掉了Stage，所以我们要通过源码编译手动安装，已经有大佬适配了ROS2版本的Stage：

请先调用如下指令安装依赖：

sudo apt-get install git cmake g++ libjpeg8-dev libpng-dev libglu1-mesa-dev libltdl-dev libfltk1.1-dev

进入ROS2工作空间的src目录，调用如下指令下载相关仓库：

git clone https://github.com/damuxt/Stage.git
git clone https://github.com/damuxt/stage_ros2.git

进入工作空间目录，使用colcon build指令进行构建。

（这是第三方作者适配Humble版本的，暂时不支持Jazzy版本，Jazzy版本目前编译会报错，可能作者会更新Jazzy版本的）

2.节点说明

stage_ros2节点是stage_ros2功能包的核心节点之一。它通过加载world文件来创建一个仿真场景，包括地图、障碍物和机器人等元素。world文件描述了虚拟环境的属性，节点会根据文件中的描述构建对应的环境。stage_ros2节点利用这个虚拟环境来模拟机器人的运动、感知和控制。通过该节点和world文件的结合，可以进行机器人的仿真和测试。

3.使用

在功能包下，已经内置了使用示例，终端下可以执行如下指令启动示例：

ros2 launch stage_ros2 my_house.launch.py

模拟器以及rviz2将被启动，并且在rviz2中可以显示里程计、激光雷达以及深度相机等相关信息，运行结果如下。

stage_ros2仿真框架搭建

本节开始，我们将介绍如何通过stage_ros2自定义仿真环境。

1.准备工作

首先请创建一个功能包，指令如下：

ros2 pkg create demo_stage_sim --dependencies stage_ros2

然后在功能包下新建launch、world、config目录，并修改功能包下的CMakeLists.txt文件，添加如下代码：

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

其中，launch目录用于存储launch文件，config用于存储rviz2的配置文件，world用于存储仿真相关文件。world目录中请新建maps目录，并将课程配套资料中的图片素材复制进去。

2.搭建代码框架

在功能包的launch目录下，新建stage_sim.launch.py文件，并输入如下内容：

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    this_directory = get_package_share_directory('demo_stage_sim')
return LaunchDescription([
        Node(
            package='stage_ros2',
            executable='stage_ros2',
            name='stage',
            parameters=[{"world_file": os.path.join(this_directory,'world','sim.world')}],
        )
    ])

该launch文件将运行stage_ros2节点，并加载world目录下的sim.world文件。

在world目录下，新建sim.world文件，并输入如下内容：

# -----------------------------------------------------------------------------
# 设置窗体
# 设置模拟器地图分辨率(以 米/像素 为单位)
resolution 0.02
# 设置模拟器的时间步长(以 毫秒 为单位)
interval_sim 100

sim.world文件当前只是声明了仿真环境的一些通用参数。

3.编译

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

colcon build --packages-select demo_stage_sim

4.执行

当前工作空间下，启动终端，并输入如下指令：

. install/setup.bash
ros2 launch demo_stage_sim stage_sim.launch.py

执行该launch文件后，将生成一个窗口，该窗口中并无任何内容，下一步我们就可以着手创建具体的仿真内容了。

5.world文件参数

摘要和默认值

name                      <worldfile name>
interval_sim              100
quit_time                 0
resolution                0.02
show_clock                0
show_clock_interval       100
threads                   0

详解

name
用于世界的识别名称，例如在图形用户界面的标题栏中使用。
interval_sim
每次调用 World::Update() 时运行的模拟时间量。每个模型都有其自己可配置的更新间隔，可以大于或小于此值，但比此值更短的间隔在图形用户界面或 World 更新回调中是不可见的。您可能不需要改变默认值100毫秒：此值在客户端内部使用，如Player和WebSim。
quit_time 在模拟时间达到指定的秒数后停止模拟。在 libstage 中，World::Update() 返回 true。在带有图形用户界面的 Stage 中，模拟被暂停。在没有图形用户界面的 Stage 中，Stage 会退出。
resolution 底层位图模型的分辨率（以米为单位）。较大的值可以加快射线追踪速度，但会以碰撞检测和感知精度为代价。通常情况下，默认值是一个合理的选择。
show_clock
如果非零，每经过 $show_clock_interval 次更新就在标准输出上打印模拟时间。这对于观察非 GUI 模拟的进展非常有用。
show_clock_interval
设置在启用 $show_clock 的情况下，在标准输出上打印时间之间的更新次数。默认值是每经过 10 秒模拟时间打印一次。较小的值会稍微降低模拟速度。
threads
要生成的工作线程数。一些模型可以并行更新（例如激光器、测距仪），在这里运行 2 个或更多线程可能会使模拟运行更快，取决于可用的 CPU 内核数量和世界文件的情况。如果您有启用并行的高分辨率模型（例如带有数百或数千个样本的激光器或大量模型），则每个内核使用一个线程。

stage_ros2设置窗体

stage 窗体由菜单栏和模拟世界的视图组成。我们可以放大和缩小视图，并滚动视图以查看模拟世界的更多内容。模拟的机器人设备、障碍物等被渲染为彩色多边形。窗体中还可以实现各种传感器和执行器模型的数据和配置的可视化。菜单则具有用于控制呈现哪些数据和配置的选项。本节我们将介绍如何设置以及操作stage窗体。

1.设置窗体

设置窗体相关参数，可以在stage_sim.launch.py文件中添加如下内容：

# 配置窗体参数
window
(
  size [ 700.000 700.000 ] # 窗体尺寸(以 像素 为单位)
  scale 35  # 缩放比
  center [ 0  0 ] # 地图相对于窗体的偏移量(以 米 为单位)
  rotate [ 0  0 ] # 地图旋转角度
  show_data 1     # 是否显示传感器数据 1=on 0=off
)

上述代码创建了一个窗体对象，该窗体分辨率为700*700，使用了35倍缩放比，后续加载的地图相对于窗体无偏移无旋转且会以可视化的方式显示传感器数据。

2.编译执行

构建功能包并执行launch文件后，运行结果如下。

3.窗体参数

摘要和默认值

window
(
  size [ 400 300 ]

  # camera options
  center [ 0 0 ]
  rotate [ 0 0 ]
  scale 1.0

  # perspective camera options
  pcam_loc [ 0 -4 2 ]
  pcam_angle [ 70 0 ]

  # GUI options
  show_data 0
  show_flags 1
  show_blocks 1
  show_clock 1
  show_footprints 0
  show_grid 1
  show_trailarrows 0
  show_trailrise 0
  show_trailfast 0
  show_occupancy 0
  show_tree 0
  pcam_on 0
  screenshots 0
)

详解

speedup
Stage 将尝试以实时的这个倍数运行。如果设置为 -1，Stage 将尽可能以最快速度运行，并且不会尝试跟踪实时时间。
size [ \]
窗口的大小（以像素为单位）。
center [ \]
窗口中心的位置，使用世界坐标表示（以米为单位）。
rotate [ \]
相对于垂直向上的角度，旋转角度（以度为单位）。
scale
世界坐标到像素坐标的比例（窗口缩放）。
pcam_loc [ \]
透视摄像机的位置（以米为单位）。
pcam_angle [ \]
透视摄像机的垂直和水平角度。
pcam_on
是否启用透视摄像机（0/1）。

4.操作窗体

滚动视图

在背景上左键单击并拖动以移动你对世界的视图。

缩放视图

滚动鼠标滚轮可放大或缩小鼠标光标处的位置。

保存世界

你可以保存当前世界中所有事物的位置，使用“文件/保存”菜单项。警告：保存的位置将覆盖当前的世界文件。在保存之前，先复制你的世界文件，以保留旧的位置。另外，可以使用“文件/另存为”菜单项将其保存到一个新的世界文件中。

暂停和恢复时钟

按下“p”键可以暂停或恢复模拟。按下“.”（句号）键可以运行一个模拟步骤。按住“.”键可以重复多次步进。步进会让模拟暂停，所以按下“p”键可以恢复运行。可以在世界文件中使用“paused”属性来设置初始的暂停/运行状态。

选择模型

可以通过点击左键来选择模型。通过按住Shift键并点击多个模型，也可以选择多个模型。选择的模型可以通过拖动来移动，或者通过按住右键并移动鼠标来旋转。点击世界中的空白位置可以清除选择。在清除选择后，最后选中的单个模型将被保存为影响特定模型的几个视图选项的目标。

视图选项

视图菜单提供了许多影响世界渲染方式的功能。在每个选项的右侧，通常有一个按键快捷键，可快速切换相关选项。

“Data”选项（快捷键’d’）可切换传感器数据可视化。滤波器数据选项（快捷键Shift+’d’）打开对话框，可启用或禁用特定传感器的可视化。对话框中的“Visualize All”选项可切换是否为所有模型启用传感器可视化，还是仅为当前选择的模型启用。

“Follow”选项可保持视图始终在最后选择的模型上。

“Perspective camera”选项可以从正交视图切换到透视视图。

保存截图

要保存世界的一系列截图，从视图菜单中选择“Save screenshots”选项以开始录制图像，然后再次从菜单中选择该选项以停止。

stage_ros2基本模型

基本模型（model）模拟具有基本属性的对象；位置、大小、速度、颜色、对各种传感器的可见性等。基本模型还具有由线性列表组成的主体。在内部，基本模型被用作所有其他模型类型的基类。我们可以使用基本模型来模拟环境对象。在stage中，底盘模型、雷达模型、相机模型等都可以看作是基本模型的子类。

1.添加基本模型

在stage_sim.launch.py文件中添加如下内容：

# -----------------------------------------------------------------------------
# 添加障碍物
model( pose [ -2 -4 0 0 ] color "green")
# define a block
define my_block model
(
  size [1.0 1.0 1.0]
  gui_nose 0
  gui_grid 0
  gui_outline 0
)

# throw in a block
my_block( pose [ 0 4 0 90 ] color "red" bitmap "maps/ghost.png")

上述代码中，直接使用默认的基础model创建了一个绿色模型对象，并且还自定义了继承自model的my_block模型，然后创建了该模型对象。

2.编译执行

构建功能包并执行launch文件后，运行结果如下。

3.窗体参数

摘要和默认值

model (
    pose [ 0.0 0.0 0.0 0.0 ]
    size [ 0.1 0.1 0.1 ]
    origin [ 0.0 0.0 0.0 0.0 ]
    velocity [ 0.0 0.0 0.0 0.0 ]
    color "red"
    color_rgba [ 0.0 0.0 0.0 1.0 ]
    bitmap ""
    ctrl ""
    # determine how the model appears in various sensors
    fiducial_return 0
    fiducial_key 0
    obstacle_return 1
    ranger_return 1
    blob_return 1
    laser_return LaserVisible
    gripper_return 0
    gravity_return 0
    sticky_return 0
    # GUI properties
    gui_nose 0
    gui_grid 0
    gui_outline 1
    gui_move 0 (1 if the model has no parents);
    boundary 0
    mass 10.0
    map_resolution 0.1
    say ""
    alwayson 0
)

详解

pose [ x: y: z: heading: \] 指定模型在其父坐标系中的姿态。
size [ x: y: z: \] 指定模型在各个维度上的大小。
origin [ x: y: z: heading: \] 指定对象中心的位置，相对于其姿态。
velocity [ x: y: z: heading: omega: \] 指定模型的初始速度。请注意，如果模型撞到障碍物，其速度将被设置为零。
velocity_enable int (默认为0) 大多数模型忽略其速度状态。这样可以节省处理时间，因为大多数模型的速度永远不会被设置为非零值。一些子类（例如ModelPosition）会改变这个默认值，因为它们期望移动。用户可以在这里指定一个非零值来启用对该模型的速度控制。这与调用Model::VelocityEnable()的效果相同。
color 使用X11数据库（rgb.txt）中的颜色名称指定对象的颜色。
bitmap filename: 通过解释位图中的线条来绘制模型（支持bmp、jpeg、gif、png）。文件被打开并解析为一组线条。这些线条被缩放以适应模型当前大小所定义的矩形内。
ctrl 指定模型的控制器模块及其参数字符串。例如，字符串"foo bar bash"将加载libfoo.so，其Init()函数将以整个字符串作为参数进行调用（包括库名称）。控制器需要解析字符串（如果需要参数）。
fiducial_return fiducial_id 如果非零，则通过fiducialfinder传感器检测到此模型。该值用作基准标识符。
fiducial_key 仅当模型和fiducialfinder的fiducial\_key值匹配时，fiducial\_id模型才会被fiducialfinder检测到。这允许您在同一环境中拥有几种独立类型的基准标识，每种类型只显示在为其"调谐"的fiducialfinders中。
obstacle_return 如果为1，则此模型可以与具有此属性设置的其他模型发生碰撞。
ranger_return 如果为1，则该模型可以被ranger传感器检测到。
blob_return 如果为1，则该模型可以在blob\_finder中被检测到（取决于其颜色）。
laser_return 如果为0，则此模型不会被激光传感器检测到。如果为1，则该模型在激光传感器中显示为正常（0）反射。如果为2，则显示为高（1）反射。
gripper_return 如果为1，则该模型可以被夹爪夹取，并且可以通过与具有非零obstacle\_return的任何东西的碰撞来推动。
gui_nose 如果为1，则在模型上绘制显示其朝向的鼻子（正X轴）。
gui_grid 如果为1，则在模型上绘制比例网格。
gui_outline 如果为1，则在模型周围绘制边界框，指示其大小。
gui_move 如果为1，则模型可以在GUI窗口中通过鼠标移动。

stage_ros2添加地图

本节我们将在仿真环境中，添加一张全局地图，请先准备一张作为全局地图的图片。

1.添加地图

在stage_sim.launch.py文件中添加如下内容：

# -----------------------------------------------------------------------------
# 设置地图(定义模型)
define floorplan model
(
  color "gray30"  # 颜色
  boundary 1  # 为地图设置边框

  gui_nose 0
  gui_grid 0 

  gui_outline 0
  gripper_return 0
  fiducial_return 0
  laser_return 1
)

# 加载地图
floorplan
( 
  name "room"
  size [16.000 16.000 0.800]
  pose [0 0 0 0]
  bitmap "maps/room.jpg"
  gui_move 0
)

上述代码自定义了floorplan模型，并根据此模型创建了一个加载了地图数据的floorplan对象。

2.编译执行

构建功能包并执行launch文件后，运行结果如下。

stage_ros2添加机器人

本节将在仿真环境中添加一个由底盘、摄像头以及激光雷达组成的虚拟机器人。

1.代码框架搭建

在world目录下新建robot目录，robot目录中再创建car_base.inc、camera.inc、laser.inc以及mycar.inc等文件，各文件作用如下：

car_base.inc：用于设置机器人底盘模块；
camera.inc：用于设置机器人相机模块；
laser.inc：用于设置机器人雷达模块；
mycar.inc：用于组装机器人各模块。

除此之外，还会在sim.world中包含mycar.inc并调用其生成机器人的功能。

2.设置机器人底盘

stage中的position模型可以以差速、全向或阿克曼的方式模拟移动机器人底盘。在car_base.inc中输入如下代码：

# 机器人底盘配置
define car_base position 
(
  color "red"                   # 车身颜色
  drive "diff"                  # 车辆运动学模型                  
  obstacle_return 1             
  ranger_return 1           
  blob_return 1                  
  fiducial_return 1             

  localization "odom"           # 定位方式
  odom_error [ 0.05 0.05 0.0 0.1 ]  # 里程计误差
  # localization_origin [0 0 0 0]   # 定位原点，默认为机器人的初始位置。

  # [ xmin xmax ymin ymax zmin zmax amin amax ]        
  velocity_bounds [-1 1 0 0 0 0 -45.0 45.0 ]         # 速度最值 
  acceleration_bounds [-0.5 0.5 0 0 0 0 -45 45.0 ]   # 加速度最值


  size [0.44 0.38 0.22]  # 车体尺寸
  origin [0 0 0 0] # 旋转中心与车体中心的偏移量
  mass 23.0 # 车体质量，单位kg

  gui_nose 0 # 是否绘制方向指示标记

  block( 
    points 8
    point[0] [-0.2 0.18]
    point[1] [-0.2 -0.18]
    point[2] [-0.15 -0.27]
    point[3] [0.12 -0.23]
    point[4] [0.2 -0.12]
    point[5] [0.2 0.12]
    point[6] [0.12 0.23]
    point[7] [-0.15 0.27]
    z [0 0.22]
  )
)

position摘要和默认值

position(
    drive "diff"
    localization "gps"
    localization_origin [<defaults to model's start pose>]
    odom_error [0.03 0.03 0.00 0.05]
    velocity_enable 1
)

详解

drive “diff”, “omni” 或 “car” 选择差速转向模型、全向模式或类似汽车的阿克曼模式。
localization “gps” 或 “odom” 如果选择 “gps”，位置模型将以完全准确的精度报告位置。如果选择 “odom”，将使用简单的里程计模型，并且位置数据会随时间与真实位置的差异而漂移。里程计模型由 odom_error 属性参数化。
localization_origin [x y z theta] 您可以使用 localization_origin 参数来设置定位坐标系的原点。默认情况下，它将复制模型的初始姿态，因此机器人将报告相对于起始位置的位置。提示: 如果将 localization_origin 设置为 [0 0 0 0] 并且定位方式为 “gps”，模型将返回其真实的全局位置。这种设置是不现实的，但在希望抽象定位细节的研究中很有用。
odom_error [x y z theta] 在选择 “odom” 定位方式时用到的里程计误差模型参数，每个值是计算里程计位置估计时积分 x、y 和 theta 速度的误差比例的最大值。对于每个轴，如果在此处指定的值为 E，则实际比例在启动时在 -E/2 到 +E/2 的范围内随机选择。请注意，由于舍入误差，将这些值设置为零并不能让定位完美无误 - 为了实现这一点，您需要选择 “gps” 定位方式。

3.设置摄像头

stage中的camera模型可以模拟深度相机。在camera.inc中输入如下代码：

define my_camera camera
(
    range [ 0.3 3.0 ] # 相机采样范围
    resolution [ 160 90 ]  #相机分辨率 1280 × 720 / 8
    fov [ 87 58 ] # 相机视场
    pantilt [ 0 0 ] # 相机姿态
    alwayson 1 # 是否一直处于启动状态
    size [ 0.025 0.09 0.025 ] # 相机尺寸
    color "gray" # 相机颜色
)

camera摘要和默认值

camera(
  resolution [ 32 32 ]
  range [ 0.2 8.0 ]
  fov [ 70.0 40.0 ]
  pantilt [ 0.0 0.0 ]
  size [ 0.1 0.07 0.05 ]
  color "black"
  watts 100.0 
)

详解

resolution [ width: height:\] 相机分辨率。
range [ min: max: \] 相机报告的距离范围，以米为单位。距离小于`min`或大于`max`的物体将无法显示。`min`数字越小，深度精度越低 - 不要将此值设置得太接近 0。
fov [ horizontal: vertical: \] 水平和垂直视野的角度，以度为单位。
pantilt [ pan: tilt: \] 相机的朝向角度，以度为单位。左右位置称为 pan，上下位置称为 tilt。

4.设置激光雷达

stage中的ranger模型可以模拟激光雷达。在laser.inc中输入如下代码：

define my_laser ranger
(
  sensor(

    range [ 0.0  15.0 ]     # 雷达数据采集区间
    fov 360.0               # 视角
    samples 720             # 采样数
    color_rgba [ 0 0 1 0.15 ] # 可视化光束颜色以及透明度
  )
  model # 雷达外观
  (
    pose [ 0 0 0 0 ]        # 雷达位姿
    size [ 0.07 0.07 0.05 ] # 雷达尺寸信息
    color "blue"            # 雷达颜色
  )
)

ranger摘要和默认值

 sensor(
   samples 180
   range_max 8.0
   fov 360.0
   resolution 1
   size [ 0.15 0.15 0.2 ]
   color "blue"
 )

详解

samples 每次扫描的激光样本数量。
range_max 激光扫描仪所报告的最大距离，以米为单位。扫描仪将不会检测超出此范围的物体。
fov 激光扫描仪的角度视野。
resolution 仅计算第 n 个激光样本的真实距离。缺失的样本将用线性插值填充。一般来说，使用较少的样本会更好，但某些（实现不好的）程序需要固定数量的样本数。设置此数字大于 1 可以减少所需的计算量，适用于固定大小的激光矢量。

5.组装机器人

在mycar.inc中实现机器人的组装：

# 组装机器人各个模块
include "robot/car_base.inc"
include "robot/camera.inc"
include "robot/laser.inc"

define my_car car_base(
    my_camera(pose [0.15 0 0 0])
    my_laser()
)

my_car模型是在car_base模型之上，集成了my_camera和my_laser模型。

6.在仿真环境中生成机器人

在sim.world中添加如下代码生成一个机器人：

# -----------------------------------------------------------------------------
# 生成机器人
# 文件包含
include "robot/mycar.inc"

my_car(
  name "robot_0"
  color "red"
  pose [ -3 -7 0 90 ] 
)

7.构建执行

构建功能包并执行launch文件后，运行结果如下。

启动rviz2可以查看仿真环境下传感器相关数据，启动键盘控制节点后，也可以控制机器人运动。

stage_ros2添加多机器人

在stage_ros2中也可以很方便的生成多个机器人。

1.生成多个机器人

在sim.world中还可以继续创建my_car对象以生成新的机器人模型，添加代码如下：

my_car(
  name "robot_1"
  color "yellow"
  pose [ -1 -7 0 90 ] 
)

2.构建执行

构建功能包并执行launch文件后，运行结果如下。

在多机器人运行时，不同的机器人发布的话题会以各自对应的name值为命名空间，且不同机器人发布的坐标变换也会以name值为前缀。

Gezebo仿真平台
简述

Gazebo目前主要分为两个版本，

一个是旧版Gazebo Classic，一个是新版Ignition Gazebo。

本文中称呼的Gazebo均代指Gazebo Classic，而Ignition Gazebo会加以区别。

场景

在ROS机器人开发中，实体机器人虽然具有直接性和真实性的优势，但也存在一些不足，比如：

高昂的成本：研发一款自主移动机器人需要购买昂贵的硬件组件，如传感器、电机、控制器等，且这些硬件在研发初期可能需要频繁更换或升级，导致成本急剧上升。

资源限制：由于资金和资源有限，可能无法同时拥有多台实体机器人进行测试。这会导致测试周期延长，研发进度受阻。

环境的不确定性和复杂性：在真实环境中测试机器人时，可能会遇到各种不可预测的情况，如光线变化、地面不平整、电磁干扰等，这些因素都可能影响机器人的性能。

安全风险：在测试过程中，如果机器人的控制算法或硬件出现故障，可能会导致机器人失控，对人员或环境造成损害。

在ROS机器人开发的领域里，仿真技术被广泛应用以弥补实体机器人测试中的不足。

概念

机器人仿真 是一种利用计算机模型和仿真技术来模拟机器人在虚拟环境中的行为和性能的过程。它通过创建虚拟的机器人和环境模型，模拟机器人的感知、控制和运动能力，以及与环境和其他对象的交互。

作用

通过仿真测试，可以降低机器人研发成本和风险，提高机器人系统的性能和可靠性，并为实际机器人部署提供参考和指导。

仿真优势:

仿真在机器人系统研发过程中占有举足轻重的地位，在研发与测试中较之于实体机器人实现，仿真有如下几点的显著优势:

低成本: 当前机器人成本居高不下，动辄几十万，仿真可以大大降低成本，减小风险
高效: 搭建的环境更为多样且灵活，可以提高测试效率以及测试覆盖率
高安全性: 仿真环境下，无需考虑耗损问题

仿真技术为开发者构建了一个既高效又安全，且成本低廉的全方位测试和验证平台。

仿真缺陷:

机器人在仿真环境与实际环境下的表现差异较大，换言之，仿真并不能完全做到模拟真实的物理世界，存在一些”失真”的情况，原因:

仿真器所使用的物理引擎目前还不能够完全精确模拟真实世界的物理情况
仿真器构建的是关节驱动器（电机&齿轮箱）、传感器与信号通信的绝对理想情况，目前不支持模拟实际硬件缺陷或者一些临界状态等情形

总之，仿真技术虽然重要，但并不能完全替代实体测试。实体测试可以验证仿真结果的准确性，并发现仿真中可能忽略的问题。

Gazebo(Gazebo Classic)

这个是老版Gazebo，仅在ROS1和ROS2 Humble上可用。（但是ROS2并不推荐使用老版Gazebo，更加建议使用新版Gazebo）

ROS2老版Gazebo仅在Humble版本上可用，在Jazzy及以后的版本已移除。

（不想学老版Gazebo的，直接跳到下一节的Ignition Gazebo即可）

这俩对咱们的区别不算太大，也就是一个教程多，一个教程少的区别，你直接学老版Gazebo也是一样用的。

（初学者也可以只学教程多的Gazebo Classic,可以少走一些弯路）

Gezebo官网：

https://gazebosim.org/home

如果你想从老版的Gazebo迁移到新版的Gazebo，那么请看下方官方教程：

从Gazebo Classic迁移到ROS2 Humble的Ign Gazebo(Gazebo Fortress)：

https://gazebosim.org/docs/fortress/gazebo_classic_migration/

从Gazebo Classic迁移到ROS2 Jazzy的Gazebo Sim(Gazebo Harmonic)：

Jazzy之后的版本应该变化不会太大，也可以暂时参照下面这个教程（或者去官网找对应版本的教程）：

https://gazebosim.org/docs/harmonic/gazebo_classic_migration/

Gazebo Classic安装与运行

ROS2只有humble有老版Gazebo

sudo apt install ros-humble-gazebo-ros ros-humble-gazebo-ros-pkgs

安装完成后，我们就可以通过下面的命令行来启动gazebo并加载ros2插件。

gazebo --verbose -s libgazebo_ros_init.so -s libgazebo_ros_factory.so 

看到下面这个日志和gazebo界面，没啥大问题就说明成功了。（一些警告是说Gazebo Classic已经弃用，鼓励使用新版Ingition Gazebo，忽略这些警告即可）

root@Dell-G15-5511:/home/tungchiahui/UserFolder/MySource/ROS_WS/ROS2_WS/6.ws_simulations$ gazebo --verbose -s libgazebo_ros_init.so -s libgazebo_ros_factory.so 
Gazebo multi-robot simulator, version 11.10.2
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Msg] Waiting for master.
Gazebo multi-robot simulator, version 11.10.2
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Wrn] [gazebo_ros_init.cpp:178] 
#     # ####### ####### ###  #####  #######
##    # #     #    #     #  #     # #
# #   # #     #    #     #  #       #
#  #  # #     #    #     #  #       #####
#   # # #     #    #     #  #       #
#    ## #     #    #     #  #     # #
#     # #######    #    ###  #####  #######

This version of Gazebo, now called Gazebo classic, reaches end-of-life
in January 2025. Users are highly encouraged to migrate to the new Gazebo
using our migration guides (https://gazebosim.org/docs/latest/gazebo_classic_migration?utm_source=gazebo_ros_pkgs&utm_medium=cli)


[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 192.168.31.60
[Msg] Loading world file [/usr/share/gazebo-11/worlds/empty.world]
XDG_RUNTIME_DIR (/run/user/1000) is not owned by us (uid 0), but by uid 1000! (This could e.g. happen if you try to connect to a non-root PulseAudio as a root user, over the native protocol. Don't do that.)
ALSA lib pcm_dmix.c:1032:(snd_pcm_dmix_open) unable to open slave
AL lib: (EE) ALCplaybackAlsa_open: Could not open playback device 'default': No such file or directory
[Err] [OpenAL.cc:84] Unable to open audio device[default]
 Audio will be disabled.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 192.168.31.60
[Wrn] [GuiIface.cc:298] Couldn't locate specified .ini. Creating file at "/root/.gazebo/gui.ini"
[Wrn] [GuiIface.cc:120] QStandardPaths: runtime directory '/run/user/1000' is not owned by UID 0, but a directory permissions 0700 owned by UID 1000 GID 1000
[Wrn] [Event.cc:61] Warning: Deleting a connection right after creation. Make sure to save the ConnectionPtr from a Connect call

libcurl: (35) error:0A000126:SSL routines::unexpected eof while reading
[Wrn] [ModelDatabase.cc:212] Unable to connect to model database using [http://models.gazebosim.org//database.config]. Only locally installed models will be available.
[Wrn] [Event.cc:61] Warning: Deleting a connection right after creation. Make sure to save the ConnectionPtr from a Connect call

插件及节点服务介绍

使用之前的命令启动Gazebo并加载gazebo_ros插件，我们使用下面的指令来看插件的节点，以及改节点为我们提供的服务有哪些？

节点列表

ros2 node list

然后我们看看这个节点对外提供的服务有哪些？

ros2 service list

除去和参数相关的几个服务，我们可以看到另外三个特殊服务：

/spawn_entity，用于加载模型到gazebo中
/get_model_list，用于获取模型列表
/delete_entity，用于删除gazbeo中已经加载的模型

我们想要让gazebo显示出我们配置好的机器人模型使用/spawn_entity来加载即可。

接着我们可以来请求服务来加载模型，先带你看一下服务的接口类型。

ros2 service type /spawn_entity

ros2 interface show gazebo_msgs/srv/SpawnEntity

可以看到服务的请求内容包括：

string name ，需要加载的实体的名称 (可选的)。
string xml ，实体的XML描述字符串, URDF或者SDF。
string robot_namespace ，产生的机器人和所有的ROS接口的命名空间，多机器人仿真的时候很有用。
geometry_msgs/Pose initial_pose ，机器人的初始化位置
string reference_frame ，初始姿态是相对于该实体的frame定义的。如果保持”empty”或”world”或“map”，则使用 gazebo的world作为frame。如果指定了不存在的实体，则会返回错误

调用服务加载模型

我们这里教程使用鱼香ROS的fishbot模型：https://github.com/fishros/fishbot/blob/navgation2/src/fishbot_description/urdf/fishbot_gazebo.urdf

<?xml version="1.0"?>
<robot name="fishbot">


  <!-- Robot Footprint -->
  <link name="base_footprint"/>

  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0.0 0.0 0.076" rpy="0 0 0"/>
  </joint>


  <!-- base link -->
  <link name="base_link">
          <visual>
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
      <geometry>
                <cylinder length="0.12" radius="0.10"/>
      </geometry>
      <material name="blue">
              <color rgba="0.1 0.1 1.0 0.5" /> 
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
      <geometry>
                <cylinder length="0.12" radius="0.10"/>
      </geometry>
      <material name="blue">
              <color rgba="0.1 0.1 1.0 0.5" /> 
      </material>
    </collision>
    <inertial>
      <mass value="0.2"/>
      <inertia ixx="0.0122666" ixy="0" ixz="0" iyy="0.0122666" iyz="0" izz="0.02"/>
    </inertial>
  </link>
    
  <!-- laser link -->
  <link name="laser_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.02"/>
      </geometry>
      <material name="black">
        <color rgba="0.0 0.0 0.0 0.5" /> 
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.02" radius="0.02"/>
      </geometry>
      <material name="black">
        <color rgba="0.0 0.0 0.0 0.5" /> 
      </material>
    </collision>
    <inertial>
    <mass value="0.1"/>
      <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
    </inertial>
  </link>
  
  <!-- laser joint -->
  <joint name="laser_joint" type="fixed">
      <parent link="base_link" />
      <child link="laser_link" />
      <origin xyz="0 0 0.075" />
  </joint>

  <link name="imu_link">
          <visual>
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
      <geometry>
                    <box size="0.02 0.02 0.02"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0.0" rpy="0 0 0"/>
      <geometry>
                    <box size="0.02 0.02 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
        <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
      </inertial>
  </link>

  <!-- imu joint -->
  <joint name="imu_joint" type="fixed">
      <parent link="base_link" />
      <child link="imu_link" />
      <origin xyz="0 0 0.02" />
  </joint>


  <link name="left_wheel_link">
      <visual>
        <origin xyz="0 0 0" rpy="1.57079 0 0"/>
        <geometry>
          <cylinder length="0.04" radius="0.032"/>
        </geometry>
          <material name="black">
            <color rgba="0.0 0.0 0.0 0.5" /> 
          </material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="1.57079 0 0"/>
        <geometry>
          <cylinder length="0.04" radius="0.032"/>
        </geometry>
          <material name="black">
            <color rgba="0.0 0.0 0.0 0.5" /> 
          </material>
      </collision>
      <inertial>
        <mass value="0.2"/>
          <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
        </inertial>
  </link>
    
  <link name="right_wheel_link">
      <visual>
        <origin xyz="0 0 0" rpy="1.57079 0 0"/>
        <geometry>
          <cylinder length="0.04" radius="0.032"/>
        </geometry>
          <material name="black">
            <color rgba="0.0 0.0 0.0 0.5" /> 
          </material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="1.57079 0 0"/>
        <geometry>
          <cylinder length="0.04" radius="0.032"/>
        </geometry>
          <material name="black">
            <color rgba="0.0 0.0 0.0 0.5" /> 
          </material>
      </collision>
      <inertial>
      <mass value="0.2"/>
      <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
    </inertial>
  </link>
    
  <joint name="left_wheel_joint" type="continuous">
      <parent link="base_link" />
      <child link="left_wheel_link" />
      <origin xyz="-0.02 0.10 -0.06" />
      <axis xyz="0 1 0" />
  </joint>

  <joint name="right_wheel_joint" type="continuous">
      <parent link="base_link" />
      <child link="right_wheel_link" />
      <origin xyz="-0.02 -0.10 -0.06" />
      <axis xyz="0 1 0" />
  </joint>

  <link name="caster_link">
      <visual>
        <origin xyz="0 0 0" rpy="1.57079 0 0"/>
        <geometry>
            <sphere radius="0.016"/>
        </geometry>
          <material name="black">
            <color rgba="0.0 0.0 0.0 0.5" /> 
          </material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="1.57079 0 0"/>
        <geometry>
            <sphere radius="0.016"/>
        </geometry>
          <material name="black">
            <color rgba="0.0 0.0 0.0 0.5" /> 
          </material>
      </collision>
      <inertial>
      <mass value="0.02"/>
      <inertia ixx="0.000190416666667" ixy="0" ixz="0" iyy="0.0001904" iyz="0" izz="0.00036"/>
    </inertial>
  </link>
    
  <joint name="caster_joint" type="fixed">
      <parent link="base_link" />
      <child link="caster_link" />
      <origin xyz="0.06 0.0 -0.076" />
      <axis xyz="0 1 0" />
  </joint>



  <gazebo reference="caster_link">
    <material>Gazebo/Black</material>
  </gazebo>

  <gazebo reference="caster_link">
    <mu1 value="0.0"/>
    <mu2 value="0.0"/>
    <kp value="1000000.0" />
    <kd value="10.0" />
    <!-- <fdir1 value="0 0 1"/> -->
  </gazebo>


  <gazebo>
    <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
          <ros>
            <namespace>/</namespace>
            <remapping>cmd_vel:=cmd_vel</remapping>
            <remapping>odom:=odom</remapping>
          </ros>
          <update_rate>30</update_rate>
          <!-- wheels -->
          <!-- <left_joint>left_wheel_joint</left_joint> -->
          <!-- <right_joint>right_wheel_joint</right_joint> -->
          <left_joint>left_wheel_joint</left_joint>
          <right_joint>right_wheel_joint</right_joint>
          <!-- kinematics -->
          <wheel_separation>0.2</wheel_separation>
          <wheel_diameter>0.065</wheel_diameter>
          <!-- limits -->
          <max_wheel_torque>20</max_wheel_torque>
          <max_wheel_acceleration>1.0</max_wheel_acceleration>
          <!-- output -->
          <publish_odom>true</publish_odom>
          <publish_odom_tf>true</publish_odom_tf>
          <publish_wheel_tf>false</publish_wheel_tf>
          <odometry_frame>odom</odometry_frame>
          <robot_base_frame>base_footprint</robot_base_frame>
      </plugin>


      <plugin name="fishbot_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
        <ros>
          <remapping>~/out:=joint_states</remapping>
        </ros>
        <update_rate>30</update_rate>
        <joint_name>right_wheel_joint</joint_name>
        <joint_name>left_wheel_joint</joint_name>
      </plugin>    
      </gazebo> 

      <gazebo reference="laser_link">
        <material>Gazebo/Black</material>
      </gazebo>

    <gazebo reference="imu_link">
      <sensor name="imu_sensor" type="imu">
      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
          <ros>
            <namespace>/</namespace>
            <remapping>~/out:=imu</remapping>
          </ros>
          <initial_orientation_as_reference>false</initial_orientation_as_reference>
        </plugin>
        <always_on>true</always_on>
        <update_rate>100</update_rate>
        <visualize>true</visualize>
        <imu>
          <angular_velocity>
            <x>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
              </noise>
            </x>
            <y>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
              </noise>
            </y>
            <z>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
              </noise>
            </z>
          </angular_velocity>
          <linear_acceleration>
            <x>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>1.7e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
              </noise>
            </x>
            <y>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>1.7e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
              </noise>
            </y>
            <z>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>1.7e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
              </noise>
            </z>
          </linear_acceleration>
        </imu>
      </sensor>
    </gazebo>

    <gazebo reference="laser_link">
      <sensor name="laser_sensor" type="ray">
      <always_on>true</always_on>
      <visualize>true</visualize>
      <update_rate>5</update_rate>
      <pose>0 0 0.075 0 0 0</pose>
      <ray>
          <scan>
            <horizontal>
              <samples>360</samples>
              <resolution>1.000000</resolution>
              <min_angle>0.000000</min_angle>
              <max_angle>6.280000</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>0.120000</min>
            <max>3.5</max>
            <resolution>0.015000</resolution>
          </range>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.01</stddev>
          </noise>
      </ray>

      <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
        <ros>
          <!-- <namespace>/tb3</namespace> -->
          <remapping>~/out:=scan</remapping>
        </ros>
        <output_type>sensor_msgs/LaserScan</output_type>
        <frame_name>laser_link</frame_name>
      </plugin>
      </sensor>
    </gazebo>

</robot>

看到这里你是不是迫不及待敲起来命令行来加载我们的机器人到gazebo了，别着急，小鱼再推荐一个可视化服务请求工具，其实在第六章中小鱼介绍过，在rqt工具集里有一个叫服务请求工具。

命令行输入rqt，在插件选项中选择Services->Service Caller,然后再下拉框选择/spawn_entity服务，即可看到下面的界面。

接着我们把我们的FishBot的URDF模型复制粘贴，放到xml中（注意要把原来的’‘删掉哦！）

然后点右上角的call，可以显示下图成功了。

接着就可以看到工厂返回说成功把机器人制作出来送入gazebo了。

此时再看我们的Gazebo,一个小小的，白白的机器人出现了。

shift+鼠标左键，或者直接点滚轮中键，都可以拖动视角。很多玩过第三人称游戏的学弟学妹肯定都不陌生这种操控吧。

在不同位置加载多个机器人

可以再生产一个fishbot（为了后面需要多机器人仿真的小伙伴）。

修改rqt中的参数，增加一个命名空间，然后修改一个位置，让第二个机器人和第一个相距1m的地方生产，然后点击Call。

返回成功，此时拖送Gazebo观察一下，发现多出了一个机器人，距离刚好是在X轴（红色）1米（一个小格子一米）处。

查询和删除机器人

利用rqt工具，我们再对另外两个服务接口进行请求。

首先先查询有几个模型在仿真环境中

查到了三个模型，一个大地，一个fishbot，一个fishbot_0。

我们接着尝试把fishbot_0删掉，选择删除实体，输入fishbot_0的名字，拿起小电话通知工厂回收我们的0号fishbot。

调用成功，观察gazebo发现机器人，人没了

创建工作空间

本节代码参考鱼香ROS：https://github.com/fishros/fishbot/tree/navgation2

# 创建工作空间
mkdir -p ws_simulations/src
cd ws_simulations/src

创建功能包

ros2 pkg create fishbot_description --build-type ament_cmake
cd fishbot_description

然后配置package.xml

  <exec_depend>rviz2</exec_depend>
  <exec_depend>xacro</exec_depend>
  <exec_depend>robot_state_publisher</exec_depend>
  <exec_depend>joint_state_publisher</exec_depend>
  <exec_depend>ros2launch</exec_depend>

然后修改cmakelists.txt

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

克隆下鱼香ROS的仓库，并复制下里面的文件到咱们的目录下。这都是上一节的东西，与本节学习无关，直接复制就行。

打开工作空间，在src/fishbot_description/launch中添加一个gazebo.launch.py文件，我们开始编写launch文件来在gazebo中加载机器人模型。

我们主要需要做两件事：

启动gazebo，我们可以将命令行写成一个launch节点

ExecuteProcess(
        cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', gazebo_world_path],
        output='screen')

上面我们加载机器人是直接将XML格式的URDF复制过去进行加载的，这样很不方便，我们可以使用gazebo_ros为我们提供好的一个叫做spawn_entity.py节点，该节点支持从文件地址直接生产机器人到Gazebo。

该节点需要两个参数，一个机器人的模型名字和urdf的文件地址，这个简单，前面我们曾经使用package_share来拼接过urdf路径。

spawn_entity_cmd = Node(
    package='gazebo_ros', 
    executable='spawn_entity.py',
    arguments=['-entity', robot_name_in_model,  '-file', urdf_model_path ], output='screen')

先加载赵虚左老师的模板

我们首先需要ExecuteProcess来输入终端命令，所以要先把from launch.actions import ExecuteProcess的注释先打开。

为了方便修改加载的机器人模型和urdf,我们还需要使用share目录，所以也要把from ament_index_python.packages import get_package_share_directory的注释打开，并import os。

写完后的launch文件如下：

from launch import LaunchDescription
from launch_ros.actions import Node

# 封装终端指令相关类
from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
# from launch.actions import DeclareLaunchArgument
# from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    robot_name_in_model = 'fishbot'
    package_name = 'fishbot_description'
    urdf_name = "fishbot_gazebo.urdf"

    pkg_share = get_package_share_directory(f"{package_name}")
    urdf_model_path = os.path.join(pkg_share, f'urdf/urdf/{urdf_name}')

    # Start Gazebo server
    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
        output='screen')
        
    # Launch the robot
    spawn_entity_cmd = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=['-entity', robot_name_in_model,  '-file', urdf_model_path ], output='screen')

    return LaunchDescription([start_gazebo_cmd,spawn_entity_cmd])

编译运行

colcon build --packages-select fishbot_description
source install/setup.bash
ros2 launch fishbot_description gazebo.launch.py

插件

使用下面的指令可以查看所有的动态链接库：

ls /opt/ros/humble/lib/libgazebo_ros*

/opt/ros/humble/lib/libgazebo_ros2_control.so
/opt/ros/humble/lib/libgazebo_ros_ackermann_drive.so
/opt/ros/humble/lib/libgazebo_ros_bumper.so
/opt/ros/humble/lib/libgazebo_ros_camera.so
/opt/ros/humble/lib/libgazebo_ros_diff_drive.so
/opt/ros/humble/lib/libgazebo_ros_elevator.so
/opt/ros/humble/lib/libgazebo_ros_factory.so
/opt/ros/humble/lib/libgazebo_ros_force.so
/opt/ros/humble/lib/libgazebo_ros_force_system.so
/opt/ros/humble/lib/libgazebo_ros_ft_sensor.so
/opt/ros/humble/lib/libgazebo_ros_gps_sensor.so
/opt/ros/humble/lib/libgazebo_ros_hand_of_god.so
/opt/ros/humble/lib/libgazebo_ros_harness.so
/opt/ros/humble/lib/libgazebo_ros_imu_sensor.so
/opt/ros/humble/lib/libgazebo_ros_init.so
/opt/ros/humble/lib/libgazebo_ros_joint_pose_trajectory.so
/opt/ros/humble/lib/libgazebo_ros_joint_state_publisher.so
/opt/ros/humble/lib/libgazebo_ros_node.so
/opt/ros/humble/lib/libgazebo_ros_p3d.so
/opt/ros/humble/lib/libgazebo_ros_planar_move.so
/opt/ros/humble/lib/libgazebo_ros_projector.so
/opt/ros/humble/lib/libgazebo_ros_properties.so
/opt/ros/humble/lib/libgazebo_ros_ray_sensor.so
/opt/ros/humble/lib/libgazebo_ros_state.so
/opt/ros/humble/lib/libgazebo_ros_template.so
/opt/ros/humble/lib/libgazebo_ros_tricycle_drive.so
/opt/ros/humble/lib/libgazebo_ros_utils.so
/opt/ros/humble/lib/libgazebo_ros_vacuum_gripper.so
/opt/ros/humble/lib/libgazebo_ros_video.so
/opt/ros/humble/lib/libgazebo_ros_wheel_slip.so

运动控制插件+里程计odom

本节课通过配置两轮差速控制插件，让我们的机器人动起来

插件介绍：

Gazebo是一个独立于ROS的软件，对外提供了丰富的API可以使用，gazebo的插件按照用途大致可以分为两种：

用于控制的插件，通过插件可以控制机器人关节运动，可以进行位置、速度、力的控制，比如我们这节课的两轮差速控制器。
用于数据采集的插件，比如IMU传感器用于采集机器人的惯性，激光雷达用于采集机器人周围的点云信息。

当然上面两类插件功能也可以写到一个插件里，两轮差速插件（gazebo_ros_diff_drive）就是一个二合一加强版。（差速控制+odom）

两轮差速插件用于控制机器人轮子关节的位置变化，同时该插件还会获取轮子的位置以及速度的信息的反馈，根据反馈的位置信息结合运动学模型即可计算出当前机器人的位姿（里程计）。

两轮差速控制器可以将轮子的目标转速发送给Gazebo，并从Gazebo获取到实际的速度和位置。（注意：发送给Gazebo是目标速度，反馈回来的是实际速度。目标!=实际，比如轮子卡住了，无论你发什么目标速度，实际速度都是0。）

要想快速了解一个系统的功能，最直接的就是看系统的对外的输入和输出是什么？什么都不要说，看下图：

上图就是对gazebo_ros_diff_drive的输入和输出信息的总结，可以很直观的看到该插件主要输入控制指令，主要输出里程计信息。接着小鱼带你分别认识一下输入和输出两个部分。

这个插件需要配置一系列参数如下图：

不知道你是否还记得在第七章中，小鱼对两轮差速底盘的运动学正的介绍。如果要完成底盘的正逆解和里程计的推算就必须要知道轮子的直径和间距。

同时该插件还提供了一些可以控制输出的选项，因为是仿真，所以还要告诉插件轮子对应的joint名称等信息，这样就有了下面这个参数表格：

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 配置项 | 含义 | |:—|:—| | ros | ros相关配置，包含命名空间和话题重映射等 | | update_rate | 数据更新速率 | | left_joint | 左轮关节名称 | | right_joint | 右轮关节名称 | | wheel_separation | 左右轮子的间距 | | wheel_diameter | 轮子的直径 | | max_wheel_torque | 轮子最大的力矩 | | max_wheel_acceleration | 轮子最大的加速度 | | publish_odom | 是否发布里程计 | | publish_odom_tf | 是否发布里程计的tf开关 | | publish_wheel_tf | 是否发布轮子的tf数据开关 | | odometry_frame | 里程计的framed ID，最终体现在话题和TF上 | | robot_base_frame | 机器人的基础frame的ID |

控制指令：两轮差速控制器默认通过订阅话题cmd_vel来获取目标线速度和角速度。该话题的类型为：geometry_msgs/msg/Twist

该接口里主要是一些线速度和角速度都包含在x、y、z，代表坐标系的三个方向上的对应速度。 （详细的接口内容请看硬件平台章节）

两轮差速控制器收到这个话题数据后将其中的角速度和线速度转换上两个轮子的转动速度发送给Gazebo。

输出的信息：

里程计信息默认的输出话题为odom，其消息类型为：nav_msgs/msg/Odometry

其数据主要包含三个部分： （详细的接口内容请看硬件平台章节）

header，表示该消息发布的时间
pose，表示当前机器人位置和朝向
twist，表示当前机器人的线速度和角速度
数据中还包含一个covariance，其代表协方差矩阵，后面小鱼写篇文章来介绍下，这里只需了解其含义即可。

里程计TF信息也可以输出：设为true，订阅tf话题里你就可以看到像下面的msg，建议后面配置好后，手动修改下，对比区别

- header:
    stamp:
      sec: 6157
      nanosec: 907000000
    frame_id: odom
  child_frame_id: base_footprint
  transform:
    translation:
      x: 0.0005557960241049835
      y: -0.0007350446303238693
      z: 0.01599968753145574
    rotation:
      x: 4.691143395208505e-07
      y: 7.115496626557812e-06
      z: -0.018531475772549166
      w: 0.9998282774331005

轮子TF信息也可以输出：设为true，订阅tf话题里你就可以看到像下面的msg，建议后面配置好后，手动修改下，对比区别

- header:
    stamp:
      sec: 6157
      nanosec: 941000000
    frame_id: base_link
  child_frame_id: left_wheel_link
  transform:
    translation:
      x: -0.02
      y: 0.1
      z: -0.06
    rotation:
      x: 0.0
      y: 0.049519025127821005
      z: 0.0
      w: 0.9987731805321918
- header:
    stamp:
      sec: 6157
      nanosec: 941000000
    frame_id: base_link
  child_frame_id: right_wheel_link
  transform:
    translation:
      x: -0.02
      y: -0.1
      z: -0.06
    rotation:
      x: 0.0
      y: -0.0663387077034509
      z: 0.0
      w: 0.9977971616817898

咱们之前下载的那个鱼香ROS的fishbot的urdf已经包含了差速插件的内容，如下：

因为是给Gazebo的插件，所以在URDF中，我们需要使用<gazebo>进行配置，因为是要给gazebo配置插件，所有要在gazebo标签下添加plugin子插件。

  <gazebo>
    <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
          <ros>
            <namespace>/</namespace>
            <remapping>cmd_vel:=cmd_vel</remapping>
            <remapping>odom:=odom</remapping>
          </ros>
          <update_rate>30</update_rate>
          <!-- wheels -->
          <left_joint>left_wheel_joint</left_joint>
          <right_joint>right_wheel_joint</right_joint>
          <!-- kinematics -->
          <wheel_separation>0.2</wheel_separation>
          <wheel_diameter>0.065</wheel_diameter>
          <!-- limits -->
          <max_wheel_torque>20</max_wheel_torque>
          <max_wheel_acceleration>1.0</max_wheel_acceleration>
          <!-- output -->
          <publish_odom>true</publish_odom>
          <publish_odom_tf>true</publish_odom_tf>
          <publish_wheel_tf>true</publish_wheel_tf>
          <odometry_frame>odom</odometry_frame>
          <robot_base_frame>base_footprint</robot_base_frame>
      </plugin>
    </gazebo> 

编译一下：

colcon build
source install/setup.bash
ros2 launch fishbot_description gazebo.launch.py

然后可以看看下面这俩命令：

ros2 node list
ros2 topic list

可以看到了我们插件订阅的的/cmd_vel和发布的/odom了。

然后我们可以通过teleop-twist-keyboard节点发布cmd_vel来控制fishbot。

sudo apt install ros-humble-teleop-twist-keyboard

使用下方节点来控制

ros2 run teleop_twist_keyboard teleop_twist_keyboard

接着尝试使用来控制机器人运动

   U    I    O
   J    K    L
   M    <    >

点一下，你就能看到fishbot在Gazebo中飞速的移动。接着打开终端，打印一下odom话题和tf话题，移动机器人观察数据变化。

也可以使用rqt显示速度数据

rqt

选择Plugin->Visualization->Plot

在上方Topic输入/cmd_vel/linear/x，再输入/cmd_vel/angular/z，然后用键盘控制机器人移动。

也可以在rviz2里看odom

rviz2

点键盘控制节点按U，让机器人转圈。

虽然机器人的轨迹已经在RVIZ中显示出来了，但是并没有机器人的模型，也看不到轮子的转动，咱们来带你一起解决这个问题。

前面咱们介绍过，要发布机器人模型我们所使用的节点是robot_state_publisher,所以我们在gazebo.launch.py中加入这个节点，同时再加上rviz2的启动节点，最终的gazebo.launch.py内容如下：

from launch import LaunchDescription
from launch_ros.actions import Node

# 封装终端指令相关类
from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
# from launch.actions import DeclareLaunchArgument
# from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    robot_name_in_model = 'fishbot'
    package_name = 'fishbot_description'
    urdf_name = "fishbot_gazebo.urdf"

    pkg_share = get_package_share_directory(f"{package_name}")
    urdf_model_path = os.path.join(pkg_share, f'urdf/urdf/{urdf_name}')
    # gazebo_world_path = os.path.join(pkg_share, 'world/fishbot.world')

    # Start Gazebo server
    # start_gazebo_cmd = ExecuteProcess(
    #     cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so',gazebo_world_path],
    #     output='screen')
    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
        output='screen')
        
    # Launch the robot
    spawn_entity_cmd = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=['-entity', robot_name_in_model,  '-file', urdf_model_path ], output='screen')
    
    # Start Robot State publisher
    start_robot_state_publisher_cmd = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        arguments=[urdf_model_path]
    )

    # Launch RViz
    start_rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        # arguments=['-d', default_rviz_config_path]
        )

    return LaunchDescription([start_gazebo_cmd,spawn_entity_cmd,start_robot_state_publisher_cmd,start_rviz_cmd])

保存编译启动

colcon build
ros2 launch fishbot_description gazebo.launch.py

这样rviz2里就有模型了。

可以保存下rviz2的配置

然后在launch里添加上rviz2的路径配置：

    default_rviz_config_path = os.path.join(pkg_share, 'rviz/rviz2.rviz')

    # Launch RViz
    start_rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', default_rviz_config_path]
        )

如下为全部的launch：

from launch import LaunchDescription
from launch_ros.actions import Node

# 封装终端指令相关类
from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
# from launch.actions import DeclareLaunchArgument
# from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    robot_name_in_model = 'fishbot'
    package_name = 'fishbot_description'
    urdf_name = "fishbot_gazebo.urdf"

    pkg_share = get_package_share_directory(f"{package_name}")
    urdf_model_path = os.path.join(pkg_share, f'urdf/urdf/{urdf_name}')
    # gazebo_world_path = os.path.join(pkg_share, 'world/fishbot.world')
    default_rviz_config_path = os.path.join(pkg_share, 'rviz/rviz2.rviz')

    # Start Gazebo server
    # start_gazebo_cmd = ExecuteProcess(
    #     cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so',gazebo_world_path],
    #     output='screen')
    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
        output='screen')
        
    # Launch the robot
    spawn_entity_cmd = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=['-entity', robot_name_in_model,  '-file', urdf_model_path ], output='screen')
    
    # Start Robot State publisher
    start_robot_state_publisher_cmd = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        arguments=[urdf_model_path]
    )

    # Launch RViz
    start_rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', default_rviz_config_path]
        )

    return LaunchDescription([start_gazebo_cmd,spawn_entity_cmd,start_robot_state_publisher_cmd,start_rviz_cmd])

可以自行编译测试。

惯性计IMU

上节课通过配置两轮差速控制器我们已经成功的让fishbot在gazebo中动了起来，本节课我们通过给fishbot的URDF配置IMU传感器插件，让IMU模块工作起来。

惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。在导航中有着很重要的应用价值。

上面这段话是小鱼从百科中摘抄出来的，你需要知道的一个关键点是IMU可以测量以下三组数据：

三维加速度计加速度
三维陀螺仪角速度
三维磁力计（有的也没有磁力计）

用六轴、九轴算法或其他算法等可以输出三轴欧拉角（Yaw,Pitch,Roll），欧拉角可以转化为四元数。

IMU长啥样？直接线下找控制组要就行，他们经常会玩这个。

便宜的长这样（MPU6050,MPU9050等）：

MPU6050是六轴的，MPU9050是九轴的。

贵的长这样（HWT101CT，HWT605等）：

其中HWT101CT是三轴的，HWT605是六轴的。（各有优缺点）

不要钱的长什么样？

仿真的不要钱哈哈，接着我们来配置一下仿真的IMU。

IMU对应的消息类型为sensor_msgs/msg/Imu

ROS的imu信息只有三轴加速度，角速度和四元数。（并没有磁力计和欧拉角，磁力计并非必须要填的，而欧拉角和四元数可以互相转换，四元数更好被算法运算，所以选择用四元数）

具体imu接口请看硬件平台章节。

可以看到除了每个数据对应的三个协方差之外，每一个还都对应一个3*3的协方差矩阵。

有了上节课的经验，我们可以很轻松的添加IMU传感器，但是还有一个需要注意的地方，为了更真实的模拟IMU传感器，我们需要给我们的仿真IMU传感器加点料。

加什么？加点高斯噪声，高斯噪声只需要指定平均值和标准差两个参数即可，不过因为IMU传感器的特殊性，我们还需要给模型添加两个偏差参数，分别是 平均值偏差和标准差偏差。

有关Gazebo仿真和噪声模型更深入的介绍可以参考鱼香ROS发的两篇推文：

下面是IMU传感器的URDF配置代码，大家结合文章对应可以理解一下，IMU对应的插件库libgazebo_ros_imu_sensor.so：

    <gazebo reference="imu_link">
      <sensor name="imu_sensor" type="imu">
      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
          <ros>
            <namespace>/</namespace>
            <remapping>~/out:=imu</remapping>
          </ros>
          <initial_orientation_as_reference>false</initial_orientation_as_reference>
        </plugin>
        <always_on>true</always_on>
        <update_rate>100</update_rate>
        <visualize>true</visualize>
        <imu>
          <angular_velocity>
            <x>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
              </noise>
            </x>
            <y>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
              </noise>
            </y>
            <z>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
              </noise>
            </z>
          </angular_velocity>
          <linear_acceleration>
            <x>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>1.7e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
              </noise>
            </x>
            <y>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>1.7e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
              </noise>
            </y>
            <z>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>1.7e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
              </noise>
            </z>
          </linear_acceleration>
        </imu>
      </sensor>
    </gazebo>

我们之前下载的fishbot已经包含该内容了，所以不用再添加了，直接运行即可。

ros2 launch fishbot_description gazebo.launch.py

ros2 topic list

ros2 topic info /imu
ros2 topic echo /imu

header:
  stamp:
    sec: 150
    nanosec: 599000000
  frame_id: base_footprint
orientation:
  x: 3.434713830866392e-07
  y: 7.119913105768616e-06
  z: -0.00028312437320413914
  w: 0.9999999598948884
orientation_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
  x: -0.00013597855247901325
  y: 0.0006306135617081868
  z: -0.00015794894627685146
angular_velocity_covariance:
- 4.0e-08
- 0.0
- 0.0
- 0.0
- 4.0e-08
- 0.0
- 0.0
- 0.0
- 4.0e-08
linear_acceleration:
  x: 0.08679200038530369
  y: 0.07753419258567491
  z: 9.687910969061628
linear_acceleration_covariance:
- 0.00028900000000000003
- 0.0
- 0.0
- 0.0
- 0.00028900000000000003
- 0.0
- 0.0
- 0.0
- 0.00028900000000000003

用rqt可视化：

雷达Laser

本节我们来认识一个新的传感器，该传感器在自动驾驶、室内导航等应用非常多，比如扫地机器人上就是用的它作为感知环境的重要工具，该传感器是激光雷达。

激光雷达（Light Detection And Ranging）,缩写LiDAR，英文也叫laser,翻译一下叫——激光探测与测距。

激光雷达的原理也很简单，就像蝙蝠的定位方法一样，蝙蝠定位大家都知道吧，像下面这样子的回声定位。

普通的单线激光雷达一般有一个发射器，一个接收器，发射器发出激光射线到前方的目标上，物品会将激光反射回来，然后激光雷达的接受器可以检测到反射的激光。

通过计算发送和反馈之间的时间间隔，乘上激光的速度，就可以计算出激光飞行的距离，该计算方法成为TOF（飞行时间法Time of flight，也称时差法）。

除了TOF之外还有其他方法进行测距，比如三角法，这里就不拓展了放一篇文章，大家自行阅读。激光三角测距原理详述

目前市面上的激光雷达，几乎都是采用三角测距，比如思岚的：

需要注意的是虽然只有一个发射器和一个接受器，激光雷达通过电机可以进行旋转，这样就可以达到对周围环境360度测距的目的。

五位数的长这样：

四位数的长这样（咱们有一台）

三位数的长这样（咱们也有）

两位数的长这样

不要钱的长这样

仿真的，不要钱

因为激光雷达是属于射线类传感器，该类传感在在Gazebo插件中都被封装成了一个动态库libgazebo_ros_ray_sensor.so。

接着我们来看看LiDAR的话题消息接口sensor_msgs/msg/LaserScan。

雷达的数据结构有些复杂，但通过注释和名字相信你可以看的七七八八，看不懂也没关系，一般情况下我们不会直接的对雷达的数据做操作。

雷达的模型不需要collision，请删掉，否则会挡激光射出。

有了前面的经验，我们需要在URDF添加以下内容即可，但我们下载的是鱼香ROS添加好的，所以不用改了：

  <gazebo reference="laser_link">
      <sensor name="laser_sensor" type="ray">
      <always_on>true</always_on>
      <visualize>true</visualize>
      <update_rate>10</update_rate>
      <pose>0 0 0.075 0 0 0</pose>
      <ray>
          <scan>
            <horizontal>
              <samples>360</samples>
              <resolution>1.000000</resolution>
              <min_angle>0.000000</min_angle>
              <max_angle>6.280000</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>0.120000</min>
            <max>3.5</max>
            <resolution>0.015000</resolution>
          </range>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.01</stddev>
          </noise>
      </ray>

      <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
        <ros>
          <remapping>~/out:=scan</remapping>
        </ros>
        <output_type>sensor_msgs/LaserScan</output_type>
        <frame_name>laser_link</frame_name>
      </plugin>
      </sensor>
    </gazebo>

可以看到:

雷达也可以设置更新频率update_rate，这里设置为5
雷达可以设置分辨率，设置为1，采样数量360个，最终生成的点云数量就是360
雷达也有噪声，模型为gaussian
雷达有扫描范围range，这里配置成0.12-3.5，0.015分辨率
雷达的pose就是雷达的joint中位置的设置值

下面这个蓝色的就是激光雷达的光线覆盖范围：

ros2 topic list

ros2 topic info /scan
ros2 topic echo /scan

接着我们尝试使用rviz2进行可视化激光雷达数据

添加和修改RVIZ2的如下：（通过LaserScan插件可以看到激光数据）

相信你改完之后依然是看不到任何激光雷达的数据的，反看topic的echo出来的数据，不是0就是inf(无限大)，再看看gazebo你会发现，激光雷达并没有达到任何一个物体上。

所以我们可以手动的给激光雷达周围添加一下东西，点击Gazebo工具栏的正方体，圆球或者圆柱，随意放置几个到我们激光雷达的最大扫描半径内。

超声波Ultrasonic

这玩意对于ROS2算法意义不是很大，控制组直接在MCU上实现即可，有需求可以学本节。 （可以跳过本节）

在实际的机器人开发过程中，我们可能会利用超声波传感器实现实时避障的功能，毕竟超声波的价格相较于激光雷达要便宜很多（便宜的几块钱）。

所以本节我们来说一下如何使用ROS2+Gazebo来仿真超声波传感器。

百科来一段：

超声波传感器是将超声波信号转换成其它能量信号（通常是电信号）的传感器。超声波是振动频率高于20kHz的机械波。它具有频率高、波长短、绕射现象小，特别是方向性好、能够成为射线而定向传播等特点。超声波对液体、固体的穿透本领很大，尤其是在阳光不透明的固体中。超声波碰到杂质或分界面会产生显著反射形成反射回波，碰到活动物体能产生多普勒效应。超声波传感器广泛应用在工业、国防、生物医学等方面。

接着看看长什么样子：

便宜的就长这样子，一共两个头，一个头用于发送波，一个头接收波。这个还稍微高级一点，带一个光敏电阻，可以为超声波数据做一些补偿。

超声波传感器原理是什么呢？

距离=(发送时间-接收时间)*速度/2Copy to clipboardErrorCopied

看了超声波的原理，你有没有发现和前面的激光雷达传感器是一样的，是的，所以超声波传感器插件和激光雷达传感器插件在Gazebo插件中是同一个：

libgazebo_ros_ray_sensor.so

超声波总要装在机器人身上某个位置，所以我们先添加一个关节和Joint，为了省事，link我们就只写个名字，你如果有需要可以按照前面的章节那样添加一下。

  <link name="ultrasonic_sensor_link" />

  <joint name="ultrasonic_sensor_joint" type="fixed">
    <parent link="base_link"/>
    <child link="ultrasonic_sensor_link"/>
    <origin xyz="0.07 0.0 0.076" rpy="0 0 0"/>
  </joint>

添加完了关节，我们就可以配置gazebo的插件了，gazebo插件配置如下

  <gazebo reference="ultrasonic_sensor_link">
    <sensor type="ray" name="ultrasonic_sensor">
      <pose>0 0 0 0 0 0</pose>
      <!-- 是否可视化，gazebo里能不能看到 -->
      <visualize>true</visualize>
      <!-- 扫描速率，也就是数据更新速率 -->
      <update_rate>5</update_rate>
      <ray>
        <scan>
          <!-- 水平扫描的点数 -->
          <horizontal>
            <samples>5</samples>
            <resolution>1</resolution>
            <min_angle>-0.12</min_angle>
            <max_angle>0.12</max_angle>
          </horizontal>
          <!-- 垂直方向扫描的点数 -->
          <vertical>
            <samples>5</samples>
            <resolution>1</resolution>
            <min_angle>-0.01</min_angle>
            <max_angle>0.01</max_angle>
          </vertical>
        </scan>
        <!-- 超声波检测的范围和数据分辨率单位m -->
        <range>
          <min>0.02</min>
          <max>4</max>
          <resolution>0.01</resolution>
        </range>
        <!-- 数据噪声采用高斯噪声 -->
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="ultrasonic_sensor_controller" filename="libgazebo_ros_ray_sensor.so">
        <ros>
          <!-- 重映射输出的话题名称 -->
          <remapping>~/out:=ultrasonic_sensor_1</remapping>
        </ros>
        <!-- 输出消息的类型，注意与雷达区分，这里是sensor_msgs/Range -->
        <output_type>sensor_msgs/Range</output_type>
        <!-- 射线类型，这里要写ultrasound，注意和雷达区分 -->
        <radiation_type>ultrasound</radiation_type>
        <!-- frame名称，填写link名称即可 -->
        <frame_name>ultrasonic_sensor_link</frame_name>
      </plugin>
    </sensor>
  </gazebo>

添加完成后就可以编译测试下代码

colcon build --packages-select fishbot_description
source install/setup.bash
ros2 launch fishbot_description gazebo.launch.py

没有物体的前面可以放个东西

打开终端，输入下面指令

ros2 topic list 
ros2 topic info /ultrasonic_sensor_1
ros2 topic echo /ultrasonic_sensor_1Copy to clipboardErrorCopied

不出意外可以看到下面的数据

header:
  stamp:
    sec: 4458
    nanosec: 1000000
  frame_id: ultrasonic_sensor_link
radiation_type: 0
field_of_view: 0.23999999463558197
min_range: 0.019999999552965164
max_range: 4.0
range: 2.6798219680786133

这里的range就是fishbot到墙之间的距离：2.67982

我们来讲一讲超声波传感器的数据类型sensor_msgs/msg/Range

# ros2 topic info /ultrasonic_sensor_1
Type: sensor_msgs/msg/Range
Publisher count: 1
Subscription count: 0

你可以使用ros2 interface show sensor_msgs/msg/Range看到详细的解释，我们翻译一下

# Single range reading from an active ranger that emits energy and reports
# one range reading that is valid along an arc at the distance measured.
# This message is  not appropriate for laser scanners. See the LaserScan
# message if you are working with a laser scanner.
#
# This message also can represent a fixed-distance (binary) ranger.  This
# sensor will have min_range===max_range===distance of detection.
# These sensors follow REP 117 and will output -Inf if the object is detected
# and +Inf if the object is outside of the detection range.

std_msgs/Header header # timestamp in the header is the time the ranger
                             # returned the distance reading

# Radiation type enums
# If you want a value added to this list, send an email to the ros-users list
uint8 ULTRASOUND=0
uint8 INFRARED=1

uint8 radiation_type    # 传感器射线类型
                        # (sound, IR, etc) [enum]

float32 field_of_view   # 距离数据对应的弧[rad]的大小，测量物体的范围介于         
                        # -field_of_view/2 到 field_of_view/2 之间。
                        # 0 角度对应于传感器的 x 轴。

float32 min_range       # 最小范围值 [m]
float32 max_range       # 最大范围值 [m]
                        #  固定距离需要 min_range==max_range

float32 range           # 范围数据 [m]
                        # (Note: values < range_min or > range_max should be discarded)
                        # Fixed distance rangers only output -Inf or +Inf.
                        # -Inf represents a detection within fixed distance.
                        # (Detection too close to the sensor to quantify)
                        # +Inf represents no detection within the fixed distance.
                        # (Object out of range)

结论，主要关注range就可以了。

在rviz2里添加超声波数据

Add ->By topic->Range

搭建世界地图

本节我们要在Gazebo中建立一个测试的环境，其实也很简单，利用Gazebo的画墙工具即可完成。

world即世界，gazebo的world文件就是用于描述世界模型的，也就是环境模型。

Gazebo已经为我们准备了很多常用的物体模型，除了基础的圆球，圆柱，立方体外的，其实还有飞机、汽车、房子等你现实中无法拥有的。

但是一开始安装Gazebo的时候并不会帮你下载好这些模型，需要我们手动下载，找一个你要存模型的文件夹，打开终端，复制粘贴下面这句

git clone https://github.com/osrf/gazebo_models

并把存放模型的路径加到~/.bashrc里（第二个冒号后面的可以不写，第二个冒号后面的是多个路径。）

GAZEBO_MODEL_PATH是老版Gazebo Classic的宏。

IGN_GAZEBO_RESOURCE_PATH是新版Igntion Gazebo的宏。

模型都通用，可以一起配置上。

刷新环境变量

source ~/.bashrc

此时再次打开终端，输入gazebo，把选项卡切换到Insert

在Insert选项卡下可以看到一个目录，以及目录下的模型名称，随着下载脚本的不断下载，这里的模型会越来越多。

随手拖几个，搭建一个漂亮的环境出来~

每个成功的男人都有一辆车，咱们也不例外

上面是Gazebo为我们准备好的开源模型，我们也可以通过Gazebo的工具来自己画一个环境。

然后也可以用墙壁工具建墙

Gazebo左上角->Edit->Building Editor

接着可以看到这样一个编辑界面

点击左边的Wall,你就可以在上方的白色区域进行建墙了，这个和模拟人生游戏不一样，这个是画二维的墙生成三维的墙，模拟人生是直接在三维里画。

建完后还可以用选Add Color或者Add Texture，然后点击下方墙，给墙添加颜色或者纹理。

首先你要有一个地图，小鱼为你准备了两个，两个图片都是800*600像素的。

打开Gazebo->Gazebo左上角->Edit->Building Editor->左下方选Import

将上面两个图片存到本地，在这个界面选图片，记着选Next

左边选尺寸对应关系

我们选择默认的，100像素/米。点击OK（需要手动将100改变一下才能点击OK哦），之后就可以用图片画墙了。

注意：导入完图片不会直接出来墙，图片只是提供了墙的大概位置，需要你手动用墙再将边描一遍。

建完后点击File->Exit,在退出的弹框中选Exit。

接着在Gazebo界面中就可以看到墙了，我们再手动添加几个物体，就可以用于下面的导航使用了。

添加完，接着点击File->SaveWorld，将文件保存到我们的fishbot_descrption的world下。

没有world目录的小伙伴可以先手动创建下

加载world其实也很简单，可以先启动Gazebo，再手动的加载文件，也可以在Gazebo启动时加载：

比如在前面加载ROS2插件基础上再加载fishbot.world。

gazebo --verbose  -s libgazebo_ros_init.so -s  libgazebo_ros_factory.so 你的world文件目录/fishbot.world

修改launch文件，将上面的命令行写到gazebo.launch.py中即可。

    gazebo_world_path = os.path.join(pkg_share, 'world/fishbot.world')

    # Start Gazebo server
    start_gazebo_cmd =  ExecuteProcess(
        cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', gazebo_world_path],
        output='screen')

下面是整个launch文件：

from launch import LaunchDescription
from launch_ros.actions import Node

# 封装终端指令相关类
from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
# from launch.actions import DeclareLaunchArgument
# from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    robot_name_in_model = 'fishbot'
    package_name = 'fishbot_description'
    urdf_name = "fishbot_gazebo.urdf"
    world_name = "fishbot.world"

    pkg_share = get_package_share_directory(f"{package_name}")
    urdf_model_path = os.path.join(pkg_share, f'urdf/urdf/{urdf_name}')
    gazebo_world_path = os.path.join(pkg_share, f'world/{world_name}')
    default_rviz_config_path = os.path.join(pkg_share, 'rviz/rviz2.rviz')

    # Start Gazebo server
    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so',gazebo_world_path],
        output='screen')
        
    # Launch the robot
    spawn_entity_cmd = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=['-entity', robot_name_in_model,  '-file', urdf_model_path ], output='screen')
    
    # Start Robot State publisher
    start_robot_state_publisher_cmd = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        arguments=[urdf_model_path]
    )

    # Launch RViz
    start_rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', default_rviz_config_path]
        )

    return LaunchDescription([start_gazebo_cmd,spawn_entity_cmd,start_robot_state_publisher_cmd,start_rviz_cmd])

最后记得修改cmakelists.txt文件，让编译后将world文件拷贝到install目录下

colcon build
source install/setup.bash
ros2 launch fishbot_description gazebo.launch.py 

框架优化

本节代码学长也发到仓库了，有需要的学弟学妹请看:https://github.com/tungchiahui/ROS_WS/tree/main/ROS2_WS%2F6.ws_simulations%2Fsrc%2Ffishbot_description

比如说支持xacro等优化。

首先先把原来的fishbot_gazebo.urdf里的内容分成好几个urdf和xacro再用一个总的fishbot.urdf.xacro去引用。（学过xacro的肯定都会）

优化后的launch如下：

from launch import LaunchDescription
from launch_ros.actions import Node

# 封装终端指令相关类
from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
# 文件包含相关
# from launch.actions import IncludeLaunchDescription
# from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关
# from launch.event_handlers import OnProcessStart,OnProcessExit
# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command,LaunchConfiguration
import os

def generate_launch_description():
    robot_name_in_model = 'fishbot'
    package_name = 'fishbot_description'
    # urdf_xacro_name = "fishbot_gazebo.urdf"
    world_name = "fishbot.world"

    pkg_share = get_package_share_directory(f"{package_name}")
    urdf_xacro_model_path = os.path.join(pkg_share, "urdf/xacro","fishbot.urdf.xacro")
    gazebo_world_path = os.path.join(pkg_share, f'world/{world_name}')
    default_rviz_config_path = os.path.join(pkg_share, 'rviz/rviz2.rviz')

    model = DeclareLaunchArgument(name="model", default_value=urdf_xacro_model_path)
    robot_description = ParameterValue(Command(["xacro ",LaunchConfiguration("model")]))

    # Start Gazebo server
    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so',gazebo_world_path],
        output='screen')
        
    # Launch the robot
    spawn_entity_cmd = Node(
        package='gazebo_ros', 
        executable='spawn_entity.py',
        arguments=['-entity', robot_name_in_model,  '-topic', '/robot_description' ], output='screen')
    
    # Start Robot State publisher
    start_robot_state_publisher_cmd = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{"robot_description": robot_description}]
    )

    # Launch RViz
    start_rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', default_rviz_config_path]
        )

    return LaunchDescription([model,start_gazebo_cmd,spawn_entity_cmd,start_robot_state_publisher_cmd,start_rviz_cmd])

colcon build
source install/setup.bash
ros2 launch fishbot_description gazebo.launch.py

然后就可以去搞导航啦！也可以接着学新版Ignition Gazebo，但这东西教程巨少，截止2024年只有赵虚左老师讲了。

Ignition Gazebo
Ign Gazebo安装与运行

Gazebo每个版本的变化都很大。

特别是ROS1用的老版Gazebo(黑色界面)和ROS2用的新版Gazebo(白色界面)。

ROS2的不同版本的Gazebo跨度也很大，比如Humble和Jazzy及Jazzy之后的版本之间很多标签区别很大。

本文使用humble版本(即Ignition Gazebo)当做教程。

当然，为了兼容以后的Gazebo，在下方也会有教程教你如何从ign gazebo迁移到gazebo sim（最最最新版gazebo）。

Ignition Gazebo 是 ROS2 中使用的全新机器人仿真工具，它是 Gazebo 的升级版本。在Humble他还叫Ignition Gazebo(也叫Gazebo Fortress)，在Jazzy中叫Gazebo Harmonic(去掉了Ignition的名字)(https://community.gazebosim.org/t/a-new-era-for-gazebo/1356)。它具备更好的性能和可用性，并通过紧密集成 ROS2 来提供强大的仿真环境。Ignition Gazebo 支持各种机器人平台和传感器，并提供灵活的配置选项和易于使用的界面。它的物理引擎和传感器模型可以帮助开发人员进行机器人系统的开发、测试和验证。无论是研究还是教育，Ignition Gazebo 都是一个强大的工具。

如果想从Ignition Gazebo(ROS2 Humble)迁移到Gazebo(ROS2 Jazzy)，请往下翻翻，下面有一节是讲如何迁移的。

https://docs.ros.org/en/humble/Tutorials/Advanced/Simulators/Gazebo/Gazebo.html

下面这个网站是官方教程(ROS2 Humble的Ignition Gazebo Fortress)：

https://gazebosim.org/docs/fortress/getstarted/

https://gazebosim.org/docs/fortress/library_reference_nav/

源码：https://github.com/gazebosim/docs/blob/master/fortress/tutorials

安装

Ignition Gazebo 是不依赖于ROS2的一个独立的项目，可以独自安装。但是如果安装了ROS2，在ROS2存储库中已经集成了对应版本的 Ignition Gazebo，可以调用如下指令直接安装：

# 通用命令
sudo apt install ros-${ROS_DISTRO}-ros-gz

# Humble版本
sudo apt install ros-humble-ros-gz

# Jazzy版本
sudo apt install ros-jazzy-ros-gz

运行

Ignition Gazebo 安装完毕之后，可以通过两种方式启动。

方式1，以Ignition Gazebo 的方式启动，指令如下：

# Humble版本
ign gazebo

# Jazzy版本
gz sim

方式2，以ROS2的方式启动，指令如下：

ros2 launch ros_gz_sim gz_sim.launch.py

二者运行结果一致，如下图所示：在弹出窗口中，选择仿真环境然后点击run按钮即可运行。

界面介绍

接下来以Empty仿真环境为例，介绍一下Ignition Gazebo的界面组成。

注意：如果你的Gazebo不卡，但是Ignition Gazebo巨卡的话，请确认Ignition Gazebo是以独显打开的，而不是核显。

如果不会切换应用显卡，可以直接把核显关闭掉，从混合输出切换为独立显卡输出。

工具栏

顶部的工具栏包含两个按钮，左侧的文件菜单按钮（水平条纹）和右侧的插件按钮（垂直省略号）。

文件菜单按钮（水平条纹）

文件菜单按钮包含将仿真环境保存到文件、保存和加载界面配置以及自定义界面样式等设置。

右侧的插件按钮（垂直省略号）

插件按钮列出了所有可用的插件。点击后会弹出插件列表，向下滚动此列表以查看所有插件。当选择一个时，其界面将出现在右侧面板中。

3D视窗

左上方工具栏包含多种几何体（球体、框体、圆柱体）按钮和变换控件。通过集合体按钮可以直接将盒子、球体或圆柱体模型插入仿真环境。只需单击要插入的形状，然后将其放入环境中。该形状将自动捕捉到地平面上。

主视图会显示仿真环境，我们可以通过鼠标以不同方式来导航场景，相关操作如下：

左键单击：选择实体
右键单击：打开带有选项的菜单：
   Move to：移动到以实体为中心的场景
   Follow：选择一个实体让视图保持居中，无论是移动还是平移
   Remove：从模拟中删除实体
   Copy：复制实体
   Past: 粘贴实体
   View：显示实体的重心（Center of Mass）、碰撞边界（Collisions）、惯性（Inertia）、
         关节（Joints）、坐标系（Frames）、透明度（Transparent）、线框（Wireframe）等属性
左键单击并拖动：在场景中平移
右键单击并拖动：放大和缩小
滚轮向前/向后：放大和缩小
滚轮单击并拖动：旋转场景

想移动这个球，需要点左上角的移动模式，再左键单击选中物体

在视窗的底部，从左到右分别是是播放、步长按钮和实时因子（Real-Time Factor，RTF）。点击播放按钮将开始运行仿真环境，再次点击可以暂停运行。步长按钮用于设置仿真时间的离散单位，可以通过将鼠标悬停在按钮上来自定义步长。实时因子表示仿真运行速度相对于真实时间的比例。

右侧面板

右侧面板用于显示插件，当前仿真环境默认包含两个插件：Model和Entity Tree。

Entity Tree 中会显示仿真环境中的实体列表；
点击Entity Tree中的实体后，可以在Model中显示该实体的相关信息。
也可以按住 Ctrl 并单击以选择多个实体；
还可以右键单击任何插件以打开基本设置或关闭。

在Ignition Gazebo中内置了许多插件，可以点击工具栏的右侧按钮自行添加，比如：可以选择 Grid Config 插件调整世界网格的特征，包括单元格大小、网格位置、单元格计数、或颜色等。

后期随着应用的深入，也会陆续介绍其他一些插件。

与ROS2集成

本节将介绍如何实现Ignition Gazebo与ROS2的集成，以实现二者之间的交互，比如，可以通过ROS2的键盘控制节点控制机器人运动，并且在rviz2中显示机器人的里程计(odom)数据。其流程大致如下：

启动 Ignition Gazebo 仿真环境；
通过 ros_gz_bridge 建立 ROS2 与 Ignition Gazebo 的连接；
启动 ROS2 相关节点实现与 Ignition Gazebo 的数据收发。

Ignition Gazebo与ROS2的的所有集成实现，基本都遵循上述流程。

启动仿真环境

在 Ignition Gazebo 安装时，已经内置了一些仿真环境，直接启动即可。在此我们可以使用名为visualize_lidar.sdf的仿真文件，该文件对应的仿真环境中包括了差速机器人以及激光雷达的仿真。启动指令如下：

ign gazebo -v 4 -r visualize_lidar.sdf
#或者
gz sim -v 4 -r visualize_lidar.sdf

或者也可以以ROS2 launch的方式启动，指令如下：

ros2 launch ros_gz_sim gz_sim.launch.py gz_args:="-v 4 -r visualize_lidar.sdf" # 启动状态

两种方式本质相同，都是启动了Ignition Gazebo并且加载了visualize_lidar.sdf文件。

建立连接

虽然仿真环境中的机器人已经配置了运动控制插件，可以通过/model/vehicle_blue/cmd_vel话题订阅速度指令并运动，但是Ignition Gazebo与ROS2中的消息格式并不一致，所以还需要通过ros_gz_bridge这一桥接功能包，实现二者之间消息的转换，调用指令如下：

ros2 run ros_gz_bridge parameter_bridge /model/vehicle_blue/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist

通过该指令可以将发布在/model/vehicle_blue/cmd_vel话题上的geometry_msgs/msg/Twist类型的ROS2消息转换成可以被Ignition Gzebo识别的gz.msgs.Twist类型的消息。

启动ROS2节点

启动ROS2的键盘控制节点，并将话题重映射为/model/vehicle_blue/cmd_vel，指令如下：

ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/model/vehicle_blue/cmd_vel

接下来就可以使用键盘控制机器人运动了。

ros_gz_bridge

ros_gz_bridge是连接ROS2与Ignition Gazebo的桥梁，ROS2与Ignition Gazebo使用的消息并不兼容，必须通过ros_gz_bridge进行转换。

ros_gz_bridge使用语法

ROS2与Ignition Gazebo的桥接是通过ros_gz_bridge包中的parameter_bridge节点实现，其使用语法如下：

parameter_bridge [<topic@ROS2_type@Ign_type> ..]  [<service@ROS2_srv_type[@Ign_req_type@Ign_rep_type]> ..]

在话题Topic中， 第一个@ 符号是话题名称和消息类型的 分隔符 。

第一个@符号后面是ROS消息类型。

ROS消息类型后面是@、[或]符号：

@ 表示双向桥接；
[ 表示从Ignition Gazebo到ROS的桥接；
] 表示从ROS到Ignition Gazebo的桥接。

方向符号后是Gazebo Transport消息类型。

（两个@不是同一个含义）

在服务Service中， 第一个@ 符号是服务名称和类型的 分隔符 。

第一个@符号后面是ROS服务类型。可以选择地包括Gazebo请求和响应类型，在它们之间用@符号分隔。

仅支持将Gazebo服务公开为ROS服务，即ROS服务将请求转发到Gazebo服务，然后将响应转发回ROS客户端。

双向桥接示例：

parameter_bridge /chatter@std_msgs/String@gz.msgs.StringMsg

从Gazebo到ROS的桥接示例：

parameter_bridge /chatter@std_msgs/String[gz.msgs.StringMsg

从ROS到Gazebo的桥接示例：

parameter_bridge /chatter@std_msgs/String]gz.msgs.StringMsg

服务桥接示例：

parameter_bridge /world/default/control@ros_gz_interfaces/srv/ControlWorld
或者：
parameter_bridge /world/default/control@ros_gz_interfaces/srv/ControlWorld@gz.msgs.WorldControl@gz.msgs.Boolean

也可以运行ros2 run ros_gz_bridge parameter_bridge -h指令查看官方说明文档。

ros_gz_bridge支持的消息类型

以下是ROS2与Ignition Gazebo中话题消息类型对应表：

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | ROS2消息类型 | Gazebo Transport 类型 | |:—|:—| | builtin_interfaces/msg/Time | gz.msgs.Time | | geometry_msgs/msg/Point | gz.msgs.Vector3d | | geometry_msgs/msg/Pose | gz.msgs.Pose | | geometry_msgs/msg/PoseArray | gz.msgs.Pose_V | | geometry_msgs/msg/PoseStamped | gz.msgs.Pose | | geometry_msgs/msg/PoseWithCovariance | gz.msgs.PoseWithCovariance | | geometry_msgs/msg/Quaternion | gz.msgs.Quaternion | | geometry_msgs/msg/Transform | gz.msgs.Pose | | geometry_msgs/msg/TransformStamped | gz.msgs.Pose | | geometry_msgs/msg/Twist | gz.msgs.Twist | | geometry_msgs/msg/TwistWithCovariance | gz.msgs.TwistWithCovariance | | geometry_msgs/msg/TwistWithCovarianceStamped | gz.msgs.TwistWithCovariance | | geometry_msgs/msg/Vector3 | gz.msgs.Vector3d | | geometry_msgs/msg/Wrench | gz.msgs.Wrench | | geometry_msgs/msg/WrenchStamped | gz.msgs.Wrench | | nav_msgs/msg/Odometry | gz.msgs.Odometry | | nav_msgs/msg/Odometry | gz.msgs.OdometryWithCovariance | | rcl_interfaces/msg/ParameterValue | gz.msgs.Any | | ros_gz_interfaces/msg/Altimeter | gz.msgs.Altimeter | | ros_gz_interfaces/msg/Contact | gz.msgs.Contact | | ros_gz_interfaces/msg/Contacts | gz.msgs.Contacts | | ros_gz_interfaces/msg/Dataframe | gz.msgs.Dataframe | | ros_gz_interfaces/msg/Entity | gz.msgs.Entity | | ros_gz_interfaces/msg/Float32Array | gz.msgs.Float_V | | ros_gz_interfaces/msg/GuiCamera | gz.msgs.GUICamera | | ros_gz_interfaces/msg/JointWrench | gz.msgs.JointWrench | | ros_gz_interfaces/msg/Light | gz.msgs.Light | | ros_gz_interfaces/msg/SensorNoise | gz.msgs.SensorNoise | | ros_gz_interfaces/msg/StringVec | gz.msgs.StringMsg_V | | ros_gz_interfaces/msg/TrackVisual | gz.msgs.TrackVisual | | ros_gz_interfaces/msg/VideoRecord | gz.msgs.VideoRecord | | ros_gz_interfaces/msg/WorldControl | gz.msgs.WorldControl | | rosgraph_msgs/msg/Clock* | gz.msgs.Clock* | | sensor_msgs/msg/BatteryState | gz.msgs.BatteryState | | sensor_msgs/msg/CameraInfo | gz.msgs.CameraInfo | | sensor_msgs/msg/FluidPressure | gz.msgs.FluidPressure | | sensor_msgs/msg/Image | gz.msgs.Image | | sensor_msgs/msg/Imu | gz.msgs.IMU | | sensor_msgs/msg/JointState | gz.msgs.Model | | sensor_msgs/msg/Joy | gz.msgs.Joy | | sensor_msgs/msg/LaserScan | gz.msgs.LaserScan | | sensor_msgs/msg/MagneticField | gz.msgs.Magnetometer | | sensor_msgs/msg/NavSatFix | gz.msgs.NavSat | | sensor_msgs/msg/PointCloud2 | gz.msgs.PointCloudPacked | | std_msgs/msg/Bool | gz.msgs.Boolean | | std_msgs/msg/ColorRGBA | gz.msgs.Color | | std_msgs/msg/Empty | gz.msgs.Empty | | std_msgs/msg/Float32 | gz.msgs.Float | | std_msgs/msg/Float64 | gz.msgs.Double | | std_msgs/msg/Header | gz.msgs.Header | | std_msgs/msg/Int32 | gz.msgs.Int32 | | std_msgs/msg/String | gz.msgs.StringMsg | | std_msgs/msg/UInt32 | gz.msgs.UInt32 | | tf2_msgs/msg/TFMessage | gz.msgs.Pose_V | | trajectory_msgs/msg/JointTrajectory | gz.msgs.JointTrajectory |

以及服务消息类型对应表：

与ROS2集成优化

在 Ignition Gazebo与ROS2集成 实现中需要在终端中使用不同的指令启动不同模块，该流程实现稍显复杂，本节将介绍如何以launch文件的方式进行优化。

新建功能包

请首先调用如下指令创建一个功能包：

ros2 pkg create demo_gazebo_sim

添加目录

在新建的功能包下添加目录： launch、rviz、world。并在CmakeLists.txt中添加如下代码：

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

launch目录用于存储launch文件，rviz目录由于存储rviz2的配置文件，而world目录则用于存储Ignition Gazebo仿真环境的相关文件。

rviz目录中生成rviz2的配置文件

启动 rviz2，直接将默认配置保存至当前功能包的rviz目录，保存文件命名为sim.rviz。

复制world文件

在ignition安装路径下的worlds目录（/usr/share/ignition/ignition-gazebo6/worlds）中复制visualize_lidar.sdf文件至world目录。

如果该路径下没有，那可能在ROS的安装路径下：

/opt/ros/jazzy/opt/gz_sim_vendor/share/gz/gz-sim8/worlds/

如果还没有的话，手动查找一下：

sudo find / -name "visualize_lidar.sdf"

编写launch文件

launch目录下新建launch文件gazebo_sim_demo.launch.py，并输入如下内容：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node

def generate_launch_description():

    this_pkg = get_package_share_directory('demo_gazebo_sim')
    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
    world_file = os.path.join(this_pkg,'world','visualize_lidar.sdf')

    gz_sim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
        launch_arguments={
            'gz_args': '-r ' + world_file
        }.items(),
    )

    # RViz
    rviz = Node(
       package='rviz2',
       executable='rviz2',
       arguments=['-d', os.path.join(this_pkg, 'rviz', 'sim.rviz')],
       condition=IfCondition(LaunchConfiguration('rviz'))
    )

    # Bridge
    bridge = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=['/model/vehicle_blue/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist',
                   '/model/vehicle_blue/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry',
                   '/model/vehicle_blue/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V',
                   ],
        parameters=[{'qos_overrides./model/vehicle_blue.subscriber.reliability': 'reliable'}],
        remappings=[
                ('/model/vehicle_blue/tf', '/tf'),
                ('/model/vehicle_blue/cmd_vel','cmd_vel')
            ],
        output='screen'
    )

    return LaunchDescription([
        gz_sim,
        DeclareLaunchArgument('rviz', default_value='true',
                              description='Open RViz.'),
        bridge,
        rviz
    ])

该launch文件中，启动了Ignition Gazebo仿真环境、通过ros_gz_bridge建立了仿真与ROS2的连接，并且启动了rviz2节点。其中建立连接时，实现了速度指令、里程计以及坐标变换等消息的转换。

构建

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

colcon build  --packages-select demo_gazebo_sim

执行

终端中进入当前工作空间，调用如下指令执行launch文件：

. install/setup.bash
ros2 launch demo_gazebo_sim gazebo_sim_demo.launch.py

新开终端，启动键盘控制节点：

ros2 run teleop_twist_keyboard teleop_twist_keyboard

再配置rviz2，将Fixed Frame设置为vehicle_blue/odom，添加TF插件，添加Odometry插件并将话题设置为/model/vehicle_blue/odometry，当通过键盘控制发送速度指令时，仿真环境的机器人开始运动，并且在rviz2中可以回显坐标变换以及里程计等消息。

仿真环境创建 SDF文件

前面几节内容我们使用的是Ignition Gazebo内置的仿真环境，本节开始将介绍如何自行搭建仿真环境。本节案例将仿真一个长10m宽5m的矩形房间。该案例可以先启动Ignition Gazebo以拖拽的方式搭建仿真环境，然后再修改仿真环境对应的文件以调整细节。

SDF、URDF 和 Xacro 的关系：

URDF 和 SDF 的区别：
- 复杂性： SDF 支持的功能更强大，能够描述完整的仿真环境；URDF 更适合定义机器人模型。
- 用途： URDF 是 ROS 的标准；SDF 是 Gazebo 的标准。
- 物理引擎支持： URDF 通过插件支持 Gazebo；SDF 原生支持 Gazebo。
- 格式转换： URDF 可以转换为 SDF（通过 ROS 提供的工具gz sdf -p）。
Xacro 的作用：
- Xacro 是 URDF 的生成工具，帮助用户高效编写 URDF 文件，但它与 SDF 无直接关系。

实践建议

在 Gazebo 仿真中： 如果你用的是 ROS 2 和 Gazebo，可以直接使用 SDF 文件，功能更强大。
在 ROS 中： 如果主要用于机器人控制和规划，推荐使用 URDF 或由 Xacro 生成的 URDF。
两者结合： 使用 URDF 进行控制，使用 SDF 进行仿真。例如，使用 URDF 定义机器人结构后，借助 Gazebo 插件将其转换为 SDF。

示例对比

URDF 示例：

<robot name="example_robot">
  <link name="base_link">
    <inertial>
      <mass value="1.0" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
    </inertial>
  </link>
</robot>

Xacro 示例（生成 URDF）：

<xacro:robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="example_robot">
  <xacro:macro name="base_link" params="mass">
    <link name="base_link">
      <inertial>
        <mass value="${mass}" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
      </inertial>
    </link>
  </xacro:macro>

  <xacro:base_link mass="1.0" />
</xacro:robot>

SDF 示例：

<sdf version="1.6">
  <model name="example_robot">
    <link name="base_link">
      <inertial>
        <mass>1.0</mass>
        <inertia>
          <ixx>1.0</ixx>
          <iyy>1.0</iyy>
          <izz>1.0</izz>
        </inertia>
      </inertial>
    </link>
  </model>
</sdf>

1.创建sdf文件

首先请调用指令ign gazebo启动Ignition Gazebo，选择Empty仿真环境，然后添加立方体，每一个立方体都对应一堵墙。

上下左右立方体box、box_1、box_2、box_3对应的坐标分别为(5.0,0.0,0.5)、(-5.0,0.0,0.5)、(0.0,2.5,0.5)、(0.0,-2.5,0.5)。

（以上坐标是指X，Y，Z坐标，没有旋转度）

保存文件到功能包的world目录下，保存的文件名称需要以.sdf为后缀，此处文件名为house.sdf。

<sdf version='1.9'>
  <world name='empty'>
    <physics name='1ms' type='ignored'>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
    </physics>
    <plugin name='gz::sim::systems::Physics' filename='ignition-gazebo-physics-system'/>
    <plugin name='gz::sim::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
    <plugin name='gz::sim::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
    <plugin name='gz::sim::systems::Contact' filename='ignition-gazebo-contact-system'/>
    <gravity>0 0 -9.8</gravity>
    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
    <atmosphere type='adiabatic'/>
    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>true</shadows>
    </scene>
    <model name='ground_plane'>
      <static>true</static>
      <link name='link'>
        <collision name='collision'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <inertial>
          <pose>0 0 0 0 -0 0</pose>
          <mass>100</mass>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
        </inertial>
        <enable_wind>false</enable_wind>
      </link>
      <pose>0 0 0 0 -0 0</pose>
      <self_collide>false</self_collide>
    </model>
    <model name='box'>
      <pose>5.0 0 0.5 -0 0 0</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>16.666</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>16.666</iyy>
            <iyz>0</iyz>
            <izz>16.666</izz>
          </inertia>
          <mass>100</mass>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>0.1 5 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>0.1 5 1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.7 0.7 0.7 1</diffuse>
            <specular>1 1 1 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <enable_wind>false</enable_wind>
      </link>
      <static>false</static>
      <self_collide>false</self_collide>
    </model>
    <model name='box_0'>
      <pose>-5.0 -0 0.50000 -0 0 0</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>16.666</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>16.666</iyy>
            <iyz>0</iyz>
            <izz>16.666</izz>
          </inertia>
          <mass>100</mass>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>0.1 5 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>0.1 5 1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.7 0.7 0.7 1</diffuse>
            <specular>1 1 1 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <enable_wind>false</enable_wind>
      </link>
      <static>false</static>
      <self_collide>false</self_collide>
    </model>
    <model name='box_1'>
      <pose>-0 -2.5 0.5 -0 -0 -0</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>16.666</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>16.666</iyy>
            <iyz>0</iyz>
            <izz>16.666</izz>
          </inertia>
          <mass>100</mass>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.7 0.7 0.7 1</diffuse>
            <specular>1 1 1 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <enable_wind>false</enable_wind>
      </link>
      <static>false</static>
      <self_collide>false</self_collide>
    </model>
    <model name='box_2'>
      <pose>-0 2.5 0.5 0 -0 -0</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>16.666</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>16.666</iyy>
            <iyz>0</iyz>
            <izz>16.666</izz>
          </inertia>
          <mass>100</mass>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.7 0.7 0.7 1</diffuse>
            <specular>1 1 1 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <enable_wind>false</enable_wind>
      </link>
      <static>false</static>
      <self_collide>false</self_collide>
    </model>
    <light name='sun' type='directional'>
      <pose>0 0 10 0 -0 0</pose>
      <cast_shadows>true</cast_shadows>
      <intensity>1</intensity>
      <direction>-0.5 0.1 -0.9</direction>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <linear>0.01</linear>
        <constant>0.90000000000000002</constant>
        <quadratic>0.001</quadratic>
      </attenuation>
      <spot>
        <inner_angle>0</inner_angle>
        <outer_angle>0</outer_angle>
        <falloff>0</falloff>
      </spot>
    </light>
  </world>
</sdf>

2.修改sdf文件

修改sdf文件，调整立方体的尺寸，实现墙体的合围。在sdf文件中，四个立方体分别对应了四个<model>标签，其name属性分别为box、box_1、box_2、box_3，将box和box_1中的<size>1 1 1</size>修改为<size>0.1 5 1</size>，将box_2和box_3中的<size>1 1 1</size>修改为<size>10 0.1 1</size>（注意：每个<model>标签下，都包含两个<size>标签，分别位于<collision>标签和<visual>标签下，两个<size>标签内容都需要修改）。

修改后与的house.sdf文件内容如下：

<sdf version='1.9'>
  <world name='empty'>
    <physics name='1ms' type='ignored'>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
    </physics>
    <plugin name='ign::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
    <plugin name='ign::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
    <plugin name='ign::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
    <plugin name='ign::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
    <gravity>0 0 -9.8</gravity>
    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
    <atmosphere type='adiabatic'/>
    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>true</shadows>
    </scene>
    <model name='ground_plane'>
      <static>true</static>
      <link name='link'>
        <collision name='collision'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <inertial>
          <pose>0 0 0 0 -0 0</pose>
          <mass>100</mass>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
        </inertial>
        <enable_wind>false</enable_wind>
      </link>
      <pose>0 0 0 0 -0 0</pose>
      <self_collide>false</self_collide>
    </model>
    <model name='box'>
      <pose>5.0 0 0.5 -0 0 0</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>16.666</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>16.666</iyy>
            <iyz>0</iyz>
            <izz>16.666</izz>
          </inertia>
          <mass>100</mass>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>0.1 5 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>0.1 5 1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.7 0.7 0.7 1</diffuse>
            <specular>1 1 1 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <enable_wind>false</enable_wind>
      </link>
      <static>false</static>
      <self_collide>false</self_collide>
    </model>
    <model name='box_0'>
      <pose>-5.0 -0 0.50000 -0 0 0</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>16.666</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>16.666</iyy>
            <iyz>0</iyz>
            <izz>16.666</izz>
          </inertia>
          <mass>100</mass>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>0.1 5 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>0.1 5 1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.7 0.7 0.7 1</diffuse>
            <specular>1 1 1 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <enable_wind>false</enable_wind>
      </link>
      <static>false</static>
      <self_collide>false</self_collide>
    </model>
    <model name='box_1'>
      <pose>-0 -2.5 0.5 -0 -0 -0</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>16.666</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>16.666</iyy>
            <iyz>0</iyz>
            <izz>16.666</izz>
          </inertia>
          <mass>100</mass>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.7 0.7 0.7 1</diffuse>
            <specular>1 1 1 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <enable_wind>false</enable_wind>
      </link>
      <static>false</static>
      <self_collide>false</self_collide>
    </model>
    <model name='box_2'>
      <pose>-0 2.5 0.5 0 -0 -0</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>16.666</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>16.666</iyy>
            <iyz>0</iyz>
            <izz>16.666</izz>
          </inertia>
          <mass>100</mass>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.7 0.7 0.7 1</diffuse>
            <specular>1 1 1 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <enable_wind>false</enable_wind>
      </link>
      <static>false</static>
      <self_collide>false</self_collide>
    </model>
    <light name='sun' type='directional'>
      <pose>0 0 10 0 -0 0</pose>
      <cast_shadows>true</cast_shadows>
      <intensity>1</intensity>
      <direction>-0.5 0.1 -0.9</direction>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <linear>0.01</linear>
        <constant>0.90000000000000002</constant>
        <quadratic>0.001</quadratic>
      </attenuation>
      <spot>
        <inner_angle>0</inner_angle>
        <outer_angle>0</outer_angle>
        <falloff>0</falloff>
      </spot>
    </light>
  </world>
</sdf>

3.编写launch文件

在launch目录下新建launch文件gazebo_sim_world.launch.py，并输入如下内容：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():

    this_pkg = get_package_share_directory('demo_gazebo_sim')
    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
    world_file = os.path.join(this_pkg,"world","house.sdf")

    gz_sim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
        launch_arguments={
            'gz_args': '-r ' + world_file
        }.items(),
    )
    return LaunchDescription([
        gz_sim
    ])

4.构建

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

colcon build  --packages-select demo_gazebo_sim

5.执行

终端中进入当前工作空间，调用如下指令执行launch文件：

. install/setup.bash
ros2 launch demo_gazebo_sim gazebo_sim_world.launch.py

运行结果如下图所示。

也可以根据个人喜好，继续设计房间模型。

IgnG添加模型

在Ignition Gazebo官网提供了许多仿真模型，可以自行下载并使用以优化仿真环境，使其更多样、美观且真实。

资源下载

仿真Ignition Gazebo的官方模型链接：

http://app.ignitionrobotics.org/fuel/models

自行选择仿真模型点击进入详情页面，然后点击下载按钮即可将模型资源保存到本地。

在用户目录下新建ign_models目录，将下载的资源解压缩到该目录以作备用。

资源配置

为了可以让Ignition Gazebo识别到模型资源，下一步还需要修改用户目录下的 .bashrc 文件，添加如下代码：

# Humble版本一般是下面的，但是有可能会更新，如果不生效，请尝试Jazzy的宏
export IGN_GAZEBO_RESOURCE_PATH=~/ign_models
# Jazzy版本的宏改了，如下：
export GZ_SIM_RESOURCE_PATH=~/ign_models

https://gazebosim.org/docs/latest/fuel_insert/

模型添加

终端下进入功能包demo_gazebo_sim的world目录，使用指令ign gazebo house.sdf 或者gz sim house.sdf启动仿真环境，点击窗口右上的折叠按钮，搜索Resource Spawner并打开，点击Local resources并选择模型拖拽至仿真环境中。将修改后的内容保存至house.sdf文件。

正常下载资源后，这个local resources这里就会显示了

house.sdf文件示例内容如下：

<sdf version='1.9'>
  <world name='empty'>
    <physics name='1ms' type='ignored'>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
    </physics>
    <plugin name='gz::sim::systems::Physics' filename='ignition-gazebo-physics-system'/>
    <plugin name='gz::sim::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
    <plugin name='gz::sim::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
    <plugin name='gz::sim::systems::Contact' filename='ignition-gazebo-contact-system'/>
    <gravity>0 0 -9.8</gravity>
    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
    <atmosphere type='adiabatic'/>
    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>true</shadows>
    </scene>
    <model name='ground_plane'>
      <static>true</static>
      <link name='link'>
        <collision name='collision'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='visual'>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <inertial>
          <pose>0 0 0 0 -0 0</pose>
          <mass>100</mass>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
        </inertial>
        <enable_wind>false</enable_wind>
      </link>
      <pose>0 0 0 0 -0 0</pose>
      <self_collide>false</self_collide>
    </model>
    <model name='box'>
      <pose>5.02632 -2e-06 0.500002 -0 4.4e-05 4.6e-05</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>16.666</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>16.666</iyy>
            <iyz>0</iyz>
            <izz>16.666</izz>
          </inertia>
          <mass>100</mass>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>0.1 5 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>0.1 5 1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.7 0.7 0.7 1</diffuse>
            <specular>1 1 1 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <enable_wind>false</enable_wind>
      </link>
      <static>false</static>
      <self_collide>false</self_collide>
    </model>
    <model name='box_0'>
      <pose>-5.01336 -0.00029 0.500002 0 -4.2e-05 -0.005335</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>16.666</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>16.666</iyy>
            <iyz>0</iyz>
            <izz>16.666</izz>
          </inertia>
          <mass>100</mass>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>0.1 5 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>0.1 5 1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.7 0.7 0.7 1</diffuse>
            <specular>1 1 1 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <enable_wind>false</enable_wind>
      </link>
      <static>false</static>
      <self_collide>false</self_collide>
    </model>
    <model name='box_1'>
      <pose>-0 -2.5 0.5 1e-06 0 0</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>16.666</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>16.666</iyy>
            <iyz>0</iyz>
            <izz>16.666</izz>
          </inertia>
          <mass>100</mass>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.7 0.7 0.7 1</diffuse>
            <specular>1 1 1 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <enable_wind>false</enable_wind>
      </link>
      <static>false</static>
      <self_collide>false</self_collide>
    </model>
    <model name='box_2'>
      <pose>-0.000154 2.52488 0.500821 -0.018068 -0 -0.003156</pose>
      <link name='box_link'>
        <inertial>
          <inertia>
            <ixx>16.666</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>16.666</iyy>
            <iyz>0</iyz>
            <izz>16.666</izz>
          </inertia>
          <mass>100</mass>
          <pose>0 0 0 0 -0 0</pose>
        </inertial>
        <collision name='box_collision'>
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode/>
            </friction>
            <bounce/>
            <contact/>
          </surface>
        </collision>
        <visual name='box_visual'>
          <geometry>
            <box>
              <size>10 0.1 1</size>
            </box>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.7 0.7 0.7 1</diffuse>
            <specular>1 1 1 1</specular>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <enable_wind>false</enable_wind>
      </link>
      <static>false</static>
      <self_collide>false</self_collide>
    </model>
    <include>
      <uri>file://Bed</uri>
      <name>Bed</name>
      <pose>2.82155 1.18752 0 0 -0 0</pose>
    </include>
    <include>
      <uri>file://Office Desk</uri>
      <name>Desk</name>
      <pose>2.78306 -1.97796 0 0 -0 1.57</pose>
    </include>
    <include>
      <uri>file://Bathtub</uri>
      <name>Bathtub</name>
      <pose>-3.87509 1.82783 0 0 -0 0</pose>
    </include>
    <include>
      <uri>file://Vanity</uri>
      <name>Vanity</name>
      <pose>-2.5974 1.85613 -0.010992 0.021648 0 -1.57</pose>
    </include>
    <include>
      <uri>file://Vanity</uri>
      <name>Vanity_1</name>
      <pose>-2.5974 0.634325 -0.010992 0.021648 0 -1.57</pose>
    </include>
    <include>
      <uri>file://Dining Table</uri>
      <name>DiningTable</name>
      <pose>-0.374337 1.33602 0 0 0 -1.57</pose>
    </include>
    <include>
      <uri>file://Chair</uri>
      <name>Chair</name>
      <pose>2.79762 -1.26474 -0 -0 0 -2.3062</pose>
    </include>
    <include>
      <uri>file://Sofa</uri>
      <name>Sofa</name>
      <pose>-0.546136 -1.92328 0.000119 -0 0 1.57</pose>
    </include>
    <light name='sun' type='directional'>
      <pose>0 0 10 0 -0 0</pose>
      <cast_shadows>true</cast_shadows>
      <intensity>1</intensity>
      <direction>-0.5 0.1 -0.9</direction>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <linear>0.01</linear>
        <constant>0.90000000000000002</constant>
        <quadratic>0.001</quadratic>
      </attenuation>
      <spot>
        <inner_angle>0</inner_angle>
        <outer_angle>0</outer_angle>
        <falloff>0</falloff>
      </spot>
    </light>
  </world>
</sdf>

构建

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

colcon build  --packages-select demo_gazebo_sim

执行

终端中进入当前工作空间，调用如下指令执行launch文件：

. install/setup.bash
ros2 launch demo_gazebo_sim gazebo_sim_world.launch.py

运行结果如下图所示。

IgnG添加机器人

Ignition Gazebo中可以直接创建机器人模型，或者也可以加载ROS2中URDF格式的机器人模型，此处我们使用后者（也可以选择用自己的urdf小车，但是注意修改launch的路径）。

我没有用赵虚左老师的mycar_description，用的自己的小车模型，后面的一系列源码都在下方这个Github仓库中，需要的可以自行clone.

https://github.com/tungchiahui/ROS2_WS/tree/main/6.ws_simulations

准备机器人模型功能包

复制机器人描述功能包mycar_description到工作空间的src目录，ign_models中新建mycar_description目录，并将功能包mycar_description下的mesh目录复制进ign_models中的mycar_description目录。

机器人模型功能包下新建launch文件

新建launch文件mycar_desc_sim.launch.py，并输入如下内容：

from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command,LaunchConfiguration
from launch.actions import DeclareLaunchArgument

#示例： ros2 launch cpp06_urdf display.launch.py model:=`ros2 pkg prefix --share cpp06_urdf`/urdf/urdf/demo01_helloworld.urdf
def generate_launch_description():

    MYCAR_MODEL = os.environ['MYCAR_MODEL']

    mycar_description = get_package_share_directory("mycar_description")
    default_model_path = os.path.join(mycar_description,"urdf",MYCAR_MODEL + ".urdf")
    model = DeclareLaunchArgument(name="model", default_value=default_model_path)

    # 加载机器人模型
    # 启动 robot_state_publisher 节点并以参数方式加载 urdf 文件
    robot_description = ParameterValue(Command(["xacro ",LaunchConfiguration("model")]))
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"robot_description": robot_description}]
    )

    return LaunchDescription([
        model,
        robot_state_publisher,
    ])

较之于以往该文件缺少了joint_state_publisher节点，该节点作用是发布活动关节状态，这一功能后续由ignition实现。

添加机器人模型

修改gazebo_sim_world.launch.py文件，包含机器人模型的发布文件并在Gazebo中生成机器人模型，修改后的代码如下：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node

def generate_launch_description():

    this_pkg = get_package_share_directory("demo_gazebo_sim")
    mycar_desc_pkg = get_package_share_directory("mycar_description")
    pkg_ros_gz_sim = get_package_share_directory("ros_gz_sim")
    world_file = os.path.join(this_pkg,"world","house.sdf")

    gz_sim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_ros_gz_sim, "launch", "gz_sim.launch.py")),
        launch_arguments={
            "gz_args": "-r " + world_file
        }.items(),
    )
    mycar_desc = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(mycar_desc_pkg,"launch","mycar_desc_sim.launch.py")
        )
    )
    spawn = Node(package="ros_gz_sim", executable="create",
                arguments=[
                "-name", "mycar",
                "-x", "0",
                "-z", "0.01", #设置为0,可能会陷进地里
                "-y", "0",
                "-topic", "/robot_description"],
            output="screen")

    return LaunchDescription([
        gz_sim,
        spawn,
        mycar_desc,
    ])

构建

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

colcon build --packages-select mycar_description demo_gazebo_sim

执行

终端中进入当前工作空间，调用如下指令执行launch文件：（执行起来有问题的话，你只要学过urdf怎么跑，应该拥有自我寻找错误的能力了，自己找吧）

. install/setup.bash
# MYCAR_MODEL值可以设置为arduino、stm32_2w 或stm32_4w（这个是具体的urdf文件名，在mycar_description包下的）
export MYCAR_MODEL=stm32_4w
ros2 launch demo_gazebo_sim gazebo_sim_world.launch.py

运行结果如下图所示。

IgnG运动控制器(实质上就是标签)

本节将介绍如何让你的机器人动起来。

原理就是给urdf或xacro等添加标签：

http://sdformat.org/tutorials?tut=sdformat_urdf_extensions&cat=specification&

https://gazebosim.org/api/plugin/2/index.html

安装库：

https://gazebosim.org/api/plugin/2/installation.html

sudo apt-get update
sudo apt-get install lsb-release
sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
sudo apt-get update
sudo apt install libgz-plugin2-dev

利用插件去让小车动，比如有两轮差速插件，四轮麦轮插件等等

同时该插件还提供了一些可以控制输出的选项，因为是仿真，所以还要告诉插件轮子对应的joint名称等信息，这样就有了下面这个参数表格：

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 配置项 | 含义 | |:—|:—| | ros | ros相关配置，包含命名空间和话题重映射等 | | update_rate | 数据更新速率 | | left_joint | 左轮关节名称 | | right_joint | 右轮关节名称 | | wheel_separation | 左右轮子的间距 | | wheel_diameter | 轮子的直径 | | max_wheel_torque | 轮子最大的力矩 | | max_wheel_acceleration | 轮子最大的加速度 | | publish_odom | 是否发布里程计 | | publish_odom_tf | 是否发布里程计的tf开关 | | publish_wheel_tf | 是否发布轮子的tf数据开关 | | odometry_frame | 里程计的framed ID，最终体现在话题和TF上 | | robot_base_frame | 机器人的基础frame的ID |

修改URDF文件

arduino.urdf 和 stm32_2w.urdf 文件中，在<robot>根标签下添加如下代码：

  <!-- 运动控制插件 -->
  <gazebo>
      <plugin filename="libignition-gazebo-diff-drive-system.so"
        name="ignition::gazebo::systems::DiffDrive">
        <left_joint>left_joint</left_joint>
        <right_joint>right_joint</right_joint>
        <wheel_separation>0.2097</wheel_separation>
        <wheel_radius>0.03415</wheel_radius>
        <odom_publish_frequency>10</odom_publish_frequency>
        <frame_id>odom</frame_id>
        <child_frame_id>base_footprint</child_frame_id>
        <topic>/cmd_vel</topic>
        <max_linear_acceleration>10</max_linear_acceleration>
        <min_linear_acceleration>-10</min_linear_acceleration>
        <max_angular_acceleration>10</max_angular_acceleration>
        <min_angular_acceleration>-10</min_angular_acceleration>
        <max_linear_velocity>0.5</max_linear_velocity>
        <min_linear_velocity>-0.5</min_linear_velocity>
        <max_angular_velocity>1</max_angular_velocity>
        <min_angular_velocity>-1</min_angular_velocity>
      </plugin>

  </gazebo>

  <!-- 关节状态发布 -->
  <gazebo>
    <plugin filename="ignition-gazebo-joint-state-publisher-system"
      name="ignition::gazebo::systems::JointStatePublisher">
    </plugin>
  </gazebo>

stm32_4w.urdf 文件中，在<robot>根标签下添加如下代码：

<!-- 运动控制插件 -->
<gazebo>
    <plugin
        filename="ignition-gazebo-diff-drive-system"
        name="ignition::gazebo::systems::DiffDrive">
        <left_joint>left_former_joint</left_joint>
        <left_joint>left_rear_joint</left_joint>
        <right_joint>right_former_joint</right_joint>
        <right_joint>right_rear_joint</right_joint>
        <wheel_separation>0.4</wheel_separation>
        <wheel_radius>0.0415</wheel_radius>
        <odom_publish_frequency>50</odom_publish_frequency>
        <frame_id>odom</frame_id>
        <child_frame_id>base_footprint</child_frame_id>
        <topic>/cmd_vel</topic>
        <max_linear_acceleration>10</max_linear_acceleration>
        <min_linear_acceleration>-10</min_linear_acceleration>
        <max_angular_acceleration>10</max_angular_acceleration>
        <min_angular_acceleration>-10</min_angular_acceleration>
        <max_linear_velocity>0.5</max_linear_velocity>
        <min_linear_velocity>-0.5</min_linear_velocity>
        <max_angular_velocity>1</max_angular_velocity>
        <min_angular_velocity>-1</min_angular_velocity>
      </plugin>
  </gazebo>
  <!-- 关节状态发布 -->
  <gazebo>
    <plugin filename="ignition-gazebo-joint-state-publisher-system"
      name="ignition::gazebo::systems::JointStatePublisher">
    </plugin>
  </gazebo>

如果是麦轮（ROS1的，ROS2的待更新）（把轮子关节设置为自己轮子关节名即可）：

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
 
    <!-- 传动实现:用于连接控制器与关节 -->
    <xacro:macro name="joint_trans" params="joint_name">
        <!-- Transmission is important to link the joints and the controller -->
        <transmission name="${joint_name}_trans">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="${joint_name}">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            </joint>
            <actuator name="${joint_name}_motor">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>
    </xacro:macro>
 
    <!-- 每一个驱动轮都需要配置传动装置 -->
    <xacro:joint_trans joint_name="LeftFrontwheelToBase" />
    <xacro:joint_trans joint_name="LeftBackwheelToBase" />
    <xacro:joint_trans joint_name="RightFrontwheelToBase" />
    <xacro:joint_trans joint_name="RightBackwheelToBase" />
 
    <!-- 麦轮控制器 -->
    <gazebo>
        <plugin name="mecanum_controller" filename="libgazebo_ros_planar_move.so">
            <commandTopic>cmd_vel</commandTopic>
            <odometryTopic>odom</odometryTopic>
            <odometryFrame>odom</odometryFrame>
            <leftFrontJoint>LeftFrontwheelToBase</leftFrontJoint>
            <rightFrontJoint>RightFrontwheelToBase</rightFrontJoint>
            <leftRearJoint>LeftBackwheelToBase</leftRearJoint>
            <rightRearJoint>RightBackwheelToBase</rightRearJoint>
            <odometryRate>100</odometryRate>
            <robotBaseFrame>base_footprint</robotBaseFrame>
            <broadcastTF>1</broadcastTF>
        </plugin>
    </gazebo>
 
</robot>

修改launch文件

修改gazebo_sim_world.launch.py文件，修改后的代码如下：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node

def generate_launch_description():

    this_pkg = get_package_share_directory("demo_gazebo_sim")
    mycar_desc_pkg = get_package_share_directory("mycar_description")
    pkg_ros_gz_sim = get_package_share_directory("ros_gz_sim")
    world_file = os.path.join(this_pkg,"world","base.sdf")

    gz_sim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_ros_gz_sim, "launch", "gz_sim.launch.py")),
        launch_arguments={
            "gz_args": "-r " + world_file
        }.items(),
    )
    mycar_desc = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(mycar_desc_pkg,"launch","mycar_desc_sim.launch.py")
        )
    )
    spawn = Node(package="ros_gz_sim", executable="create",
                arguments=[
                "-name", "mycar",
                "-x", "-4",
                "-z", "0.01", #设置为0,可能会陷进地里
                "-y", "0",
                "-topic", "/robot_description"],
            output="screen")

    # Bridge
    bridge = Node(
        package="ros_gz_bridge",
        executable="parameter_bridge",
        arguments=["/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist",
                   "/model/mycar/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry",
                   "/model/mycar/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V",
                   "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
                   "/world/empty/model/mycar/joint_state@sensor_msgs/msg/JointState[gz.msgs.Model",
                   ],
        parameters=[{"qos_overrides./model/mycar.subscriber.reliability": "reliable"}],
        remappings=[
                ("/model/mycar/tf", "/tf"),
                ("/world/empty/model/mycar/joint_state","joint_states"),
                ("/model/mycar/odometry","/odom")
            ],
        output="screen"
    )

    return LaunchDescription([
        gz_sim,
        spawn,
        mycar_desc,
        bridge
    ])

构建

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

colcon build --packages-select mycar_description demo_gazebo_sim

执行

终端中进入当前工作空间，调用如下指令执行launch文件：

. install/setup.bash
export MYCAR_MODEL=stm32_4w # MYCAR_MODEL值可以设置为arduino、stm32_2w 或stm32_4w（这个是具体的urdf文件名，在mycar_description包下的）
ros2 launch demo_gazebo_sim gazebo_sim_world.launch.py

再启动键盘控制节点，就可以控制机器人运动了。

ros2 run teleop_twist_keyboard teleop_twist_keyboard

还可以启动rviz2，以查看里程计消息以及坐标变换。终端中进入当前工作空间，调用如下指令执行launch文件：

启动rviz2

. install/setup.bash
rviz2

RVIZ2软件配置如下图所示：

Ignition Gazebo仿真之传感器

本节将介绍如何为仿真机器人添加雷达、相机等传感器。本节代码部分内容对于我们教程中涉及到的arduino、stm32_2w以及stm32_4w等机器人模型而言是完全通用的。

添加传感器插件

在进行传感器模拟之前，需要先添加一个名为ignition-gazebo-sensors-system的插件，打开urdf文件，在<robot>根标签内添加如下代码：

  <!-- 添加传感器插件 -->

<gazebo>
    <plugin
      filename="ignition-gazebo-sensors-system"
      name="ignition::gazebo::systems::Sensors">
      <render_engine>ogre2</render_engine>
    </plugin>
  </gazebo>

ignition-gazebo-sensors-system是Ignition Gazebo仿真环境的插件，提供传感器模型和相关功能，用于创建、模拟和测试各种传感器设备。它包含常见传感器模型，如摄像头、激光雷达等。

添加各种传感器

(注意，你的模型一定要有以下几个传感器的模型)

雷达的模型不需要collision，请删掉，否则会挡激光射出。

 <!-- 雷达传感器 -->
  <gazebo reference="laser">
    <sensor name='gpu_lidar' type='gpu_lidar'>
      <topic>scan</topic>
      <update_rate>30</update_rate>
      <lidar>
        <scan>
          <horizontal>
            <samples>640</samples>
            <resolution>1</resolution>
            <min_angle>-3.1415926</min_angle>
            <max_angle>3.1415926</max_angle>
          </horizontal>
          <vertical>
            <samples>16</samples>
            <resolution>1</resolution>
            <min_angle>-0.261799</min_angle>
            <max_angle>0.261799</max_angle>
          </vertical>
        </scan>
        <range>
          <min>0.08</min>
          <max>10.0</max>
          <resolution>0.01</resolution>
        </range>
      </lidar>
      <alwaysOn>1</alwaysOn>
      <visualize>true</visualize>
      <ignition_frame_id>laser</ignition_frame_id>
    </sensor>
  </gazebo>

  <!-- 单目相机 -->

  <gazebo reference="camera" >
    <sensor name="cam_link" type="camera">
      <update_rate>10.0</update_rate>
      <always_on>true</always_on>
      <ignition_frame_id>camera</ignition_frame_id>
      <pose>0 0 0 0 0 0</pose>
      <topic>/image_raw</topic>
      <camera name="my_camera">
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
           <width>600</width>
           <height>600</height>
           <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
      </camera>
    </sensor>
  </gazebo>

  <!-- 深度相机 -->

  <gazebo reference="camera">
    <sensor name="depth_camera" type="depth_camera">
          <update_rate>10</update_rate>
          <topic>depth_camera</topic>
          <camera>
            <horizontal_fov>1.05</horizontal_fov>
            <image>
              <width>256</width>
              <height>256</height>
              <format>R_FLOAT32</format>
            </image>
            <clip>
              <near>0.1</near>
              <far>10.0</far>
            </clip>
          </camera>
          <alwaysOn>1</alwaysOn>
          <ignition_frame_id>camera</ignition_frame_id>
      </sensor>
  </gazebo>

从官网找到的imu传感器的

    <!-- 添加IMU传感器插件（和雷达，摄像头等不是一个插件） -->
    <gazebo>
        <plugin filename="libignition-gazebo-imu-system.so"
                name="ignition::gazebo::systems::Imu">
        </plugin>
    </gazebo>

    <!-- 陀螺仪 -->
    <!-- imu坐标系直接用base_link-->
    <gazebo reference="base_link">
        <sensor name="imu_sensor" type="imu">
            <always_on>1</always_on>
            <update_rate>30</update_rate>
            <visualize>true</visualize>
            <topic>imu</topic>
        </sensor>
    </gazebo>

可以用ign topic -e -t /imu测试gazebo是否发布了话题，后面再用gazebo_bridge把话题给ROS2就行了。

默认情况下，rviz2没有显示imu消息的插件，需要自行安装相关插件，具体安装指令如下：

sudo apt install ros-${ROS_DISTRO}-imu-tools

SolidWorks自动生成的模型可能翻转了laser_joint,请你修改回正，这样可能rivz2就有激光了，然后修改一下可视化的模型，让模型正常，不要给碰撞，不然可能会遮挡激光。

修改gazebo_sim_world.launch.py文件，修改后的代码如下：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node

def generate_launch_description():

    this_pkg = get_package_share_directory("demo_gazebo_sim")
    mycar_desc_pkg = get_package_share_directory("mycar_description")
    pkg_ros_gz_sim = get_package_share_directory("ros_gz_sim")
    world_file = os.path.join(this_pkg,"world","house.sdf")

    gz_sim = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
        os.path.join(pkg_ros_gz_sim, "launch", "gz_sim.launch.py")),
        launch_arguments={
        "gz_args": "-r " + world_file
        }.items(),
    )
    mycar_desc = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(mycar_desc_pkg,"launch","mycar_desc_sim.launch.py")
        )
    )
    spawn = Node(package="ros_gz_sim", executable="create",
        arguments=[
            "-name", "mycar",
            "-x", "-4",
            "-z", "0.01", #设置为0,可能会陷进地里
            "-y", "0",
            "-topic", "/robot_description"],
        output="screen")

    # Bridge
    bridge = Node(
        package="ros_gz_bridge",
        executable="parameter_bridge",
        arguments=["/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist",
            "/model/mycar/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry",
            "/model/mycar/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V",
            "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
            "/world/empty/model/mycar/joint_state@sensor_msgs/msg/JointState[gz.msgs.Model",
            "/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan",
            "/scan/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked",
            "/image_raw@sensor_msgs/msg/Image@gz.msgs.Image",
            "/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo",
            "/depth_camera@sensor_msgs/msg/Image@gz.msgs.Image",
            "/imu@sensor_msgs/msg/Imu[gz.msgs.IMU",
            "/imu/angular_velocity@geometry_msgs/msg/Vector3[gz.msgs.Vector3d"
        ],
        parameters=[{"qos_overrides./model/mycar.subscriber.reliability": "reliable"}],
        remappings=[
            ("/model/mycar/tf", "/tf"),
            ("/world/empty/model/mycar/joint_state","joint_states"),
            ("/model/mycar/odometry","/odom")
        ],
        output="screen"
    )

    return LaunchDescription([
        gz_sim,
        spawn,
        mycar_desc,
        bridge
    ])

构建

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

colcon build --packages-select mycar_description demo_gazebo_sim

执行

终端中进入当前工作空间，调用如下指令执行launch文件：

. install/setup.bash
export MYCAR_MODEL=stm32_4w # MYCAR_MODEL值可以设置为arduino、stm32_2w 或stm32_4w
ros2 launch demo_gazebo_sim gazebo_sim_world.launch.py

再启动键盘控制节点，就可以控制机器人运动了。

还可以启动rviz2，以查看机器人发布的诸多数据。终端中进入当前工作空间，调用如下指令执行launch文件：

. install/setup.bash
rviz2

我没有用赵虚左老师的mycar_description，用的自己的小车模型，后面的一系列源码都在下方这个Github仓库中，需要的可以自行clone.

https://github.com/tungchiahui/ROS2_WS/tree/main/6.ws_simulations

(把上面的全部复现，才能够进行下一章导航，下一章导航依然基于仿真)

将Ign Gazebo迁移至Gz Sim

（即ROS2Humble迁移至ROS2Jazzy及之后的版本，只用Humble的也要看一下）

由于只有ROS2 Humble的Gazebo叫做ign gazebo，到了ROS2 Jazzy及之后的版本，应该会都改名叫gazebo（简称gz sim）。

如果你要把代码从ROS2 Humble迁移到ROS2 Jazzy，那么兼容性问题最大的地方就是Gazebo，需要修改很多东西。

但是Ign Gazebo与Gz Sim的最大区别其实就是重命名了一下，所以迁移很好迁移。

为何都是ROS2，且gazebo界面功能几乎都一模一样还代码不互通呢？

是因为gazebo组织早期在为ROS2开发新版Gazebo时，为了和Gazebo Classic做出区分，所以给新版Gazebo命名为Ignition Gazebo，并在ROS2 Humble上推行。

后来Gazebo组织发现这种命名方式及其麻烦，所以在新版ROS2 Jazzy上又修改回了Gazebo的名字(简称Gz Sim)。

所以并不是功能不互通，仅仅是名字不互通罢了，只需要重命名，就这么简单。

https://gazebosim.org/docs/harmonic/migration_from_ignition/

主要修改以下部分：

SDF文件

根据官方所说，普通的要把ignition全部替换为gz.

插件要按照上面的修改，比如ignition::gazebo改为gz::sim。然后ignition::修改为gz::。

所有 ignition- 前缀 → gz-
所有 ignition::gazebo 命名空间 → gz::sim
插件系统名称 ignition-gazebo-XXX-system → gz-sim-XXX-system
新版本中alwaysOn已被always_on替代，gz_frame_id改为pose relative_to””
确保所有环境变量（如IGN_GAZEBO_RESOURCE_PATH）更新为GZ_SIM_RESOURCE_PATH

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 原字段 | 新字段 | 示例场景 | |:—|:—|:—| | ignition-gazebo-* | gz-sim-* | 插件文件名 | | ignition::gazebo::* | gz::sim::* | 插件命名空间 | | IGN_GAZEBO环境变量前缀 | GZ_SIM前缀 | 资源路径配置 |

具体代码看我的仓库：

https://github.com/CyberNaviRobot/CyberRobot_ROS2_Jazzy_WS

本章小结

本章主要介绍了2D仿真工具stage_ros2以及3D仿真工具ignition gazebo在ROS2中的应用。

相关知识点如下：

仿真的理论知识；
stage_ros2在ROS2中的使用；
ignition gazebo在ROS2中的使用。

通过上述内容我们介绍了仿真在ROS2中的应用场景、概念，以及较之于实体机器人，仿真的优势和不足。还学习了如何基于stage_ros2以及ignition gazebo分别搭建仿真环境并模拟机器人，为后续的学习奠定了基础。

另外，除了stage_ros2和ignition gazebo之外，还有其他一些可用于ROS2仿真的软件。以下是一些常见的选择：

Webots: Webots是一个强大的开源多机器人仿真软件平台，它支持各种机器人硬件和传感器模拟，并且可以与ROS 2进行集成。
Gazebo Classic: Gazebo Classic是ROS 1中常用的仿真器，也可以通过ROS 2进行连接和使用。
MORSE: MORSE是一个基于Python的开源仿真器，可以用于ROS 2仿真和机器人开发，支持多种传感器和行为模型。
V-REP: V-REP是一个强大的多机器人仿真平台，它支持ROS 2与其进行通信，可以实现各种机器人模型和场景的仿真。

这些都是一些常见的ROS 2仿真软件，你可以根据具体的需求选择合适的软件来进行仿真和开发。请注意，某些仿真器可能需要进行适配和配置以与ROS 2兼容。

机器人导航Navigation2(仿真篇)
导航概述
导航简述

概念

机器人导航是指在没有人为干预的情况下，机器人可以自主地从一个位置移动到另一个位置。在ROS2中，导航实现最为常用的框架是Nav2。

Nav2也即Navigation2，它继承自ROS的导航堆栈。它采用了与自动驾驶车辆相同的前沿技术，并经过优化和改造，专门适用于移动机器人和地面机器人的导航需求。这个项目赋予移动机器人穿越复杂环境的能力，使其能够完成用户定义的各种应用任务，几乎适用于任何类型的机器人动力学。Nav2不仅支持机器人从A点到B点的移动，还能设置中间姿态，并执行多种任务，如物体跟踪、全面覆盖导航等。作为全球100多家公司信赖的生产级高质量导航框架，Nav2以其卓越的可靠性和稳定性赢得了广泛赞誉。

作用

Nav2在机器人导航领域作用显著，主要表现在以下几个方面。

Nav2具备多样的导航功能，支持激光雷达、摄像头等多种传感器输入，实时获取环境信息，实现智能路径规划和精确运动控制。它支持多种导航模式，满足不同应用需求，并具备强大的扩展性，可与ROS 2组件无缝集成。
Nav2性能卓越，采用高效算法和数据处理技术，确保机器人在导航过程中响应迅速且稳定。
安全方面，Nav2采用多种避障算法和参数调节功能，设定安全区域，提供诊断监控，确保机器人安全导航。

总之，Nav2为机器人的导航和应用提供了强大的支持。无论是工业领域的自动化生产，还是服务领域的智能机器人，Nav2都能够发挥重要作用，提升机器人的自主性和智能化水平。

导航安装

借助于Ubuntu的包资源管理器，可以使用二进制的方式安装Nav2，安装指令如下：

sudo apt install ros-<ros2-distro>-navigation2
sudo apt install ros-<ros2-distro>-nav2-bringup

# Humble
sudo apt install ros-humble-navigation2
sudo apt install ros-humble-nav2-bringup

# Jazzy
sudo apt install ros-jazzy-navigation2
sudo apt install ros-jazzy-nav2-bringup

指令中的<ros2-distro>请替换成当前所使用的ROS2版本。

导航条件

深入学习Nav2，需了解ROS2机器人基础知识，并配备仿真或实体机器人作为实践环境。

ROS2基础

ROS2通信 ：了解ROS2中的节点（Nodes）、话题（Topics）、服务（Services）、动作（Actions）等基本概念，以及如何通过这些机制实现节点间的通信。
生命周期管理 ：熟悉ROS2中的生命周期节点（Lifecycle Nodes），理解节点的启动、配置、激活、去激活、清理等状态转换过程，这对于管理复杂的ROS2系统至关重要。
rviz2： 熟练使用rviz2进行可视化调试，包括如何设置不同的显示类型（如点云、地图、路径等），以及如何通过rviz2与ROS2节点进行交互（如设置导航目标点、观察机器人状态等）。
URDF： 了解URDF（Unified Robot Description Format）文件，这是ROS中用于描述机器人模型的一种XML格式。熟悉如何编写和修改URDF文件，以便在仿真或实际环境中准确地表示机器人结构。
TF坐标变换： 掌握TF（Transform）库的使用，理解坐标变换在机器人导航中的重要性。学会如何设置和查询不同坐标系之间的变换关系，以确保机器人能够准确地定位自身和周围环境。

实践环境

仿真环境： 搭建一个ROS2兼容的仿真环境，并配置好相应的机器人模型、传感器（如激光雷达、相机等）和仿真世界。确保仿真环境中的机器人能够接收速度指令、反馈里程计消息、发布传感器数据和TF坐标变换等。
实体机器人： 如果条件允许，准备一台实体机器人。这台机器人应该具备与仿真环境中相似的功能，即能够接收速度指令、反馈里程计消息、发布激光雷达等传感器数据以及发布TF坐标变换。同时，确保实体机器人已经安装了ROS2系统，并且已经配置好相应的驱动程序和算法库。
Nav2软件包： 安装并配置好Nav2软件包及其依赖项。Nav2是ROS2中用于机器人导航的综合性软件包，它包含了路径规划、避障、局部和全局地图维护等多个功能模块。确保Nav2软件包与您的ROS2版本兼容，并且已经根据需要进行了适当的配置。

通过掌握上述基础知识并准备好实践环境，您将能够更好地学习和应用Nav2进行机器人导航任务的开发与调试。

导航参数参考

https://docs.nav2.org/configuration/index.html

SLAM 定位与建图

概念

SLAM（Simultaneous Localization and Mapping，即时定位与建图）是机器人学和自动导航领域的一种关键技术，允许机器人在未知环境中绘制环境地图。

在ROS 2下进行SLAM，通常涉及使用专门为ROS 2开发的SLAM相关软件包。这些软件包利用ROS 2的通讯框架（如话题、服务和action）来处理接收到的传感器数据，包括激光雷达、相机、惯性测量单元等。

以下列举了一些常见的在ROS2中使用的SLAM系统：

SLAM Toolbox ：SLAM Toolbox 是一种在 ROS 2 中普遍使用的 SLAM 解决方案，由 Steve Macenski 维护。它是为了替代原有的ROS中著名的gmapping包和Cartographer SLAM包而开发的，提供了2D激光SLAM领域中的多种功能。
Cartographer for ROS 2 ：Cartographer是由Google开发的一个2D和3D环境SLAM库。尽管它最初是为ROS开发的，但已经有为ROS 2创建的适配版本，可以在ROS 2生态系统内使用。

要在ROS 2中开始一个SLAM项目，还需要具体的传感器（例如激光雷达或摄像头），计算机要有足够的算力来运行SLAM算法，以及熟悉ROS 2节点、话题、服务、action、参数等知识，以实现有效的数据处理和通信。随着项目的发展，可能还需要考虑动态重配置、3D建图和路径规划等高级功能。

作用

必须先说明的是：SLAM与Nav2并没有直接的依赖关系，它们是两种相对独立的技术框架。然而，在实际应用中，这两者却展现出紧密的联系与互补性。

独立性： SLAM能够独立完成环境地图的构建，无需Nav2的介入。它依靠传感器数据来感知环境，并通过算法估计机器人的位置与姿态，从而构建出环境的地图。与此同时，Nav2也具备自主导航的能力。即使没有SLAM地图作为输入，它也能依赖其他来源的环境信息进行导航。

互补性： 尽管两者在技术上各自独立，但它们的结合却带来了显著的优势。Nav2能够利用SLAM创建的精确地图进行高效的路径规划，确保机器人能够顺利导航至目标位置。而SLAM则能借助Nav2的导航控制功能，在机器人移动过程中实时收集环境信息，从而进一步提高地图构建的效率和准确性。

因此，尽管SLAM与Nav2在技术上各自独立，但在构建完整的机器人导航系统中，它们的互补性使得机器人能够在复杂环境中实现更加自主、精准的建图或导航。

slam_toolbox概述

概念

SLAM Toolbox是一个基于开源软件的用来构建 2D地图 的工具集，旨在为研究人员和开发者提供一个快速构建和实现SLAM算法的平台。它集成了多种常用的SLAM算法，并提供了丰富的数据处理和滤波函数，以及地图构建和环境建模的工具。

功能

算法集成 包含基于卡尔曼滤波器和粒子滤波器的SLAM算法，以及基于图优化的SLAM算法等。
数据处理 提供数据融合、数据预处理和异常检测等功能，支持激光雷达、摄像头、IMU等多种传感器数据的处理。
地图构建 利用传感器数据和机器人运动信息构建准确的地图，并进行更新和优化。
模块化设计 支持用户插入自己的算法或替换现有模块，实现高度定制化的SLAM解决方案。

优点

开源性和灵活性 作为开源软件，SLAM Toolbox提供了源代码和文档，用户可以自由修改和扩展算法。
多种传感器支持 支持激光雷达、摄像头、IMU等多种传感器数据，适应不同应用场景的需求。
高效性 利用现代C++的特性优化性能，确保在资源受限的硬件上也能高效运行。
广泛的应用前景 适用于学术研究、无人机导航、自动驾驶汽车、室内机器人和工业自动化等多个领域。

slam_toolbox安装

借助于Advanced Packaging Tools(APT)包资源管理器，可以使用二进制的方式安装slam_toolbox，安装指令如下：

sudo apt install ros-<ros2-distro>-slam-toolbox

#humble
sudo apt install ros-humble-slam-toolbox
#jazzy
sudo apt install ros-jazzy-slam-toolbox

指令中的<ros2-distro>请替换成当前所使用的ROS2版本。

slam_toolbox节点说明

在slam_toolbox中常用的节点有两个：sync_slam_toolbox_node和async_slam_toolbox_node。

sync_slam_toolbox_node：

这是一个同步节点，会等待所有传感器数据到达后再处理，确保数据完整性和一致性，提高定位和建图准确性。但同步处理可能导致延迟，更适合对数据一致性和准确性要求高、实时性要求不高的场景。

async_slam_toolbox_node：

与同步节点不同，这是一个异步节点，可以立即处理已接收的数据，减小延迟，提高响应速度。但异步处理可能导致数据不完全同步，定位和建图结果可能不如同步方式准确。因此，它更适合对实时性要求高、对数据一致性和准确性要求相对较低的场景。

在选择使用sync_slam_toolbox_node还是async_slam_toolbox_node时，需根据应用需求和环境权衡。若实时性关键且能接受一定定位或建图误差，选择async_slam_toolbox_node。若对数据一致性和准确性要求较高，且实时性非首要考虑，则选择sync_slam_toolbox_node。另外二者的主要区别在于数据处理的方式，而两个节点发布的话题、订阅的话题、发布的服务以及使用的参数等都是一样的。

订阅的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 类型 | 描述 | |:—|:—|:—| | /scan | sensor_msgs/msg/LaserScan | 来自激光雷达输入的扫描数据 | | /tf | tf2_msgs/msg/TFMessage | 配置的odom_frame到base_frame的转换 |

虽然不订阅/odom,但是需要发布/odom,以改变坐标。

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 特性 | slam_toolbox | hector_slam | |:—|:—|:—| | 地图精度 | 高 | 中 | | 实时性 | 较好，但依赖优化（回环检测） | 极高 | | 依赖数据 | 激光雷达、TF 树、里程计 | 激光雷达（可选 IMU） | | 回环检测 | 支持 | 不支持 | | 长期运行 | 支持 | 不支持 | | 适用场景 | 动态导航、复杂环境 | 简单环境，或无里程计时 |

发布的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 类型 | 描述 | |:—|:—|:—| | /map | nav_msgs/msg/OccupancyGrid | pose-graph（姿态图）在特定的更新频率（map_update_interval）下的占用栅格表示。 | | /pose | geometry_msgs/msg/PoseWithCovarianceStamped | 配置的map_frame中base_frame的位姿以及根据扫描匹配计算的协方差 |

发布的服务

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 类型 | 描述 | |:—|:—|:—| | /slam_toolbox/clear_changes | slam_toolbox/srv/Clear | 清除所有待处理的手动位姿图操作的更改 | | /slam_toolbox/deserialize_map | slam_toolbox/srv/DeserializePoseGraph | 从磁盘加载保存的序列化位姿图文件 | | /slam_toolbox/dynamic_map | nav_msgs/OccupancyGrid | 请求位姿图的当前状态作为占用网格 | | /slam_toolbox/manual_loop_closure | slam_toolbox/srv/LoopClosure | 请求对位姿图进行手动更改 | | /slam_toolbox/pause_new_measurements | slam_toolbox/srv/Pause | 暂停处理新传入的激光扫描 | | /slam_toolbox/save_map | slam_toolbox/srv/SaveMap | 保存可用于显示 AMCL 定位的地图图像文件。 | | /slam_toolbox/serialize_map | slam_toolbox/srv/SerializePoseGraph | 保存地图位姿图和数据，可用于继续建图、slam_toolbox 定位、离线操作等 | | /slam_toolbox/toggle_interactive_mode | slam_toolbox/srv/ToggleInteractive | 在交互模式与非交互模式之间切换，发布节点的交互式标记及其位置，以便在应用程序中进行更新 | | /slam_toolbox/reset | slam_toolbox/srv/Reset | 将当前地图重置回初始状态 |

参数
1. 求解器参数

solver_plugin 用于 karto 扫描解算器的非线性解算器类型。选项：solver_plugins::CeresSolver, - solver_plugins::SpaSolver, solver_plugins::G2oSolver. Default: solver_plugins::CeresSolver.
ceres_linear_solver Ceres 使用的线性求解器。选项：SPARSE_NORMAL_CHOLESKY、SPARSE_SCHUR、ITERATIVE_SCHUR、CGNR。默认为 SPARSE_NORMAL_CHOLESKY。
ceres_preconditioner 与该求解器一起使用的预处理器。选项：JACOBI、IDENTITY（none）、SCHUR_JACOBI。默认为JACOBI。
ceres_trust_strategy 信任区域策略。行搜索策略没有公开，因为它们对于这种用途表现不佳。选项：LEVENBERG_MARQUARDT、DOGLEG。默认值：LEVENBERG_MARQUARDT。
ceres_dogleg_type 如果信任策略是 DOGLEG，则使用dogleg策略。选项：TRADITIONAL_DOGLEG、SUBSPACE_DOGLEG。默认值：TRADITIONAL_DOGLEG
ceres_loss_function 拒绝异常值的损失函数类型。没有一个等于损失平方。选项：None、HuberLoss、CauchyLoss。默认值：None。
mode “建图”或“定位”模式，用于 Ceres 问题创建中的性能优化
- Toolbox参数
odom_frame 里程计坐标系
map_frame 地图坐标系
base_frame 基坐标系
scan_topic 扫描主题名，注意是/scan 不是scan
scan_queue_size 扫描消息对队列长度。在异步模式下应始终设置为 1
use_map_saver 实例化地图服务程序并自行订阅map主题
map_file_name 启动时加载的位姿图文件的名称（如果可用）
map_start_pose 启动建图/定位时的位姿（如果可用）
map_start_at_dock 在dock（第一个节点）处启动姿势图加载（如果可用）。如果同时设置了pose和dock，优先使用pose
debug_logging 更改日志以进行调试
throttle_scans 在同步模式下限制的扫描次数
transform_publish_period 里程计odom变换发布周期。 0 不会发布变换。
map_update_interval 更新 2D 占用地图的时间间隔
enable_interactive_mode 是否允许启用交互模式。交互模式将保留映射到其 ID 的激光扫描缓存，以便在交互模式下进行可视化。结果，该进程的内存将会增加。在定位和长期建图模式下可以手动禁用此功能，因为它们会随着时间的推移增加内存利用率。对于建图或连续建图模式均有效。
position_covariance_scale 从扫描匹配发布姿势时缩放位置协方差的量。这可用于调整下游定位滤波器中位姿的影响。协方差表示测量的不确定性，因此扩大协方差将减小位姿对下游滤波器的影响。默认值：1.0
yaw_covariance_scale 从扫描匹配发布位姿时缩放偏航协方差的量。请参阅position_covariance_scale 的描述。默认值：1.0
resolution 生成的 2D 占用图的分辨率
max_laser_range 用于 2D 占用地图光栅化的最大激光范围
minimum_time_interval 在同步模式下处理的扫描之间的最短持续时间
transform_timeout 查找转换 TF 超时时间限制
tf_buffer_duration 存储 TF 消息以供查询的时间。如果在同步模式下以倍速脱机运行，则设置高一些。
stack_size_to_use 将堆栈大小重置为的字节数，以启用文件的序列化/反序列化。自由默认值为 40000000，但越少越好。
minimum_travel_distance 处理新扫描之前的最小行进距离
- 匹配器参数
use_scan_matching 是否使用扫描匹配来优化里程位姿
use_scan_barycenter 是否使用重心或扫描位姿
minimum_travel_heading 合理更新的最小航向变化
scan_buffer_size 缓冲到链中的扫描次数，也用作定位模式循环缓冲区中的扫描次数
scan_buffer_maximum_scan_distance 从缓冲区中删除之前扫描，距离之前位姿的最大距离
link_match_minimum_response_fine 精细分辨率通过的阈值链接匹配算法响应
link_scan_maximum_distance 有效链接扫描之间的最大距离
Loop_search_maximum_distance 循环闭合时考虑的扫描距离的最大阈值
do_loop_close 是否进行循环闭合（如果不确定，答案是“true”）
Loop_match_minimum_chain_size 寻找循环闭合的扫描的最小链长度
Loop_match_maximum_variance_coarse 粗略搜索中传递给细化的阈值方差
Loop_match_minimum_response_coarse 粗略搜索中环路闭合算法的阈值响应要传递给细化
Loop_match_minimum_response_fine 精细搜索中循环闭合算法的阈值响应传递给细化
correlation_search_space_dimension 搜索网格大小以进行扫描相关性
correlation_search_space_resolution 搜索网格分辨率以进行扫描相关性
correlation_search_space_smear_deviation 用于平滑响应的多模态涂抹量
loop_search_space_dimension 循环闭合算法的搜索网格的大小
loop_search_space_resolution 搜索网格分辨率以进行循环闭合
loop_search_space_smear_deviation 用于平滑响应的多模态涂抹量
distance_variance_penalty 应用于匹配扫描的惩罚，因为它与里程姿势不同
angle_variance_penalty 应用于匹配扫描的惩罚，因为它与里程姿势不同
fine_search_angle_offset 用于测试精细扫描匹配的角度范围
rough_search_angle_offset 用于测试粗略扫描匹配的角度范围
coarse_angle_resolution 在扫描匹配中测试的偏移范围内的角度分辨率
minimum_angle_penalty 确保尺寸不会膨胀的最小惩罚角度
minimum_distance_penalty 扫描可以确保大小不会爆炸的最小惩罚
use_response_expansion 如果没有找到可行的匹配，是否自动增加搜索网格大小

slam_toolbox基本使用
准备工作

在src目录下，请先调用如下指令在工作空间的src目录下创建一个功能包：

ros2 pkg create mycar_slam_slam_toolbox --dependencies slam_toolbox

编写launch文件与参数文件

在功能包下，新建launch目录和params目录，launch目录下新建online_sync_launch.py文件并输入如下内容：

import os

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time')
    slam_params_file = LaunchConfiguration('slam_params_file')

    declare_use_sim_time_argument = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation/Gazebo clock')
    declare_slam_params_file_cmd = DeclareLaunchArgument(
        'slam_params_file',
        default_value=os.path.join(get_package_share_directory("mycar_slam_slam_toolbox"),
                                   'params', 'mapper_params_online_sync.yaml'),
        description='Full path to the ROS2 parameters file to use for the slam_toolbox node')

    start_sync_slam_toolbox_node = Node(
        parameters=[
          slam_params_file,
          {'use_sim_time': use_sim_time}
        ],
        package='slam_toolbox',
        executable='sync_slam_toolbox_node',
        name='slam_toolbox',
        output='screen')

    ld = LaunchDescription()

    ld.add_action(declare_use_sim_time_argument)
    ld.add_action(declare_slam_params_file_cmd)
    ld.add_action(start_sync_slam_toolbox_node)

    return ld

该launch文件主要是加载了slam_toolbox下的sync_slam_toolbox_node节点，并且会从当前功能包的params下读取一个名为mapper_params_online_sync.yaml的配置文件。这个配置文件还不存在，接下来需要在params目录下新建mapper_params_online_sync.yaml文件，并输入如下内容：

slam_toolbox:
  ros__parameters:
    solver_plugin: solver_plugins::CeresSolver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner: SCHUR_JACOBI
    ceres_trust_strategy: LEVENBERG_MARQUARDT
    ceres_dogleg_type: TRADITIONAL_DOGLEG
    ceres_loss_function: None

    odom_frame: odom
    map_frame: map
    base_frame: base_link
    scan_topic: /scan
    mode: mapping #localization

    #map_file_name: test_steve
    #map_start_pose: [0.0, 0.0, 0.0]
    #map_start_at_dock: true

    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.02 
    map_update_interval: 2.0
    resolution: 0.05
    max_laser_range: 20.0 
    minimum_time_interval: 0.5
    transform_timeout: 0.2
    tf_buffer_duration: 30.
    stack_size_to_use: 40000000 
    enable_interactive_mode: true

    use_scan_matching: true
    use_scan_barycenter: true
    minimum_travel_distance: 0.1
    minimum_travel_heading: 0.1
    scan_buffer_size: 100
    scan_buffer_maximum_scan_distance: 10.0
    link_match_minimum_response_fine: 0.1  
    link_scan_maximum_distance: 1.5
    loop_search_maximum_distance: 3.0
    do_loop_closing: true 
    loop_match_minimum_chain_size: 10           
    loop_match_maximum_variance_coarse: 3.0  
    loop_match_minimum_response_coarse: 0.35    
    loop_match_minimum_response_fine: 0.45

    correlation_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.1 

    loop_search_space_dimension: 8.0
    loop_search_space_resolution: 0.05
    loop_search_space_smear_deviation: 0.03

    distance_variance_penalty: 0.5      
    angle_variance_penalty: 1.0    

    fine_search_angle_offset: 0.00349     
    coarse_search_angle_offset: 0.349   
    coarse_angle_resolution: 0.0349        
    minimum_angle_penalty: 0.9
    minimum_distance_penalty: 0.5
    use_response_expansion: true

配置文件的内容需要根据实际情况进行动态调整。

编辑配置文件

打开CMakeLists.txt 并输入如下内容：

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

编译

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

colcon build --packages-select mycar_slam_slam_toolbox

执行
1. 请先调用如下指令启动仿真环境：
```
1
2
. install/setup.bash
ros2 launch stage_ros2 my_house.launch.py
```
1. 然后在终端下进入当前工作空间，输入如下指令：
```
1
2
. install/setup.bash
ros2 launch mycar_slam_slam_toolbox online_sync_launch.py use_sim_time:=True
```
1. 启动rviz2，将Fixed Frame设置为map，添加map插件并将话题设置为/map，即可显示slam_toolbox创建的地图了，当机器人运动时，地图也会随之更新。
2. use_sim_time:=True参数表示使用仿真的时间。
最后需要说明的是，本节内容使用的是sync_slam_toolbox_node 节点，即以同步方式建图，而异步建图节点async_slam_toolbox_node 的使用与同步类似。

我们用键盘控制节点去控制机器人跑满整张地图，

黑色：障碍物

白色：无障碍物区

灰色：未知区

SLAM是建图与定位，以上就是建图，那么定位是啥呢？

定位就是Slam会发布一个/tf，这里面会包含机器人到map之间的坐标变换。

这个/tf发布的具体是map到odom的坐标变换，所以需要你自己去处理odom和base_link之间的坐标关系。

这样的设计，可以让整条坐标树是一个链式结构，避免base_link或者base_foot_print出现两个父类，这样计算量会变大。

cartographer概述

概念

Cartographer是Google推出的一套基于图优化的激光SLAM算法库，支持二维和三维地图的构建。它结合了激光雷达和惯性测量单元（IMU）的数据，通过高效的算法实现实时、准确的定位和建图。

功能

并行扫描匹配 利用并行计算技术加快扫描匹配速度，提高建图效率。
位姿图优化 通过图优化技术估计机器人的姿态和地图的拓扑结构，减少累积误差。
实时地图更新 在机器人移动过程中实时更新地图，确保地图的准确性和时效性。
回环检测 通过回环检测识别机器人曾经访问过的区域，进一步减少累积误差，提高地图的全局一致性。
多传感器融合 支持激光雷达、IMU、里程计等多种传感器数据的融合，提高定位和建图的精度。

优点

高效稳定 Cartographer的算法经过精心设计和优化，能够在复杂环境中高效稳定地运行。
高精度 通过图优化和回环检测技术提供高精度的定位和建图结果。
灵活性 支持二维和三维地图构建，适应不同应用场景的需求。
开源免费 Cartographer是开源项目，用户可以免费获取和使用其源代码和文档。
社区支持 拥有活跃的社区支持体系，用户可以获取来自全球开发者的帮助和支持。

cartographer安装

借助于Ubuntu的包资源管理器，可以使用二进制的方式安装cartographer，安装指令如下：

sudo apt install ros-<ros2-distro>-cartographer
sudo apt install ros-<ros2-distro>-cartographer-ros

# humble
sudo apt install ros-humble-cartographer
sudo apt install ros-humble-cartographer-ros
#jazzy
sudo apt install ros-jazzy-cartographer
sudo apt install ros-jazzy-cartographer-ros

上述两条安装指令中，前者用于安装cartographer的核心库，这个包不直接与ROS2集成，而是作为一个独立的算法库存在，为地图构建和定位提供底层的计算支持。后者则是cartographer在ROS2环境下的封装，它提供了与ROS2系统的接口，使得Cartographer算法能够在ROS2环境中运行。另外指令中的<ros2-distro>请替换成当前所使用的ROS2版本。

cartographer节点说明

在Cartographer框架中，cartographer_node和cartographer_occupancy_grid_node是两个关键的节点，它们各自承担着不同的角色和功能。详细介绍如下。

cartographer_node：

主要负责订阅来自各种传感器的数据（如激光雷达、IMU、里程计等），并基于这些数据实时构建地图。它采用子图（submap）的方法来逐步构建和更新地图，确保定位的准确性和建图的实时性。

cartographer_occupancy_grid_node：

该节点负责接收cartographer_node发布的子图列表（/submap_list），并将其拼接成完整的栅格地图（occupancy grid map），然后发布这个地图。这个节点是地图生成的最终环节，它使得Cartographer能够输出人类可读且易于可视化的地图。

这两个节点的协同工作，前者负责实时构建和更新地图，后者则负责将子图拼接成完整的栅格地图并发布，使得Cartographer能够高效地实现SLAM功能。

cartographer_node订阅的话题

cartographer_node发布的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 类型 | 描述 | |:—|:—|:—| | /scan_matched_points2 | sensors_msgs/msg/PointCloud2 | 匹配好的点云数据，用于scan-to-submap matching | | /submap_list | cartographer_ros_msgs/SubmapList | 发布构建好的子图列表 |

cartographer_node发布的服务

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 类型 | 描述 | |:—|:—|:—| | /submap_query | cartographer_ros_msgs/srv/SubmapQuery | 提供查询子图的服务，获取到查询的子图 | | /start_trajectory | cartographer_ros_msgs/srv/StartTrajectory | 开始一条轨迹 | | /finish_trajectory | cartographer_ros_msgs/srv/FinishTrajectory | 结束一条给定ID的轨迹 | | /write_state | cartographer_ros_msgs/srv/WriteState | 将当前状态写入磁盘文件中 | | /get_trajectory_states | cartographer_ros_msgs/srv/GetTrajectoryStates | 获取指定轨迹的状态 | | /read_metrics | cartographer_ros_msgs/srv/ReadMetrics | 读取性能指标 |

cartographer_node参数

cartographer_node节点需要接收一个参数配置文件，该配置文件包含了地图构建、轨迹跟踪等所需的各项参数。

cartographer_occupancy_grid_node订阅的话题

cartographer_occupancy_grid_node发布的话题

cartographer_occupancy_grid_node请求的服务

cartographer_occupancy_grid_node参数

cartographer_occupancy_grid_node节点需要配置地图的分辨率和更新周期等参数，以确保生成的栅格地图满足特定的精度和实时性要求。

cartogarpher基本使用
准备工作

在src目录下，请先调用如下指令在工作空间的src目录下创建一个功能包：

ros2 pkg create mycar_slam_cartographer --dependencies cartographer

编写launch文件与参数文件

在功能包下，新建launch目录和params目录，launch目录下新建cartographer.launch.py文件并输入如下内容：

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'True')
    resolution_arg = DeclareLaunchArgument('resolution', default_value='0.05')

    cartographer_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_node',
        parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}],
        arguments = [
            '-configuration_directory', os.path.join(get_package_share_directory("mycar_slam_cartographer"),"params"),
            '-configuration_basename', 'mycar.lua'],
        output = 'screen'
    )

    cartographer_occupancy_grid_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_occupancy_grid_node',
        parameters = [
            {'use_sim_time': LaunchConfiguration('use_sim_time')},
            {'resolution': LaunchConfiguration('resolution')}],
    )

    return LaunchDescription([
        use_sim_time_arg,
        resolution_arg,
        cartographer_node,
        cartographer_occupancy_grid_node,
    ])

该launch文件主要是加载了cartographer_ros下的cartographer_node与cartographer_occupancy_grid_node节点，并且会从当前功能包的params下读取一个名为mycar.lua的配置文件。这个配置文件还不存在，接下来需要在params目录下新建mycar.lua文件，并输入如下内容：

include "map_builder.lua" -- 地图构建器
include "trajectory_builder.lua" -- 轨迹构建器

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",  -- 地图坐标系
  tracking_frame = "base_link", -- 跟踪的坐标系，可以是基坐标系、雷达或imu的坐标系
  published_frame = "odom", -- cartographer发布的位姿（pose）的坐标系
  odom_frame = "carto_odom",  -- cartographer 计算后优化的里程计，并非机器人本身里程计
  provide_odom_frame = false, -- 是否发布cartographer的里程计
  publish_frame_projected_to_2d = true, -- 是否转换成2d(无俯仰、滚动的情况下为 true)
  use_odometry = true, -- 是否订阅里程计数据
  use_nav_sat = false, -- 是否订阅GPS
  use_landmarks = false, -- 是否订阅路标
  num_laser_scans = 1, -- 订阅的雷达的数量
  num_multi_echo_laser_scans = 0, -- 订阅的多层回波激光雷达数量
  num_subdivisions_per_laser_scan = 1, -- 将激光雷达的数据拆分成多少部分发布
  num_point_clouds = 0, -- 订阅多线激光雷达的数量
  lookup_transform_timeout_sec = 1.5, -- 坐标变换超时时间
  submap_publish_period_sec = 0.5, -- 发布子图的时间间隔
  pose_publish_period_sec = 5e-3, -- 发布pose的时间间隔
  trajectory_publish_period_sec = 30e-3, -- 发布轨迹的时间间隔
  rangefinder_sampling_ratio = 1., -- 雷达采样比例
  odometry_sampling_ratio = 0.8, -- 里程计采样比例(如果里程计精度低，可以减小该设置值)
  fixed_frame_pose_sampling_ratio = 1., -- 参考坐标系采样比例
  imu_sampling_ratio = 1.,-- imu采样比例
  landmarks_sampling_ratio = 1., -- 路标采样比例
}

MAP_BUILDER.use_trajectory_builder_2d = true -- 启用2D轨迹构建器

TRAJECTORY_BUILDER_2D.min_range = 0.15 -- 最小雷达有效距离
TRAJECTORY_BUILDER_2D.max_range = 6.0 -- 最大雷达有效距离
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3. -- 缺失数据的射线长度
TRAJECTORY_BUILDER_2D.use_imu_data = false -- 是否使用 imu 数据
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true -- 是否使用在线相关扫描匹配
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) -- 运动滤波器的最大角度限制（以弧度为单位）

POSE_GRAPH.constraint_builder.min_score = 0.65 -- 建约束时的最小分数
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7 -- 全局定位时的最小分数

-- POSE_GRAPH.optimize_every_n_nodes = 0

return options

配置文件的内容需要根据实际情况进行动态调整。

编辑配置文件

打开CMakeLists.txt 并输入如下内容：

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

编译

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

colcon build --packages-select mycar_slam_cartographer

执行

（1）请先调用如下指令启动仿真环境：

. install/setup.bash
ros2 launch stage_ros2 my_house.launch.py

（2）然后在终端下进入当前工作空间，输入如下指令：

. install/setup.bash
ros2 launch mycar_slam_cartographer cartographer.launch.py

（3）启动rviz2，将Fixed Frame设置为map，添加map插件并将话题设置为/map，即可显示创建的地图了，当机器人运动时，地图也会随之更新。

地图服务

SLAM建图时，地图数据是保存在内存中的，这也意味着，一旦节点关闭，数据也会一并被释放，而更合理的实现应该是将构建的地图序列化到的磁盘以持久化存储，并且后期还要通过反序列化读取磁盘的地图数据以做其他操作。在Nav2中已经已经封装好了地图序列化和反序列化的相关功能包，该包是：nav2_map_server。

在nav2_map_server中，可以通过话题和服务接口与Nav2系统的其余部分进行交互。nav2_map_server包下有两个重要的节点，分别是map_saver_cli和map_server，通过map_saver_cli节点则可以保存地图，而map_server节点则可以在启动时显示地图。

保存地图(序列化)
地图保存节点说明

在nav2_map_server中的地图保存节点是map_saver_server，该节点相关信息如下。

订阅的话题

参数

save_map_timeout 保存地图操作的最大等待时间。
free_thresh_default 栅格单元被认为未被占用的概率阈值。
occupied_thresh_default 栅格单元被认为占用的概率阈值。
map_subscribe_transient_local 节点重启后消息不保留，默认为 true。

map_saver_cli

另外，而为了便于使用，在map_saver_server的基础之上还封装了一个名为map_saver_cli的可执行程序，它可以以实参的方式更方便的设置地图保存相关数据，并且后续执行时也是调用map_saver_cli，其实参列表如下：

-t 订阅的地图话题。
-f 地图存储路径。
--occ 栅格单元被认为占用的概率阈值。
--free 栅格单元被认为未被占用的概率阈值。
--fmt 图片格式。
--mode 地图模式，trinary(默认)或scale或raw。

地图保存基本操作

准备工作

请先启动仿真或实体机器人，然后启动SLAM相关节点，实现基本的建图功能。

保存地图

SLAM建图完毕，在终端下进入工作空间，调用如下指令保存地图：

ros2 run nav2_map_server map_saver_cli -f map/my_map

上述指令将订阅/map话题，并把/map话题里的数据保存为文件，在工作空间下的map目录(需要自行创建该目录，否则将会抛出异常)中，生成两个文件，分别名为：my_map.yaml和my_map.pgm。

地图接口

在Nav2中地图相关的接口主要有两个：

nav_msgs/msg/MapMetaData 地图元数据，包括地图的宽度、高度、分辨率等。
nav_msgs/msg/OccupancyGrid 地图栅格数据，一般会在rviz中以图形化的方式显示。

nav_msgs/msg/MapMetaData

调用指令ros2 interface show nav_msgs/msg/MapMetaData查看接口格式，显示如下内容（注释已汉化）：

# 它包含了关于OccupancyGrid特性的基本信息
# 地图加载时间
builtin_interfaces/Time map_load_time
        int32 sec
        uint32 nanosec

# 地图分辨率 [米/像素]
float32 resolution

# 地图宽度 [像素]
uint32 width

# 地图高度 [像素]
uint32 height

#地图的原点坐标[米，米，弧度]。这是地图中单元格(0,0)左下角在现实世界中的位置和方向。
geometry_msgs/Pose origin
        Point position
                float64 x
                float64 y
                float64 z
        Quaternion orientation
                float64 x 0
                float64 y 0
                float64 z 0
                float64 w 1

nav_msgs/msg/OccupancyGrid

调用指令ros2 interface show nav_msgs/msg/OccupancyGrid查看接口格式，显示如下内容（注释已汉化）：

# 它代表一个二维网格地图。
std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id

# 地图元数据
MapMetaData info
        builtin_interfaces/Time map_load_time
                int32 sec
                uint32 nanosec
        float32 resolution
        uint32 width
        uint32 height
        geometry_msgs/Pose origin
                Point position
                        float64 x
                        float64 y
                        float64 z
                Quaternion orientation
                        float64 x 0
                        float64 y 0
                        float64 z 0
                        float64 w 1

# 地图数据按照行优先的顺序进行排列，
# 这意味着首先填充第一行的所有单元格，
# 然后填充第二行，依此类推。
# 起始单元格是(0,0)，也就是地图的左上角。
# 单元格(1, 0)紧接着(0,0)，是x方向上紧邻的下一个单元格。
# 而单元格(0, 1)则位于第一行的第二个位置，其索引等于地图的宽度（info.width），
# 然后才是(1, 1)单元格，即第二行的第二个单元格。

# 关于地图数据的值，它们根据具体的应用需求来定义。但在很多情况下，
# 会使用0表示该单元格是未占用的，即机器人可以安全通过；
# 1表示该单元格是确定占用的，即存在障碍物；
# 而-1表示该单元格的状态是未知的，即机器人尚未探测到该区域的状态。
int8[] data

地图存储格式

在 地图保存基本操作 一节中，地图保存后后生成两个文件，这两个文件就是用来存储序列化后的地图数据的。其中，my_map.pgm是一张图片资源，使用图片查看程序打开即可，而my_map.yaml保存的是地图的元数据信息，用于描述图片，内容格式如下：

image: my_map.pgm
mode: trinary
resolution: 0.05
origin: [-0.955, -10.9, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

参数解释：

image 被描述的图片资源路径，可以是绝对路径也可以是相对路径。
resolution 图片分片率(单位: m/像素)。
origin 地图中左下像素的二维姿态，为（x，y，z），偏航为逆时针旋转（偏航=0 表示无旋转）。
occupied_thresh 占用概率大于此阈值的像素被视为完全占用。
free_thresh 占用率小于此阈值的像素被视为完全空闲。
negate 是否应该颠倒白色/黑色自由/占用的语义。
mode 地图模式，trinary(默认)或scale或raw。

读取地图(反序列化)
地图读取节点说明

在nav2_map_server中的地图读取节点是map_server，该节点相关信息如下。

发布的话题

参数

frame_id 地图坐标系名称。
topic_name 话题名称。
yaml_filename 地图数据源。

地图读取基本操作

准备工作

请先调用如下指令在工作空间的src目录下创建一个功能包：

ros2 pkg create mycar_map_server --dependencies nav2_map_server

在功能包下，新建launch文件夹，并在CMakeLists.txt中添加如下配置：

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

读取地图

使用map_server读取地图时，常用的方式有两种，分别是使用终端指令与launch文件集成。两种方式效果一致，都可以以话题的方式发布地图消息。

colcon build
source ./install/setup.bash

方式1：终端指令

请在终端下进入工作空间，输入如下指令：

ros2 run nav2_map_server map_server --ros-args -p yaml_filename:=map/my_map.yaml

由于map_server是具有生命周期的节点，所以接下来还需要对节点进行配置和激活，请新开终端执行如下指令：

ros2 lifecycle set /map_server configure
ros2 lifecycle set /map_server activate

执行完毕若无异常，再调用ros2 topic list即可查看到/map话题了，说明地图消息已经被发布了。

方式2：launch集成

方式1需要手动设置map_server生命周期，步骤稍显繁琐，因此，我们还可以将该节点集成进launch文件，以简化启动步骤。在launch目录下新建名为map_server.launch.py的文件，并输入如下内容：

import os
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
  map_file = os.path.join('map', 'my_map.yaml')
  map_server_node = Node(
      package='nav2_map_server',
      executable='map_server',
      name='map_server',
      output='screen',
      parameters=[{'use_sim_time': True},
                  {'yaml_filename':map_file}]
  )
  manager_mapper_node = Node(
    package='nav2_lifecycle_manager',
    executable='lifecycle_manager',
    name='lifecycle_manager_mapper',
    output='screen',
    parameters=[{'use_sim_time': True},
      {'autostart': True},
      {'node_names': ['map_server']}]
  )
  return LaunchDescription([map_server_node,manager_mapper_node])

在该文件中，使用了功能包中nav2_lifecycle_manager中的lifecycle_manager组件，该组件可以自动的配置、激活其所管理的具有生命周期的节点。构建功能包后并执行该launch文件，其最终效果与方式一类似。

ros2 launch mycar_map_server map_server.launch.py

显示地图

打开rviz2，然后添加Map插件，并将话题设置为/map，并将该话题的Durability Policy

选项设置为Transient Local，就可以正常显示地图数据了。

AMCL自适应蒙特卡洛定位

定位是机器人在已知地图上确定自身位置的过程，为机器人的导航提供了基础信息。

Nav2中的定位技术技术称之为AMCL，全称Adaptive Monte Carlo Localization，即自适应蒙特卡洛定位，是一种基于粒子滤波器的定位算法。它通过蒙特卡洛方法进行自适应定位，利用对机器人周围环境的感知和观测数据的分析，来确定机器人在环境中的位置和姿态。在Nav2中对应的功能包为nav2_amcl。

在AMCL中，粒子滤波器的核心思想是使用一组粒子（样本）来代表机器人在地图上的可能位置。每个粒子都有一个权重，表示该粒子所代表的位置的置信度。算法会根据机器人的运动模型和传感器数据来更新这些粒子的位置和权重。随着时间的推移，粒子会逐渐收敛到机器人实际位置附近，从而实现对机器人位置的准确估计。

定位节点说明

功能包nav2_amcl中的核心节点为amcl。该节点相关信息如下。

1.订阅的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 接口 | 描述 | |:—|:—|:—| | /map | /nav_msgs/msg/OccupancyGrid | 地图数据 | | /scan | /sensor_msgs/msg/LaserScan | 激光雷达数据 | | /initialpose | /geometry_msgs/msg/PoseWithCovarianceStamped | 用来初始化粒子滤波器的均值和协方差 | | /tf | /tf2_msgs/msg/TFMessage | 坐标变换消息 |

2.发布的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 接口 | 描述 | |:—|:—|:—| | /amcl_pose | /geometry_msgs/msg/PoseWithCovarianceStamped | 机器人在地图中的位姿估计 | | /particle_cloud | /nav2_msgs/msg/ParticleCloud | 位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合 | | /tf | /tf2_msgs/msg/TFMessage | 发布从 odom 与 map 的转换 |

3.发布的服务

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 接口 | 描述 | |:—|:—|:—| | /reinitialize_global_localization | std_srvs/srv/Empty | 在全局范围内初始化粒子位姿 | | /request_nomotion_update | std_srvs/srv/Empty | 在没有运动模型更新的情况下手动触发粒子群的更新 |

4.参数

通用参数

bond_disable_heartbeat_timeout: 设置为true时，禁用amcl节点与其他节点之间基于心跳的超时检测。这通常用于当节点之间的连接非常稳定，不需要频繁的心跳检测来确认连接状态时。
base_frame_id: 定义机器人基坐标系的ID，通常是base_link或类似的名称。
global_frame_id: 定义全局地图坐标系的ID，通常是map。
odom_frame_id: 定义里程计坐标系的ID，通常是odom。
tf_broadcast: 设置为true时，amcl节点会发布从里程计坐标系到全局地图坐标系的变换。
transform_tolerance: 设置TF变换的容忍度，用于处理TF树中的时间不一致性。
use_sim_time: 设置为true时，amcl将使用ROS 2的模拟时间（如果可用）。这在仿真环境中很有用。

激光模型参数

laser_model_type: 设置激光模型类型，likelihood_field是一种常用的模型，它考虑了激光束击中障碍物的概率。
laser_max_range和laser_min_range: 分别设置激光雷达的最大和最小探测范围。
laser_likelihood_max_dist: 设置激光模型考虑的最大距离，超过这个距离的数据将被忽略。
do_beamskip和相关参数（beam_skip_distance、beam_skip_threshold、beam_skip_error_threshold）: 这些参数用于控制是否跳过某些激光束的处理，以减少计算量。然而，do_beamskip被设置为false，意味着不跳过任何激光束。

粒子滤波器参数

alpha1到alpha5: 这些参数用于控制粒子滤波器中的权重更新过程，但它们的具体作用可能因amcl的实现而异。在标准的amcl实现中，这些参数可能不是直接使用的。
max_particles和min_particles: 分别设置粒子滤波器的最大和最小粒子数。
resample_interval: 设置在重采样前需要的滤波更新次数。
pf_err和pf_z: 这些参数用于控制粒子滤波器的性能，但它们的具体作用可能依赖于amcl的实现细节。

初始位姿参数

initial_pose: 定义了机器人的初始位姿（x, y, yaw, z），但在实际使用中，如果set_initial_pose被设置为true，则这个初始位姿可能会被通过服务请求设置的初始位姿所覆盖。
set_initial_pose: 设置为true时，允许通过服务请求来设置机器人的初始位姿。
always_reset_initial_pose: 设置为false时，表示不会在每个定位会话开始时自动重置初始位姿。

其他参数

first_map_only: 当设置为false时，表示amcl将订阅并处理不断更新的地图话题。
map_topic: 定义地图话题的名称，amcl将订阅这个话题以获取地图信息。
scan_topic: 定义激光雷达扫描数据话题的名称，amcl将订阅这个话题以获取用于定位的数据。
save_pose_rate: 设置保存机器人位姿的速率（以Hz为单位）。
recovery_alpha_fast和recovery_alpha_slow: 这些参数在标准的amcl实现中可能不是直接使用的，它们可能属于某个特定版本的amcl或扩展。
z_hit、z_rand、z_short、z_max和sigma_hit: 这些参数定义了激光模型中的概率分布，用于计算激光束击中障碍物或随机位置的概率。

定位节点基本操作

1.准备工作

请先调用如下指令在工作空间的src目录下创建一个功能包：

ros2 pkg create mycar_localization --dependencies nav2_amcl mycar_map_server

2.编写launch文件与参数文件

在功能包下，新建launch和params文件夹，在launch目录下新建名为mycar_loca.launch.py的文件，并输入如下内容：

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    amcl_yaml = os.path.join(get_package_share_directory('mycar_localization'),
        'params', 'amcl.yaml')
    amcl_node = Node(
        package='nav2_amcl',
        executable='amcl',
        name='amcl',
        output='screen',
        parameters=[amcl_yaml]
    )
    manager_localization_node = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_localization',
        output='screen',
        parameters=[{'use_sim_time': True},
            {'autostart': True},
            {'node_names': ['amcl']}]
    )
    map_server_launch = IncludeLaunchDescription(
        launch_description_source=PythonLaunchDescriptionSource(
            launch_file_path=([get_package_share_directory("mycar_map_server"),"/launch/map_server.launch.py"])
        )
    )
    return LaunchDescription([amcl_node,manager_localization_node,map_server_launch])

在上述代码中，创建了amcl节点，并从params目录加载了名为amcl.yaml的配置文件，且由于amcl也是拥有生命周期的节点，所以将其添加进了生命周期管理器。最后，定位必须依赖于地图信息，因此又包含了 地图读取基本操作 中的launch文件，以加载地图。

在params目录下新建名为amcl.yaml的文件，并输入如下内容：

/**:
  ros__parameters:
    use_sim_time: True
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_link"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 2.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan
    set_initial_pose: false

关于参数的具体含义，可以参考 定位节点说明 中参数相关内容。

3.编辑配置文件

打开CMakeLists.txt 并输入如下内容：

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

4.编译

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

colcon build --packages-select mycar_localization

5.执行

（1）请先调用如下指令启动仿真环境：

. install/setup.bash
ros2 launch stage_ros2 my_house.launch.py

（2）然后在终端下进入当前工作空间，输入如下指令：

. install/setup.bash
ros2 launch mycar_localization mycar_loca.launch.py

（3）启动键盘控制节点以作备用：

ros2 run teleop_twist_keyboard teleop_twist_keyboard

（4）在rviz2中，将Fixed Frme设置为map，添加TF插件，按照 地图读取基本操作 添加并显示地图。

接下来，点击rviz2菜单栏的2D Pose Estimate在地图中为机器人设置一个初始位姿。

这里需要给一个大概的机器人位置和机器人的朝向，不是很准确也可以，机器人会在运动中逐渐通过AMCL校准。

先点击2D Pose Estimate，左键在地图上点击机器人所在位置，长摁别松手，鼠标往机器人朝向的位置划，出现下方这种绿色箭头，再松手即可。

再添加ParticleCloud插件，将话题设置为/particle_cloud，并将话题下Reliability Policy设置为Best Effort，最后使用键盘控制机器人运动时，会发现，机器人周边会出现点云，并且随着机器人的运动，点云会出现不同程度的收敛或发散。

即便你的机器人撞墙了，rviz2和Gazebo的机器人位置完全偏移了，只要再让机器人运动一会儿，机器人位置会被重新预估出来，AMCL非常强。

如上图位置已经完全偏移了。

经过一段时间行驶，姿态位置再次被预估成功。

导航服务器

本节将介绍导航服务器中核心节点，实现基本导航功能，并将导航与SLAM集成，以实现自主探索建图。

下方网站是官方NAV2的各个节点的参数说明(调参的时候非常常用的网站):

https://docs.nav2.org/configuration/index.html

看不懂的英文可以用沉浸式翻译这个chrome插件来翻译,可以进行中英文对照翻译,非常好用.

https://chromewebstore.google.com/detail/%E6%B2%89%E6%B5%B8%E5%BC%8F%E7%BF%BB%E8%AF%91-%E7%BD%91%E9%A1%B5%E7%BF%BB%E8%AF%91%E6%8F%92%E4%BB%B6-pdf%E7%BF%BB%E8%AF%91-%E5%85%8D%E8%B4%B9/bpoadfkcbjbfhfodiogcnhhhpibjhbnh?utm_source=official&pli=1

https://microsoftedge.microsoft.com/addons/detail/%E6%B2%89%E6%B5%B8%E5%BC%8F%E7%BF%BB%E8%AF%91-%E7%BD%91%E9%A1%B5%E7%BF%BB%E8%AF%91%E6%8F%92%E4%BB%B6-pdf%E7%BF%BB%E8%AF%91-/amkbmndfnliijdhojkpoglbnaaahippg?utm_source=official

不会用这个插件的话,也可以看鱼香ROS翻译好的版本(但是网站不稳定):

http://fishros.org/doc/nav2/configuration/index.html

生命周期管理节点说明

Nav2中通过的nav2_lifecycle_manager功能包下的lifecycle_manager节点管理其他节点的生命周期状态转换。lifecycle_manager节点通过ROS 2的生命周期节点（Lifecycle Node）机制，提供了一种标准化的方法来控制Nav2中各个节点的状态转换。这些状态包括未配置（Unconfigured）、非活动（Inactive）、活动（Active）和结束（Finalized）等。通过精细控制这些状态转换，lifecycle_manager节点能够确保Nav2系统的各个部分在正确的时机启动、运行和停止，从而提高系统的可靠性和稳定性。

参数

/bond_disable_heartbeat_timeout: 该参数与节点之间的通信绑定（bonding）有关。节点之间的通信可以通过绑定来增强可靠性，而心跳超时是检测节点是否仍然活跃的一种方式。设置为false意味着启用心跳超时检测。
attempt_respawn_reconnection: 取值为bool值，设置为true时，表示如果管理的节点意外终止，生命周期管理器将尝试重新启动它。
autostart: 取值为bool值，设置为true时，表示在生命周期管理器启动时，它将自动尝试启动其管理的节点。
bond_respawn_max_duration: 重新连接或重新启动节点时的最大持续时间。
bond_timeout: 与节点之间的通信绑定超时有关。如果在这个时间内没有收到来自另一个节点的消息，则认为该节点已经断开连接。
diagnostic_updater:
- period: 诊断更新器的更新周期。诊断更新器用于收集和发布有关节点状态的诊断信息，定义了这些信息更新的频率。
node_names: 由该生命周期管理器管理的节点名称列表。
use_sim_time:是否使用仿真实践。

行为树节点说明

行为树（BT） 是一种在智能体（如机器人或电脑游戏中的虚拟实体）中构建不同任务之间切换结构的方式。它是一种形式化的图形建模语言，以层次化的节点组织为特征，用于描述和规划复杂系统中各种实体的交互和决策。nav2_bt_navigator功能包下的/bt_navigator是Nav2中的行为树导航器节点，它实现了基于行为树的导航策略。行为树是一种用于描述复杂行为的树状结构，通过组合不同的行为节点，可以灵活地定义机器人的导航行为，包括路径规划、避障、恢复等。以下是官网。

https://www.behaviortree.dev/

https://arxiv.org/abs/1709.00084

github链接如下：

https://github.com/BehaviorTree/BehaviorTree.CPP

BT可视化工具Groot2下载：

https://www.behaviortree.dev/groot/

订阅的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 接口 | 描述 | |:—|:—|:—| | /goal_pose | geometry_msgs/msg/PoseStamped | 导航目标点，用于触发导航任务 | | /tf | tf2_msgs/msg/TFMessage | 坐标变换消息，用于不同坐标系之间的转换 | | /odom | nav_msgs/msg/Odometry | 里程计数据，提供机器人位置和运动信息 |

请求的Action

参数

use_sim_time: 指定是否使用模拟时间而非实际时间。
global_frame: 定义全局坐标系的名称，通常为地图坐标系。
robot_base_frame: 指定机器人基座的坐标系名称。
odom_topic: 里程计数据的ROS话题名称。
bt_loop_duration: 行为树执行循环的持续时间（单位可能根据实现而异）。
default_server_timeout: 导航服务器操作的默认超时时间。
enable_groot_monitoring: 启用或禁用Groot监控功能。
groot_zmq_publisher_port: Groot监控中ZeroMQ发布者的端口号。
groot_zmq_server_port: Groot监控中ZeroMQ服务器的端口号。
plugin_lib_names: 包含导航所需插件的库名称列表。

规划器节点说明

在Nav2 导航框架中nav2_planner功能包下的planner_server节点，负责处理路径规划请求，生成从当前位置到目标位置的路径。该节点在执行时需要依赖于/global_costmap/global_costmap节点提供的地图消息。

planner_server发布的话题

planner_server 参数

/bond_disable_heartbeat_timeout: 这个参数控制是否禁用心跳超时检测，在ROS 2中，节点之间的通信绑定（bonding）机制用于增强通信的可靠性。心跳超时是检测节点是否仍然活跃的一种机制。如果该参数设置为true，则表示禁用了心跳超时检测，这可能在某些特定的网络环境下或当确信节点间通信非常稳定时使用。
GridBased: 这是一个配置块，专门用于设置基于网格的规划器的参数。基于网格的规划器通常使用地图的网格化表示来规划路径。
- allow_unknown: 控制规划器是否允许在地图的未知（即未探索）区域中规划路径。
- plugin: 指定使用的规划器插件。
- tolerance: 设置规划路径时的容忍度，通常用于考虑机器人尺寸和定位的不确定性。
- use_astar: 控制是否使用A* 算法进行路径规划。A* 算法是一种启发式搜索算法，能够找到从起点到终点的最短路径。
- use_final_approach_orientation: 控制规划器是否在路径的终点附近考虑机器人的最终朝向。
expected_planner_frequency: 这个参数表示对规划器生成新路径的频率的预期值。它帮助系统监控规划器的性能，并可能用于调试或性能优化。
planner_plugins: 这是一个列表，指定了可用的规划器插件。在这个例子中，它只包含了一个GridBased规划器，但理论上可以包含多个不同类型的规划器，以适应不同的导航需求。
use_sim_time: 这个参数控制是否使用模拟时间。在仿真环境中，时间是由仿真软件控制的，而不是由实际的物理时钟控制的。将此参数设置为true允许节点在仿真环境中正常运行，而无需依赖实际的系统时间。这对于开发和测试导航算法非常有用。

/global_costmap/global_costmap订阅的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 接口 | 描述 | |:—|:—|:—| | /global_costmap/footprint | geometry_msgs/msg/Polygon | 机器人（或任何移动平台）的足迹（footprint）信息。足迹是机器人在地图上占据的空间形状，通常用多边形表示。 | | /map | nav_msgs/msg/OccupancyGrid | 发布环境地图，特别是用于导航的占用网格图（Occupancy Grid Map）。 | | /scan | sensor_msgs/msg/LaserScan | 激光扫描数据。 |

/global_costmap/global_costmap发布的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 接口 | 描述 | |:—|:—|:—| | /global_costmap/costmap | nav_msgs/msg/OccupancyGrid | 发布全局代价地图的当前状态。 | | /global_costmap/costmap_raw | nav2_msgs/msg/Costmap | 未经进一步处理的原始代价地图数据。 | | /global_costmap/costmap_updates | map_msgs/msg/OccupancyGridUpdate | 全局代价地图的更新，该消息可以高效更新地图。 | | /global_costmap/published_footprint | geometry_msgs/msg/PolygonStamped | 发布机器人的足迹（footprint），即机器人在地图上占据的空间形状。 |

/global_costmap/global_costmap参数

/bond_disable_heartbeat_timeout: 控制是否禁用心跳超时检测，这在节点间通信绑定时用于监控对方节点的活跃度。
always_send_full_costmap: 控制是否总是发送完整的代价地图信息，而不是仅发送变化的部分。
clearable_layers: 列出可以被清除的代价图层，这些图层中的障碍物信息可以通过某种方式（如传感器数据）被更新或清除。
filters: 定义应用于代价地图的过滤器列表，用于预处理或修改地图数据。
footprint: 指定机器人在地图上的足迹形状，即机器人占据的空间范围。
footprint_padding: 为机器人的足迹添加额外的填充空间，以考虑机器人运动时的额外空间需求。
global_frame: 定义全局代价地图所使用的参考坐标系。
height和width: 定义全局代价地图的高度和宽度（以单元格数计）。
inflation_layer: 配置膨胀层的参数，膨胀层用于在障碍物周围增加一定宽度的“缓冲区”，以避免机器人与障碍物过近。
- cost_scaling_factor: 膨胀成本的缩放因子。
- enabled: 控制膨胀层是否启用。
- inflate_around_unknown: 控制是否在未知空间周围进行膨胀。
- inflate_unknown: 控制是否将未知空间视为障碍物并进行膨胀。
- inflation_radius: 膨胀的半径。
- plugin: 指定使用的膨胀层插件。
lethal_cost_threshold: 定义代价地图中视为“致命”障碍物的成本阈值。
map_topic: 指定订阅以获取地图信息的ROS话题。
observation_sources: 定义代价地图的观测源，即哪些传感器或数据源用于更新地图。
obstacle_layer: 配置障碍物层的参数，障碍物层负责处理传感器观测到的障碍物信息。
- combination_method: 定义如何组合多个观测源的信息。
- enabled: 控制障碍物层是否启用。
- footprint_clearing_enabled: 控制是否清除机器人足迹内的障碍物信息。
- max_obstacle_height和min_obstacle_height: 定义障碍物的高度范围。
- observation_sources: 指定障碍物信息的来源。
- plugin: 指定使用的障碍物层插件。
- scan: 包含与激光扫描相关的配置，如扫描数据的处理方式。
origin_x和origin_y: 定义全局代价地图原点的坐标。
plugins: 列出启用的代价地图插件，这些插件定义了如何构建和更新代价地图。
publish_frequency: 定义发布代价地图的频率。
resolution: 定义代价地图的分辨率，即每个单元格代表的实际物理尺寸。
robot_base_frame: 定义机器人基座的参考坐标系。
robot_radius: 定义机器人的半径，用于计算机器人在地图上的占用空间。
rolling_window: 控制是否使用滚动窗口（即动态变化的地图区域）而非固定大小的地图。
static_layer: 配置静态层的参数，静态层负责处理地图中的静态障碍物信息。
- enabled: 控制静态层是否启用。
- map_subscribe_transient_local: 控制是否订阅瞬态本地地图更新。
- map_topic: 指定静态地图的ROS话题（如果不同于全局地图）。
- plugin: 指定使用的静态层插件。
- subscribe_to_updates: 控制是否订阅地图更新。
- transform_tolerance: 定义坐标变换的容忍度。
track_unknown_space: 控制是否跟踪地图中的未知空间。
transform_tolerance: 定义在坐标变换过程中允许的误差范围。
trinary_costmap: 控制是否使用三态代价地图（空闲、占用、未知），而不是仅使用二态（空闲、占用）。
unknown_cost_value: 定义在代价地图中表示未知空间的值。
update_frequency: 定义更新代价地图的频率。
use_maximum: 控制是否使用多个观测源中的最大值来更新代价地图。
use_sim_time: 控制是否使用仿真时间（在仿真环境中很有用）。
voxel_layer: 配置体素层的参数，体素层使用体素网格来表示三维空间中的障碍物信息。
- enabled: 控制体素层是否启用。
- footprint_clearing_enabled: 控制是否清除机器人足迹内的体素信息。
- mark_threshold和unknown_threshold: 定义将体素视为障碍物或未知的阈值。
- max_obstacle_height和min_obstacle_height: 定义体素表示的障碍物的高度范围。
- observation_sources: 指定体素信息的来源。
- origin_z: 定义体素网格在Z轴上的原点。
- plugin: 指定使用的体素层插件。
- publish_voxel_map: 控制是否发布体素地图。
- scan: 包含与激光扫描相关的配置，如扫描数据的处理方式。
- z_resolution和z_voxels: 定义体素网格在Z轴上的分辨率和体素数。

控制器节点说明

在Nav2导航系统中nav2_controller功能包的controller_server负责处理导航任务中的控制请求，确保机器人能够按照规划的路径进行移动。其主要功能是根据nav2_planner模块计算出的全局或局部路径，生成速度、方向控制的命令，即控制机器人沿着规划好的路径行走。该节点在执行时还需要依赖于/local_costmap/local_costmap节点提供的地图消息。

1.controller_server订阅的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 接口 | 描述 | |:—|:—|:—| | /odom | nav_msgs/msg/Odometry | 机器人的里程计信息，包含位置、速度和姿态 | | /speed_limit | nav2_msgs/msg/SpeedLimit | 导航过程中的速度限制信息，用于动态调整机器人的移动速度 |

2.controller_server发布的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题名称 | 消息类型 | 描述 | |:—|:—|:—| | /cmd_vel_nav | geometry_msgs/msg/Twist | 发布控制命令，包括线性和角速度，用于控制机器人按照规划路径移动。 | | /cost_cloud | sensor_msgs/msg/PointCloud2 | 发布成本地图中的点云数据，用于避障和路径规划。 | | /local_plan | nav_msgs/msg/Path | 发布局部路径规划结果，即机器人应如何到达当前目标点附近的一个点。 | | /marker | visualization_msgs/msg/MarkerArray | 发布可视化标记，用于在RViz等可视化工具中显示路径、障碍物等信息。 | | /received_global_plan | nav_msgs/msg/Path | 发布从全局规划器接收到的全局路径，即当前位置到目标点的路径。 | | /transformed_global_plan | nav_msgs/msg/Path | 发布经过坐标变换的全局路径，确保路径与机器人的当前坐标系一致。 |

3.controller_server参数

FollowPath: 这个部分定义了一个名为FollowPath的插件或配置集，它可能是一个路径跟随行为或算法的配置。它包含了多个子参数和子配置，用于定义如何跟随路径。
- BaseObstacle: 定义了基本的障碍物评估参数，用于在路径跟随过程中避免障碍物。
  - class: 指定了类的名称，这里是BaseObstacle，表示这是一个基本障碍物评估组件。
  - scale: 定义了该障碍物评估在整体评估中的权重或影响程度。
  - sum_scores: 指示是否累加多个障碍物的分数，false可能表示使用最大值或其他逻辑。
- GoalAlign, GoalDist, PathAlign, PathDist, RotateToGoal, Oscillation: 这些都是路径跟随过程中的不同评估或行为组件，每个都有其特定的参数和用途，如对齐目标、保持与目标或路径的距离、减少振荡等。
- acc_lim_theta, acc_lim_x, acc_lim_y: 这些参数定义了机器人在不同方向上的加速度限制。
- critics: 指定了哪些评估组件（或“批评家”）将被用于路径跟随决策。
- debug_trajectory_details: 指示是否发布轨迹的详细调试信息。
- 其他与速度、加速度、时间粒度、轨迹生成等相关的参数，共同定义了路径跟随算法的行为和性能。
controller_frequency: 指定了控制器（可能是FollowPath或其他控制器）的运行频率，以赫兹为单位。
controller_plugins: 指定了将要使用的控制器插件列表，这里只包含了FollowPath。
failure_tolerance: 定义了容忍失败的时间或距离，用于在评估控制器是否失败时提供一定的缓冲。
general_goal_checker: 定义了一个通用的目标检查器，用于确定机器人是否已达到其目标位置和方向。
goal_checker_plugins: 指定了将要使用的目标检查器插件列表。
min_theta_velocity_threshold, min_x_velocity_threshold, min_y_velocity_threshold: 这些定义了机器人在不同方向上的最小速度阈值，低于这些阈值可能被视为停止或静止。
odom_topic: 指定了里程计信息的ROS主题。
progress_checker: 定义了一个进度检查器，用于评估机器人是否在向目标移动。
qos_overrides: 定义了ROS服务或主题的QoS（服务质量）覆盖设置，用于调整消息传递的可靠性和性能。
speed_limit_topic: 指定了速度限制信息的ROS主题。
use_sim_time: 指示是否使用模拟时间，这在ROS仿真环境中非常有用。

4./local_costmap/local_costmap订阅的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 接口 | 描述 | |:—|:—|:—| | /local_costmap/footprint | geometry_msgs/msg/Polygon | 机器人或移动平台的足迹多边形，用于本地代价地图的计算 | | /scan | sensor_msgs/msg/LaserScan | 激光扫描仪的扫描数据，用于环境感知和避障 |

5./local_costmap/local_costmap发布的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 接口 | 描述 | |:—|:—|:—| | /local_costmap/clearing_endpoints | sensor_msgs/msg/PointCloud2 | 清除成本图上的障碍物点云数据，通常用于动态障碍物处理 | | /local_costmap/costmap | nav_msgs/msg/OccupancyGrid | 本地成本图，表示机器人周围环境的可通行性 | | /local_costmap/costmap_raw | nav2_msgs/msg/Costmap | 未经处理的本地成本图，可能包含更详细的信息 | | /local_costmap/costmap_updates | map_msgs/msg/OccupancyGridUpdate | 本地成本图的更新信息，包括哪些区域发生了变化 | | /local_costmap/published_footprint | geometry_msgs/msg/PolygonStamped | 发布的机器人足迹多边形，时间戳表示发布时间 | | /local_costmap/voxel_grid | nav2_msgs/msg/VoxelGrid | 体素网格数据，用于成本图生成中的空间划分和优化 |

6./local_costmap/local_costmap参数

/bond_disable_heartbeat_timeout: 是否禁用节点间的心跳超时检查。当设置为true时，表示禁用该功能，可能用于减少网络通信量或适应特定网络环境。
always_send_full_costmap: 是否总是发送完整的成本图。当设置为true时，节点将不依赖于增量更新，而是始终发送完整的成本图数据。
clearable_layers: 指定可以被清除的层列表。在这个例子中，包括obstacle_layer、voxel_layer和range_layer，这意味着这些层中的障碍物数据可以被清除。
filters: 用于指定应用于成本图的过滤器列表。此处为空，表示没有应用任何过滤器。
footprint: 机器人的足迹多边形，定义了机器人在二维空间中的物理占用区域。
footprint_padding: 足迹的填充量，用于在计算成本图时给机器人足迹添加额外的空间。
global_frame: 全局参考坐标系的名称，通常用于定位和导航任务。
height: 成本图的高度（以单元格数量计）。
inflation_layer: 膨胀层的配置，用于在障碍物周围添加一定范围的膨胀区域，使机器人与障碍物保持安全距离。
lethal_cost_threshold: 致命成本阈值，超过此阈值的成本值表示不可通行的区域。
map_topic: 订阅的地图主题名称，用于获取全局地图信息。
observation_sources: 观察源的配置，用于指定哪些传感器数据将被用于更新成本图。此处为空字符串，可能是默认值或配置方式的不同。
obstacle_layer: 障碍物层的配置，用于处理来自传感器（如激光雷达）的障碍物数据。
origin_x,origin_y: 成本图原点的X和Y坐标，定义了成本图在全局坐标系中的位置。
plugins: 启用的插件列表，定义了成本图使用的不同层（如障碍物层、膨胀层等）。
publish_frequency: 成本图的发布频率（以Hz为单位）。
resolution: 成本图的分辨率（以米/单元格计）。
robot_base_frame: 机器人基座的参考坐标系名称，用于定位机器人。
robot_radius: 机器人的半径，用于在成本图中表示机器人的物理尺寸。
rolling_window: 是否使用滚动窗口。当设置为true时，成本图将随着机器人的移动而更新其位置和范围。
track_unknown_space: 是否跟踪未知空间。在某些情况下，这可能用于处理未探索或未知的区域。
transform_tolerance: 变换容差，定义了接受变换的时间差和角度差的阈值。
trinary_costmap: 是否使用三态成本图（通常是自由、占用、未知）。
unknown_cost_value: 未知区域在成本图中的成本值。
update_frequency: 成本图的更新频率（以Hz为单位），不同于发布频率。
use_maximum: 是否在多个源提供相同位置的成本信息时使用最大值。
use_sim_time: 是否使用模拟时间而非系统时间。这在仿真环境中很有用。
voxel_layer: 体素层的配置，用于将三维空间划分为体素（体积像素），以提高成本图的处理效率。

恢复器节点说明

恢复行为是机器人导航过程中一个至关重要的部分，它允许机器人在遇到障碍、卡住或其他导航问题时采取一系列预定义的动作来尝试恢复。在Nav2中由nav2_behaviors功能包的behavior_server实现这一功能。

1.订阅的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 接口 | 描述 | |:—|:—|:—| | /clock | rosgraph_msgs/msg/Clock | ROS系统时间 | | /cmd_vel_teleop | geometry_msgs/msg/Twist | 遥操作命令，用于控制机器人的线性和角速度 | | /local_costmap/costmap_raw | nav2_msgs/msg/Costmap | 局部代价地图的原始数据 | | /local_costmap/published_footprint | geometry_msgs/msg/PolygonStamped | 机器人在局部代价地图中的已发布足迹 | | /preempt_teleop | std_msgs/msg/Empty | 遥操作抢占信号，用于中断当前遥操作 |

2.发布的话题

3.提供的Action服务器

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | Action接口 | 描述 | |:—|:—|:—| | /assisted_teleop | nav2_msgs/action/AssistedTeleop | 遥控辅助操作服务，允许用户在导航时提供方向性输入 | | /backup | nav2_msgs/action/BackUp | 后退动作服务，用于在特定情况下使机器人后退 | | /drive_on_heading | nav2_msgs/action/DriveOnHeading | 按指定航向行驶的动作服务 | | /spin | nav2_msgs/action/Spin | 旋转动作服务，允许机器人在原地旋转 | | /wait | nav2_msgs/action/Wait | 等待动作服务，使机器人在当前位置等待一定时间 |

4.参数

use_sim_time: 该参数指定是否使用模拟时间而非实际时间。这在仿真环境中非常有用，因为仿真环境可以加速或减速时间流逝，而不需要等待实际时间的流逝。
global_frame: 定义全局坐标系的名称，该坐标系通常用于导航任务中的定位和路径规划。在这里，它被设置为odom，意味着使用里程计数据来作为全局坐标系的参考。
robot_base_frame: 指定机器人基座的坐标系名称。这是机器人上用于定位和运动控制的参考点，通常与机器人的物理中心或驱动轮的中心相对应。
odom_topic: 这是一个ROS话题名称，用于发布里程计数据。里程计数据包含了机器人随时间推移的位置和姿态变化信息，是导航和定位系统的关键输入之一。
/bond_disable_heartbeat_timeout：这个参数可能用于配置ROS节点之间的心跳检测机制。将其设置为true可能意味着禁用或调整心跳超时的行为，以便在特定情况下（如仿真环境）避免不必要的超时错误。
assisted_teleop,backup,drive_on_heading,spin,wait: 这些是行为树中可能使用的行为插件的配置项。每个插件都定义了机器人可以执行的一种特定行为，如辅助遥操作、后退、按指定方向行驶、原地旋转和等待。
behavior_plugins: 列出了在行为树中可用的行为插件名称。
cmd_vel_teleop: 指定了用于遥操作的速度控制命令的ROS话题名称。
costmap_topic: 定义了局部代价地图的ROS话题名称，代价地图用于表示环境中的障碍物和可通行区域。
cycle_frequency: 定义了导航系统更新其状态和规划新路径的频率（以赫兹为单位）。
max_rotational_vel,min_rotational_vel,rotational_acc_lim: 这些参数定义了机器人旋转时的最大速度、最小速度和加速度限制。
projection_time: 与代价地图的更新或预测未来障碍物位置有关的时间参数。
footprint_topic: 定义了发布机器人足迹（即机器人占据的空间）的ROS话题名称。
simulate_ahead_time,simulation_time_step: 这些参数与仿真环境相关，可能用于控制仿真过程中的时间流逝和步长。
transform_tolerance: 定义了坐标变换时的容差范围，用于处理不同坐标系之间的微小差异。

航点跟随节点说明

在Nav2 导航堆栈中，nav2_waypoint_follower包下的/waypoint_follower节点负责跟踪由路径规划器生成的一系列航点（waypoints），以确保机器人能够沿着预定的路径安全、准确地移动。该节点的主要功能是根据当前机器人位置和速度信息，以及由路径规划器（如nav2_global_planner和nav2_local_planner）提供的航点列表，计算出控制指令来控制机器人的运动。这些控制指令可能包括线性和角速度命令，或者更具体的运动学或动力学命令，具体取决于机器人的类型和配置。

1.提供的Action服务器

2.请求的Action服务

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | Action接口 | 描述 | |:—|:—|:—| | /navigate_to_pose | nav2_msgs/action/NavigateToPose | 允许planner_server（或调用它的节点）请求导航到指定的位姿 |

3.参数

use_sim_time: 指定是否使用模拟时间而非实际时间进行节点的计时和同步。这在仿真环境中特别有用，因为仿真环境可能无法提供与真实时间完全同步的时钟。
loop_rate: 定义了节点的主循环速率，即节点每秒执行其主要任务（如处理数据、发布信息等）的次数。这个参数对于控制节点的响应性和资源使用非常重要。
stop_on_failure: 指明当导航任务遇到无法克服的障碍或达到其他失败条件时，节点是否应该停止执行。这对于确保在失败情况下系统能够安全地停止并等待进一步指令很重要。
bond_disable_heartbeat_timeout: 涉及节点间通信的可靠性机制。Bond是ROS 2中用于节点间稳定通信的一种机制，其中心跳信号用于检测节点是否仍然活跃。将此参数设置为true会禁用心跳超时检测，这可能在某些特定的网络配置或应用场景中是有用的。
waypoint_task_executor_plugin: 指定了在执行路径点导航任务时要使用的插件。路径点导航通常涉及一系列预先定义的点，机器人需要按顺序访问这些点。这个参数允许用户指定用于执行这种类型任务的特定插件或算法。
wait_at_waypoint: 这是一个复合参数，用于配置在路径点等待的特定行为。
- enabled: 启用或禁用在到达每个路径点时等待的功能。
- plugin: 指定实现等待功能的插件类型。这允许用户根据需要选择不同的等待策略或行为。
- waypoint_pause_duration: 定义了在每个路径点处等待的持续时间（以毫秒为单位）。这可以用于确保机器人在移动到下一个路径点之前已经稳定或已经完成了某些操作。

路径平滑节点说明

在Nav2框架中nav2_smoother功能包下的smoother_server节点通过加载和运行各种平滑器插件，对规划出的路径进行平滑处理，使得机器人能够更流畅、连续且安全地移动。这一功能对于提高机器人的导航性能和减少硬件磨损具有重要意义。

1.订阅的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 接口 | 描述 | |:—|:—|:—| | /global_costmap/costmap_raw | nav2_msgs/msg/Costmap | 全局代价地图的原始数据，用于路径规划 | | /global_costmap/published_footprint | geometry_msgs/msg/PolygonStamped | 机器人在全局代价地图中的足迹表示 |

2.发布的话题

3.提供的Action服务器

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 动作类型 | 描述 | |:—|:—|:—| | /smooth_path | nav2_msgs/action/SmoothPath | 提供平滑路径的服务，接受路径平滑的请求，并返回平滑后的路径。这允许客户端（如行为树）异步地请求路径平滑，并在平滑完成后接收结果。 |

4.参数

/bond_disable_heartbeat_timeout: 指示是否禁用Bond的心跳超时功能。在分布式系统中，Bond用于管理节点间的连接和心跳，此参数用于调整心跳相关的行为。
costmap_topic: 代价地图数据的ROS话题名称，通常是全局代价地图的原始数据。
footprint_topic: 机器人足迹（即机器人在地图上的占用区域）的ROS话题名称，用于在全局代价地图中表示机器人的物理尺寸。
robot_base_frame: 指定机器人基座的坐标系名称，这是机器人导航中用于定位和移动的参考点。
simple_smoother:
- do_refinement: 指示是否启用路径的细化（或进一步优化）过程。
- max_its: 平滑过程中允许的最大迭代次数，用于控制平滑算法的收敛时间。
- plugin: 平滑插件的类型，这里是nav2_smoother::SimpleSmoother，表示使用简单的平滑算法。
- tolerance: 平滑算法的收敛容差，当路径变化小于此值时，认为平滑过程已完成。
- w_data: 平滑过程中数据项（如障碍物距离）的权重。
- w_smooth: 平滑过程中平滑项（如路径曲率）的权重。
smoother_plugins: 定义的平滑插件列表，这里列出了simple_smoother，表示将使用此插件进行路径平滑。
transform_tolerance: 坐标变换的容差，用于处理不同坐标系之间的转换时的不确定性。
use_sim_time: 指定是否使用模拟时间而非实际时间。在仿真环境中，这通常设置为true，以匹配仿真器的虚拟时间；在真实环境中，应设置为false以使用ROS系统的实际时间。

速度平滑节点说明

Nav2框架中的nav2_velocity_smoother包下的velocity_smoother节点主要负责平滑由Nav2框架发送给机器人控制器的速度指令。其核心功能是实现速度和加速度平滑。这一功能对于确保机器人在导航过程中的稳定性和安全性至关重要。

1.订阅的话题

2.发布的话题

3.参数

bond_disable_heartbeat_timeout: 指示是否禁用节点间的心跳超时机制。如果为true，则节点间的心跳检测不会因超时而断开连接。
deadband_velocity: 定义在哪些速度分量上应用死区平滑（即忽略小于此阈值的微小速度变化）。这里分别为X轴、Y轴和偏航角速度（theta）设置了死区值。
feedback: 指定速度平滑器的反馈类型。OPEN_LOOP表示开环控制，即不考虑机器人的实际速度反馈进行速度调整。
max_accel: 定义机器人在各个方向上的最大加速度限制，包括X轴、Y轴和偏航角速度（theta）。
max_decel: 定义机器人在各个方向上的最大减速度限制，包括X轴、Y轴和偏航角速度（theta）。注意，减速度值以负数表示。
max_velocity: 定义机器人在各个方向上的最大速度限制，包括X轴、Y轴和偏航角速度（theta）。
min_velocity: 定义机器人在各个方向上的最小速度限制，包括X轴、Y轴和偏航角速度（theta）。这通常用于避免发送过小的速度指令给底层控制器。
odom_duration: 与里程计数据相关的参数，但在此上下文中可能不直接用于velocity_smoother节点，可能是遗留或与其他功能相关联。
odom_topic: 指定里程计数据的ROS话题名称，velocity_smoother节点将订阅此话题以获取机器人的运动信息。
scale_velocities: 指示是否根据加速度限制同比例调整速度的其他分量。如果为false，则不会进行速度缩放。
smoothing_frequency: 定义速度平滑操作的执行频率（Hz），即每秒进行多少次平滑计算。
use_sim_time: 指定是否使用模拟时间而非实际系统时间。这对于仿真环境特别有用，可以确保时间的一致性和可预测性。
velocity_timeout: 如果在指定时间内未接收到新的速度指令，则velocity_smoother节点将停止发布速度指令，并可能发送零速度指令以停止机器人运动。这是为了防止在失去速度控制时机器人继续移动。

导航功能集成(重要)

1.准备工作

请先调用如下指令在工作空间的src目录下创建一个功能包：

ros2 pkg create mycar_navigation2 --dependencies navigation2 nav2_common

2.编写launch文件与参数文件

在功能包下，新建launch目录、params目录和bts目录。

launch目录下新建nav2.launch.py文件并输入如下内容：

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():

    current_pkg = get_package_share_directory("mycar_navigation2")
    bt_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","bt.yaml")
    planner_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","planner.yaml")       
    controller_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","controller.yaml")       
    behavior_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","behavior.yaml")       
    waypoint_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","waypoint.yaml")       
    velocity_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","velocity.yaml")       
    smoother_params = os.path.join(get_package_share_directory("mycar_navigation2"),"params","smoother.yaml")       

    planner_server_node = Node(
        package='nav2_planner',
        executable='planner_server',
        name='planner_server',
        output='screen',
        parameters=[planner_params],
        )

    controller_server_node = Node(
        package='nav2_controller',
        executable='controller_server',
        output='screen',
        parameters=[controller_params],
        remappings=[('cmd_vel', 'cmd_vel_nav')]
    )

    behavior_server_node = Node(
        package='nav2_behaviors',
        executable='behavior_server',
        name='behavior_server',
        output='screen',
        parameters=[behavior_params]
    )

    waypoint_node = Node(
        package='nav2_waypoint_follower',
        executable='waypoint_follower',
        name='waypoint_follower',
        output='screen',
        parameters=[waypoint_params]
    )

    velocity_smoother_node = Node(
        package='nav2_velocity_smoother',
        executable='velocity_smoother',
        name='velocity_smoother',
        output='screen',
        respawn_delay=2.0,
        parameters=[velocity_params],
        remappings=
                [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]
    )
    smoother_server_node = Node(
        package='nav2_smoother',
        executable='smoother_server',
        name='smoother_server',
        output='screen',
        parameters=[smoother_params],
    )
    bt_navigator_node = Node(
        package='nav2_bt_navigator',
        executable='bt_navigator',
        name='bt_navigator',
        output='screen',      
        parameters=[
            bt_params,
            {"default_nav_to_pose_bt_xml": os.path.join(current_pkg,"bts","bt_planner_controller_behavior.xml")},
            {"default_nav_through_poses_bt_xml": os.path.join(current_pkg,"bts","bt_planner_controller_behavior_poses.xml")}
            ],
        )

    lifecycle_manager_node = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_navigation',
        output='screen',
        parameters=[{'use_sim_time': True},
                    {'autostart': True},
                    {'node_names': [
                        'bt_navigator',
                        'planner_server',
                        'controller_server',
                        'behavior_server',
                        'waypoint_follower',
                        'velocity_smoother',
                        'smoother_server'
                    ]}])

    return LaunchDescription([
        lifecycle_manager_node,
        bt_navigator_node,
        planner_server_node,
        controller_server_node,
        behavior_server_node,
        waypoint_node,
        velocity_smoother_node,
        smoother_server_node
    ])

该launch文件主要是加载了生命周期管理器、行为树、规划器、控制器、恢复器、航点跟踪、路径平滑以及速度平滑等节点。并且除了生命周期管理器节点外，每个节点都还会加载一个配置文件，接下来需要编辑这些配置文。

（1）bt_navigator相关配置文件

bt_navigator相关配置文件有两个，分别是描述行为树的xml文件，以及yaml格式的参数文件，前者存储在bts目录下，后者存储在params目录下。

请在bts目录下，新建一个名为bt_planner_controller_behavior.xml的文件并输入如下内容：

<!--
  This Behavior Tree replans the global path periodically at 1 Hz and it also has
  recovery actions specific to planning / control as well as general system issues.
  This will be continuous if a kinematically valid planner is selected.
-->
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <RoundRobin name="RecoveryActions">
          <Sequence name="ClearingActions">
            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
          </Sequence>
          <Spin spin_dist="1.57"/>
          <Wait wait_duration="5"/>
          <BackUp backup_dist="0.30" backup_speed="0.05"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

继续在bts目录下新建一个名为bt_planner_controller_behavior_poses.xml的文件，并输入如下内容：

<!--
  This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
   and it also has recovery actions specific to planning / control as well as general system issues.
-->
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="0.333">
          <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
            <ReactiveSequence>
              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
              <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
            </ReactiveSequence>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <RoundRobin name="RecoveryActions">
          <Sequence name="ClearingActions">
            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
          </Sequence>
          <Spin spin_dist="1.57"/>
          <Wait wait_duration="5"/>
          <BackUp backup_dist="0.30" backup_speed="0.05"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

在params目录下新建一个名为bt.yaml的文件，并输入如下内容：

/**:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    bt_loop_duration: 10
    default_server_timeout: 20
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_compute_path_through_poses_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node
    - nav2_single_trigger_bt_node
    - nav2_is_battery_low_condition_bt_node
    - nav2_navigate_through_poses_action_bt_node
    - nav2_navigate_to_pose_action_bt_node
    - nav2_remove_passed_goals_action_bt_node
    - nav2_planner_selector_bt_node
    - nav2_controller_selector_bt_node
    - nav2_goal_checker_selector_bt_node

（2）planner_server相关配置文件

在params目录下新建一个名为planner.yaml的文件，并输入如下内容：

/**:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

/**:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      # robot_radius: 0.2
      footprint: "[[0.19, 0.13], [0.19, -0.13], [-0.19, -0.13], [-0.19, 0.13]]"
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 4.0
        inflation_radius: 0.55
      always_send_full_costmap: False

（3）controller_server相关配置文件

在params目录下新建一个名为controller.yaml的文件，并输入如下内容：

/**:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    # failure_tolerance: 0.3
    failure_tolerance: 1.0
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] 
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0

    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25

    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      # max_vel_x: 0.15
      max_vel_x: 0.2
      max_vel_y: 0.0
      # max_vel_theta: 1.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.2
      min_speed_theta: 0.0
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      acc_lim_x: 1.0
      acc_lim_y: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -1.0
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      # vx_samples: 20
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      # transform_tolerance: 0.2
      transform_tolerance: 1.0
      xy_goal_tolerance: 0.15
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0
/**:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: True
      width: 2
      height: 2
      resolution: 0.05
      # robot_radius: 0.20
      footprint: "[[0.19, 0.13], [0.19, -0.13], [-0.19, -0.13], [-0.19, 0.13]]"
      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.5
        cost_scaling_factor: 4.0
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True

Footprint

将毫米转换为米：长0.96m，宽0.45m
以机器人中心（base_link坐标系原点）为基准，计算矩形顶点坐标：

说明：顶点按顺时针或逆时针顺序排列，覆盖机器人长宽边界。（右手坐标系）

例如长960宽450长400

footprint: [[0.48, 0.225], [0.48, -0.225], [-0.48, -0.225], [-0.48, 0.225]]

（4）behavior_server相关配置文件

在params目录下新建一个名为behavior.yaml的文件，并输入如下内容：

/**:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 5.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors/DriveOnHeading"
    wait:
      plugin: "nav2_behaviors/Wait"
    assisted_teleop:
      plugin: "nav2_behaviors/AssistedTeleop"
    global_frame: odom
    robot_base_frame: base_link
    transform_tolerance: 0.1
    use_sim_time: True
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

（5）waypoint_follower相关配置文件

在params目录下新建一个名为waypoint.yaml的文件，并输入如下内容：

/**:
  ros__parameters:
    use_sim_time: True
    loop_rate: 20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 5000

（6）velocity_smoother相关配置文件

在params目录下新建一个名为velocity.yaml的文件，并输入如下内容：

/**:
  ros__parameters:
    use_sim_time: True
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.1, 0.0, 1.0]
    min_velocity: [-0.1, 0.0, -1.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

（7）smoother_server相关配置文件

在params目录下新建一个名为smootherr.yaml的文件，并输入如下内容：

/**:
  ros__parameters:
    costmap_topic: global_costmap/costmap_raw
    footprint_topic: global_costmap/published_footprint
    robot_base_frame: base_link
    transform_timeout: 0.1
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      do_refinement: True

3.launch集成

导航实现时，需要依赖于地图与定位功能，我们可以在一个launch文件中集成之前的定位launch以及当前编写的导航核心模块的launch，以简化导航功能的启动，在launch目录下新建一个名为bringup.launch.py的launch文件，并输入如下内容：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():

    amcl_pkg = get_package_share_directory("mycar_localization")
    nav2_pkg = get_package_share_directory("mycar_navigation2")

    amcl_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(amcl_pkg,'launch',
                                                    'mycar_loca.launch.py'))
        )

    nav2_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(nav2_pkg,'launch', 
                                                    'nav2.launch.py'))
        )

    ld = LaunchDescription()
    ld.add_action(amcl_launch)
    ld.add_action(nav2_launch)
    return ld

4.编辑配置文件

打开CMakeLists.txt 并输入如下内容：

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

5.编译

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

colcon build --packages-select mycar_navigation2

6.执行

（1）请先调用如下指令启动仿真环境：

. install/setup.bash
ros2 launch stage_ros2 my_house.launch.py

（2）然后在终端下进入当前工作空间，输入如下指令启动导航功能：

. install/setup.bash
ros2 launch mycar_navigation2 bringup.launch.py

（3）启动rviz2，加载/opt/ros/humble/share/nav2_bringup/rviz下的nav2_default_view.rviz文件，为机器人设置初始位姿后，再通过菜单栏的Nav2 Goal设置目标点，机器人就可以自动导航至目标点了。

rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz

rviz2：启动 rviz2 工具。
-d：指定要加载的 .rviz 配置文件。
/opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz：.rviz 配置文件的路径。

运行该命令后，rviz2 将启动并加载 nav2_default_view.rviz 配置文件。

如上图，左图是选择初始位置，右图是选择目标位置。

上图中，每个障碍物和墙附近的光圈是危险程度，颜色越红表示机器人经过此地越危险。

自主探索SLAM

导航需要依赖于地图与定位，例如在上一节 导航功能集成 中，导航实现时就是以launch文件的方式集成了 定位AMCL 一节中的定位功能，而机器人SLAM时，也是发布地图数据与定位信息的，所以导航时也可以不借助于amcl而是直接与SLAM结合，达到自主探索的SLAM效果。

1.编写launch文件

在功能包mycar_navigation2的launch目录下新建名为auto_slam.launch.py的launch文件，并输入如下内容：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():

    slam_pkg = get_package_share_directory("mycar_slam_slam_toolbox")
    nav2_pkg = get_package_share_directory("mycar_navigation2")

    slam_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(slam_pkg,'launch',
                                                    'online_sync_launch.py'))
        )

    # slam_launch = IncludeLaunchDescription(
    #     PythonLaunchDescriptionSource(os.path.join(slam_pkg,'launch',
    #                                                 'online_async_launch.py'))
    #     )

    nav2_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(nav2_pkg,'launch', 
                                                    'nav2.launch.py'))
        )

    ld = LaunchDescription()
    ld.add_action(slam_launch)
    ld.add_action(nav2_launch)
    return ld

2.编译

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

colcon build --packages-select mycar_navigation2

3.执行

（1）请先调用如下指令启动仿真环境：

. install/setup.bash
ros2 launch stage_ros2 my_house.launch.py

（2）然后在终端下进入当前工作空间，输入如下指令启动自主SLAM功能：

. install/setup.bash
ros2 launch mycar_navigation2 auto_slam.launch.py

（3）启动rviz2，加载/opt/ros/humble/share/nav2_bringup/rviz下的nav2_default_view.rviz文件，再通过菜单栏的Nav2 Goal设置目标点，机器人就可以自动导航至目标点，并且导航中还会实现建图的功能。

rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz

多车编队

多车编队技术通过先进的传感器、实时数据分析和高度智能化的控制系统，使得一组车辆能够在没有人工干预的情况下实现协同运行，作为一种先进的智能交通系统应用，已经在多个领域展现出其巨大的潜力和广泛的应用场景。比如：

时间优化：多车编队技术消除了人为操作中的误差和延误，降低了人力资源成本和运营风险。

空间优化：多车编队技术可以减少车辆间的空隙，最大化道路利用率。

性能优化：密集编队行驶减少了空气动力学阻力，降低了能耗和燃料消耗。

总之，多车编队技术在机器人领域有着独特的作用和价值。本节将介绍如何在ROS2中实现多车编队。

ROS2 Serial
ROS1串口库1.1

这是ROS1的库，但是也可以从第三方源码编译这个库，用于ROS2.

http://wjwwood.io/serial/doc/1.1.0/index.html

ROS2串口库1.2

顾名思义，是一个串口包，

该库适用于ROS Humble和ROS Jazzy版本。

官方详解:

https://docs.ros.org/en/humble/p/serial_driver/generated/index.html#

https://index.ros.org/p/serial_driver/

下面是根据官方文档手搓的教程（截至教程出之前，还没有任何一个像样的教程）

SerialDriver库特点

该库主要基于Boost.Asio库，所以说原理就是Boost.Asio库的特点：

事件驱动 Boost.Asio 使用操作系统提供的底层 I/O 复用机制（如 Linux 的 epoll、Windows 的 I/O Completion Ports）来监控文件描述符（如串口、套接字等）是否有可用的数据。当数据到达时，触发相应的事件。

回调机制 用户调用异步方法（如 async_read 或 async_receive）时，传入一个回调函数。当指定的事件（如数据接收完成）发生后，Boost.Asio 会自动调用用户提供的回调函数。

I/O 服务对象 Boost.Asio 的核心是一个 io_service 或 io_context 对象，它管理所有的异步操作和回调的调度。异步接收时：

用户将操作（如 async_receive）注册到 io_context。
io_context 将异步操作挂起，并等待 I/O 事件的触发。
当 I/O 事件触发时，io_context 调用对应的回调函数。

不阻塞主线程 异步操作不会阻塞调用线程。主线程可以继续执行其他任务，而异步操作在后台完成。

基于Boost.Asio库的优点

Boost.Asio库是靠系统IO进行事件触发的，并且单独开了一个线程去干这件事，所以这就使boost库拥有单片机的串口中断的特点，又拥有真多线程的特点。

而单片机的串口中断，虽然也能通过IO进行事件触发，但是由于单片机只有一个真线程，所以在进入中断时，一定会阻塞主线程，造成效率损失。

如果在主线程里直接接收并加上死循环和延时，这种效率往往是最低的，并且有很大的安全风险，非常不建议使用。

效率：Boost.Asio接收 » 单片机串口中断 » 多线程阻塞式接收 » 单线程阻塞式接收（在传输速度非常快的情况下，全都是远远大于»）

安装ROS2串口包

serial_driver 是一个封装了串口通信功能的库，它分为两个部分：

ROS2 依赖版本 ：用于 ROS2 环境中，依赖于 ROS2 的通信机制（如 rclcpp）。
独立库版本 ：不依赖 ROS2，可以直接在普通的 C++ 项目中使用。(未经测试)

方式一：在ROS2中安装

sudo apt install ros-<ros2-distro>-asio-cmake-module
sudo apt install ros-<ros2-distro>-serial-driver

#humble
sudo apt install ros-humble-asio-cmake-module
sudo apt install ros-humble-serial-driver
#jazzy
sudo apt install ros-jazzy-asio-cmake-module
sudo apt install ros-jazzy-serial-driver

方式二：通用（该方法未经测试，如果你测试了，请帮忙删掉本句话）

如果你在ROS1或者说其他地方使用，而且没有安装ROS2，请选择该方法。

克隆仓库

首先，克隆 transport_drivers 仓库（包含 serial_driver）：

https://github.com/ros-drivers/transport_drivers

git clone https://github.com/ros-drivers/transport_drivers.git

安装依赖

serial_driver 依赖于 ASIO 库（一个跨平台的 C++ 网络编程库）。确保系统中已安装 ASIO：

sudo apt install libasio-dev

编译独立库

进入 serial_driver 目录并编译：

cd transport_drivers/serial_driver
mkdir build
cd build
cmake ..
make
sudo make install

安装完毕后，在CMake里正常添加该包就可以。

其他准备工作

串口调试助手安装(Linux版) (如果需要Windows版，问控制组要去)

sudo apt-get install cutecom

权限

# 将用户权限提高
sudo usermod -aG dialout $USER
newgrp dialout

有关Linux的USB设备的更多操作请看Vinci机器人队Linux入门教程

初始化详解
创建功能包

ros2 pkg create cpp01_serial --build-type ament_cmake --dependencies rclcpp serial_driver --node-name demo01_uart_send

引用头文件：#include "serial_driver/serial_driver.hpp"
首先要初始化串口：

串口初始化函数init_port()有俩参数，

第一个参数device_name 是端口名称

第二个参数config 是串口参数设置

device_name是个字符串，是填/dev下的设备名

ls /dev | grep USB

有关Linux的USB设备的更多操作请看Vinci机器人队Linux入门教程

config是个类对象，所以要创建一下这个类对象如下：

这玩意是个轻量化的类，没必要放在堆区，直接在栈区里创建即可。

SerialPortConfig(uint32_t baud_rate, drivers::serial_driver::FlowControl flow_control , drivers::serial_driver::Parity parity, drivers::serial_driver::StopBits stop_bits)

从这个重载函数可以看到(参数不懂的问控制组)

第一个参数baud_rate 波特率

第二个参数flow_control 流控制

第三个参数parity 奇偶效验

第四个参数stop_bits 停止位

具体的参数在serial_port.hpp中有

这样串口就初始化成功了。

发送代码详解
发送主逻辑

我们接下来写发送主逻辑，首先要创建一个延时，不要使用while(true)这种死循环延迟，ROS2提供了定时器。

先创建定时器（创建定时器不用详解了，不懂的回顾上面赵老师的视频）

就需要注意一个点，我们用的是 毫秒milliseconds ，不是 微秒microseconds 。

发送核心函数是这个send函数，可以看到，入口参数是vector类型的向量，

#include "rclcpp/rclcpp.hpp"
#include "serial_driver/serial_driver.hpp"

class Serial_Node : public rclcpp::Node
{
public:
  Serial_Node() : Node("serial_node_cpp")
  {
    // 等设备准备好再初始化
    // std::this_thread::sleep_for(std::chrono::milliseconds(500));

    // 串口设备名（根据实际设备调整）
    const std::string device_name = "/dev/ttyUSB0";

    RCLCPP_INFO(this->get_logger(), "Serial port Node Open!");
    // 创建串口配置对象
    // 波特率115200；不开启流控制；无奇偶效验；停止位1。
    drivers::serial_driver::SerialPortConfig config(
        9600,
        drivers::serial_driver::FlowControl::NONE,
        drivers::serial_driver::Parity::NONE,
        drivers::serial_driver::StopBits::ONE);
    
    // 初始化串口
    try
    {
      io_context_ = std::make_shared<drivers::common::IoContext>(1);
      // 初始化 serial_driver_
      serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
      serial_driver_->init_port(device_name, config);
      serial_driver_->port()->open();
      
      RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
      RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
      RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
    }
    catch (const std::exception &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
      return;
    }

    // 设置发送定时器
    send_timer_ = this->create_wall_timer(
        std::chrono::milliseconds(500),
        std::bind(&Serial_Node::send_message_timer_callback, this));
  }

private:
  void send_message_timer_callback()
  {
    // 发送一串字符串
    const std::string message = "Hello from ROS 2!\n";
    std::vector<uint8_t> data(message.begin(), message.end());
    // std::vector<uint8_t> hex_data = {0x48, 0x65, 0x6C, 0x6C, 0x6F}; // "Hello" in ASCII
    auto port = serial_driver_->port();

    try
    {
      size_t bytes_sent_size = port->send(data);
      RCLCPP_INFO(this->get_logger(), "Sent: %s (%ld bytes)", message.c_str(), bytes_sent_size);
    }
    catch(const std::exception &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
    }
  }

  std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
  std::shared_ptr<drivers::common::IoContext> io_context_;
  rclcpp::TimerBase::SharedPtr send_timer_;
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  auto node = std::make_shared<Serial_Node>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

接收代码详解
定时器方式(低效，不建议用)

#include "rclcpp/rclcpp.hpp"
#include "serial_driver/serial_driver.hpp"
#include <string>

class Serial_Node : public rclcpp::Node
{
public:
  Serial_Node() : Node("serial_node_cpp")
  {
    // 等设备准备好再初始化
    // std::this_thread::sleep_for(std::chrono::milliseconds(500));

    // 串口设备名（根据实际设备调整）
    const std::string device_name = "/dev/ttyUSB0";

    RCLCPP_INFO(this->get_logger(), "Serial port Node Open!");
    // 创建串口配置对象
    // 波特率115200；不开启流控制；无奇偶效验；停止位1。
    drivers::serial_driver::SerialPortConfig config(
        9600,
        drivers::serial_driver::FlowControl::NONE,
        drivers::serial_driver::Parity::NONE,
        drivers::serial_driver::StopBits::ONE);
    
    // 初始化串口
    try
    {
      io_context_ = std::make_shared<drivers::common::IoContext>(1);
      // 初始化 serial_driver_
      serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
      serial_driver_->init_port(device_name, config);
      serial_driver_->port()->open();
      
      RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
      RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
      RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
    }
    catch (const std::exception &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
      return;
    }

    // 设置发送定时器
    receive_timer_ = this->create_wall_timer(
        std::chrono::milliseconds(3),
        std::bind(&Serial_Node::receive_message_timer_callback, this));
  }

private:
  void receive_message_timer_callback()
  {
    std::vector<uint8_t> data(100); // "Hello" in ASCII
    auto port = serial_driver_->port();

    try
    {
      size_t bytes_receive_size = port->receive(data);
      if(bytes_receive_size > 0)
      {
        std::string message(data.begin(),data.begin() + bytes_receive_size);
        RCLCPP_INFO(this->get_logger(), "Receive: %s (%ld bytes)", message.c_str(), bytes_receive_size);
      }
    }
    catch(const std::exception &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "Error Receiving from serial port:%s",ex.what());
    }
  }

  std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
  std::shared_ptr<drivers::common::IoContext> io_context_;
  rclcpp::TimerBase::SharedPtr receive_timer_;
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  auto node = std::make_shared<Serial_Node>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

异步方式(高效)

#include "rclcpp/rclcpp.hpp"
#include "serial_driver/serial_driver.hpp"
#include <string>

class Serial_Node : public rclcpp::Node
{
public:
  Serial_Node() : Node("serial_node_cpp")
  {
    // 等设备准备好再初始化
    // std::this_thread::sleep_for(std::chrono::milliseconds(500));

    // 串口设备名（根据实际设备调整）
    const std::string device_name = "/dev/ttyUSB0";

    RCLCPP_INFO(this->get_logger(), "Serial port Node Open!");
    // 创建串口配置对象
    // 波特率115200；不开启流控制；无奇偶效验；停止位1。
    drivers::serial_driver::SerialPortConfig config(
        9600,
        drivers::serial_driver::FlowControl::NONE,
        drivers::serial_driver::Parity::NONE,
        drivers::serial_driver::StopBits::ONE);
    
    // 初始化串口
    try
    {
      io_context_ = std::make_shared<drivers::common::IoContext>(1);
      // 初始化 serial_driver_
      serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
      serial_driver_->init_port(device_name, config);
      serial_driver_->port()->open();
      
      RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
      RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
      RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
    }
    catch (const std::exception &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
      return;
    }

    async_receive_message();
  }

private:
  void async_receive_message()  //创建一个函数更方便重新调用
  {
    auto port = serial_driver_->port();

    // 设置接收回调函数
    port->async_receive([this](const std::vector<uint8_t> &data,const size_t &size) 
    {
        if (size > 0)
        {
            // 处理接收到的数据
            std::string received_message(data.begin(), data.begin() + size);
            RCLCPP_INFO(this->get_logger(), "Received: %s (%ld bytes)", received_message.c_str(),size);
        }
        // 继续监听新的数据
        async_receive_message();
    }
    );
  }

  std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
  std::shared_ptr<drivers::common::IoContext> io_context_;
  std::vector<uint8_t> receive_data_buffer = std::vector<uint8_t>(1024); // 接收缓冲区
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  auto node = std::make_shared<Serial_Node>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

总代码(又发又收)

#include "rclcpp/rclcpp.hpp"
#include "serial_driver/serial_driver.hpp"
#include <string>

class Serial_Node : public rclcpp::Node
{
public:
  Serial_Node() : Node("serial_node_cpp")
  {
    // 等设备准备好再初始化
    // std::this_thread::sleep_for(std::chrono::milliseconds(500));

    // 串口设备名（根据实际设备调整）
    const std::string device_name = "/dev/ttyUSB0";

    RCLCPP_INFO(this->get_logger(), "Serial port Node Open!");
    // 创建串口配置对象
    // 波特率115200；不开启流控制；无奇偶效验；停止位1。
    drivers::serial_driver::SerialPortConfig config(
        9600,
        drivers::serial_driver::FlowControl::NONE,
        drivers::serial_driver::Parity::NONE,
        drivers::serial_driver::StopBits::ONE);
    
    // 初始化串口
    try
    {
      io_context_ = std::make_shared<drivers::common::IoContext>(1);
      // 初始化 serial_driver_
      serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
      serial_driver_->init_port(device_name, config);
      serial_driver_->port()->open();
      
      RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
      RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
      RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
    }
    catch (const std::exception &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
      return;
    }

        // 设置发送定时器
    transmit_timer_ = this->create_wall_timer(
        std::chrono::milliseconds(5000),
        std::bind(&Serial_Node::send_message_timer_callback, this));

    async_receive_message();   //进入异步接收
  }

private:

  void send_message_timer_callback()
  {
    // 发送一串字符串
    const std::string transmitted_message = "Hello from ROS 2!\n";
    transmit_data_buffer = std::vector<uint8_t>(transmitted_message.begin(), transmitted_message.end());
    // std::vector<uint8_t> hex_data = {0x48, 0x65, 0x6C, 0x6C, 0x6F}; // "Hello" in ASCII
    auto port = serial_driver_->port();

    try
    {
      size_t bytes_transmit_size = port->send(transmit_data_buffer);
      RCLCPP_INFO(this->get_logger(), "Transmitted: %s (%ld bytes)", transmitted_message.c_str(), bytes_transmit_size);
    }
    catch(const std::exception &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
    }
  }

  void async_receive_message()  //创建一个函数更方便重新调用
  {
    auto port = serial_driver_->port();

    // 设置接收回调函数
    port->async_receive([this](const std::vector<uint8_t> &data,const size_t &size) 
    {
        if (size > 0)
        {
            // 处理接收到的数据
            std::string received_message(data.begin(), data.begin() + size);
            RCLCPP_INFO(this->get_logger(), "Received: %s (%ld bytes)", received_message.c_str(),size);
        }
        // 继续监听新的数据
        async_receive_message();
    }
    );
  }

  std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
  std::shared_ptr<drivers::common::IoContext> io_context_;
  rclcpp::TimerBase::SharedPtr transmit_timer_;
  std::vector<uint8_t> transmit_data_buffer = std::vector<uint8_t>(1024); // 发送缓冲区
  std::vector<uint8_t> receive_data_buffer = std::vector<uint8_t>(1024);  // 接收缓冲区
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  auto node = std::make_shared<Serial_Node>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

注意事项
禁止引用boost/asio.hpp：由于该库的原理就是boost库，而且是ROS2自带的boost库，所以你用通用版的boost/asio.hpp，会导致报错出现很多标志重复的问题。
机器人硬件
机器人组成

立足角度不同，对机器人组成的认识也会有所不同。宏观上讲，机器人由硬件以及软件两大部分组成。进一步细化，机器人又可以分为三大部分： 控制系统 、 传感系统 与 硬件平台 。

1.控制系统

控制系统相当于机器人的大脑，由安装了机器人操作系统的处理器组成，是机器人的核心。控制系统的主要任务是根据作业指令和传感器反馈生成控制命令，同时负责算法处理和人机交互等功能。它在机器人中扮演着智能决策和执行任务的关键角色。

2.传感系统

传感系统相当于机器人的感官，分为内部传感器模块和外部传感器模块。

内部传感系统包括电机的编码器、陀螺仪等，可以通过自身信号反馈来检测机器人的位姿状态。
外部传感系统包括摄像头、雷达等，用于感知外部环境。

传感系统获取有用的信息，帮助机器人感知和理解周围的环境，起到辅助机器人操作和决策的作用。

3.硬件平台

硬件平台相当于机器人的躯体，提供了对机器人的物理支持，通常由驱动系统和执行机构两部分组成。

驱动系统主要负责驱动执行机构，将控制系统下达的命令转换为执行机构所需的信号，类似于人的小脑和神经。
执行机构是机器人的机械部分，类似于人的手和脚，例如机器人的行走部分和机械臂。

目前，机器人的硬件平台已经非常成熟，种类也非常丰富。包括但不限于飞行器、轮式机器人、四足机器人、人形机器人、软体机器人和水下机器人等。此外，我们甚至可以根据丰富的参考资料制作简易的机器人。在学习和工作中，大家可以根据实际需求选择适合的机器人平台。

工控机之远程开发环境

场景

机器人平台一般自带预装了ROS的控制系统。这套控制系统与我们前一阶段的学习环境基本无异，具体到开发或应用层面也大致相同，我们可以借助于平台外设的鼠标、键盘、显示器等直接在其上开发、调试程序或控制该机器人。但是在”面向平台“开发时可能遇到一些问题，比如：

机器人是一辆移动式平台时，”面向平台“的开发模式下可能需要人随车走，这显然效率低下，并存在一定的安全隐患；

”面向平台“的开发模式，还会受限于环境、地形等诸多因素的约束；

机器人与开发人员之间可能是一对多的关系，也即多位开发或测试工程师使用同一台设备，”面向平台“的开发模式下不免会出现资源抢占的情况；

总而言之，”面向平台“开发有其可行性，但是也存在诸多不便，此背景下，就可以通过搭建远程开发环境来解决上述问题了。

概念

远程开发 是一种在远程主机上进行编写、编译或运行程序的开发方式。相对于本地开发，远程开发需要将本地设备连接到远程主机，以实现数据传输和操作同步。

作用

远程开发可以带来一些优势，比如提供更强大的计算资源、便于团队协作、统一开发环境等。同时，远程开发也需要考虑网络延迟和稳定性等因素。总的来说，远程开发可以提供更灵活和便捷的开发环境，适用于不同场景和需求。

实现方式

在本教程中，我们将主要介绍两种常用的远程开发模式：SSH和NoMachine。这是两种常见的远程连接方式，它们在功能和使用方式上有一些差异。

SSH是一种命令行界面的连接方式，用户需要通过命令行输入指令进行远程操作。对于熟悉命令行的用户来说，SSH可能更加灵活和高效。
NoMachine提供了图形界面的远程连接，用户可以直观地操作远程计算机的桌面。它支持窗口、多显示器和文件传输等功能，适合那些需要图形界面的远程操作。

总的来说，SSH更适合需要执行命令行操作和快速访问的场景，而NoMachine更适合需要图形界面远程桌面访问的场景。选择哪种方式应该根据具体的需求和使用习惯来决定。

远程开发SSH

详见Vinci机器人队Linux入门教程

远程访问桌面

详见Vinci机器人队Linux入门教程

工控机之外接USB设备

详见Vinci机器人队Linux入门教程

分布式搭建

在 ROS2其他通信机制 章节里

优化日志

在实际应用上,我们很少使用RCLCPP_INFO,原因是打印数据开销太大,像Nvidia Jetson Xavier等工控机设备可能吃不消.

像下方这个图就显示了在树莓派4b下打印的开销:

由于我们想避免那么大的开销,又想在必要时查看打印的数据进行调试,所以我们常采用RCLCPP_DEBUG来打印数据,在必要时期才打印数据.

RCLCPP_DEBUG 的核心作用

调试专用 ：仅在调试阶段输出信息，生产环境自动静默
性能优化 ：通过编译选项完全移除调试代码，零运行时开销
分级控制 ：与其他日志级别（INFO/WARN/ERROR）解耦

打印出来rclcpp_debug的方式:

方式一:

ros2 run <package_name> <node_name> --ros-args --log-level debug

方式二:

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='my_package',
            executable='my_node',
            name='my_node',
            output='screen',
            parameters=[],
            arguments=['--ros-args', '--log-level', 'debug']
        )
    ])

/opt/ros/humble/include/rcutils/rcutils/logging.h定义：

硬件平台

硬件平台的选择是具有多样性的，比如：轮式底盘、两足或四足机器人、无人机等等。现在硬件平台相关技术发展的已经比较成熟了，可以自己制作，也可以直接采购，假设选择后者的话，那么购买后，关于具体使用，可以参考配套的使用手册。一般情况下，供应商会提供对应的驱动包，直接下载并编译执行即可驱动硬件平台，当然如果是集成了上位机的整套机器人方案，那么一般已经预置了驱动程序，届时直接运行即可。

机器人下位机常用的开发板有STM32，ESP32，GD32，Arduino，51MCU等等。（不懂的问控制组和电路组）

与机器人下位机开发板通信常用的有串口通信，MicroROS等方式，可以看上方的教程。

我们可以让下位机传给上位机的数据有里程计ODOM数据，惯性计IMU数据，全球定位GNSS数据等等，当然也可以让这些设备直接与工控机通信，让工控机处理好机器人的定位信息再一起传给下位机。

下面需要介绍这些数据如何进行传输。

自己写的大部分源码仅供参考，如果有更好的思路可以说：

我用Jazzy版本多一些，一切以Jazzy版本为最终参考，Humble其实与Jazzy最大区别就是Gazebo,关于Gazebo的代码可以看Humble仓库的。

https://github.com/CyberNaviRobot/CyberRobot_ROS2_Humble_WS

https://github.com/CyberNaviRobot/CyberRobot_ROS2_Jazzy_WS

里程计Odom
功能包创建

不一定非得要码盘(里程计Odom)的数据，只要把机器人6个自由度DOF，可以由电机编码器计算而来（x,y,z,yaw,pitch,roll不懂的看上方Moveit2里的教学）全部发过来就行。

首先先创建功能包

ros2 pkg create odom_plumb --build-type ament_cmake --dependencies rclcpp nav_msgs geometry_msgs tf2_ros --node-name odom_node

先编译一下

colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON

消息接口

底盘驱动启动之后，会持续广播一个比较重要的数据——里程计。里程计用于描述当前机器人相对于出发点的位姿以及速度等信息，里程计在机器人定位和导航的局部路径规划中有着重要的作用。在ROS2中里程计消息被封装为了nav_msgs/msg/Odometry接口，可通过指令ros2 interface show nav_msgs/msg/Odometry查看该接口的具体数据结构，结果如下：

它描述的是机器人位姿与速度的预估信息。其中位姿信息是以header中的frame_id为参考系的，而速度信息则是以child_frame_id为参考系的。

在 ROS2 中， odom 坐标系 （通常是 odom frame）遵循 右手坐标系 ，其定义如下：

X 轴 ：向前（前进方向）为正
Y 轴 ：向左（左侧方向）为正
Z 轴 ：向上（垂直地面）为正

在 ROS2 的 odom 坐标系 （右手坐标系）中， yaw 角的定义 如下：

yaw = 0 ：机器人面向 X 轴正方向 （向前）。
逆时针（CCW，Counter Clockwise）旋转为正 。
顺时针（CW，Clockwise）旋转为负 。

yaw 角范围

yaw ∈ (-π, π] （即 -180° 到 180°）
当 yaw = π（180°），如果继续增加，应该跳转到 -π（-180°），而不是 π。
当 yaw = -π（-180°），如果继续减少，应该跳转到 π（180°）。

std_msgs/Header header # 标头
    builtin_interfaces/Time stamp # 时间戳
        int32 sec
        uint32 nanosec
    string frame_id # 位姿信息所参考的坐标系
位姿信息所指向的坐标系，也是速度信息所参考的坐标系
string child_frame_id
相对于frame_id的预估位姿信息
geometry_msgs/PoseWithCovariance pose
    Pose pose
        Point position # 坐标
            float64 x
            float64 y
            float64 z
        Quaternion orientation # 四元数
            float64 x 0
            float64 y 0
            float64 z 0
            float64 w 1
    float64[36] covariance
相对于child_frame_id的预估线速度与角速度
geometry_msgs/TwistWithCovariance twist
    Twist twist
        Vector3  linear # 线速度
            float64 x
            float64 y
            float64 z
        Vector3  angular # 角速度
            float64 x
            float64 y
            float64 z
    float64[36] covariance

需要注意的是，

坐标和位姿是相对于frame_id的，一般是odom自己。
而线速度和角速度是相对于child_frame_id的。一般是base_link 。（这里注意，学长我最初就弄错了）
虽然Twist是相对于child_frame_id （ base_link）的，但是他表达的是瞬时值，就和汽车一样，汽车上面的速度表也是相对于车身的瞬时值。
里程计在计算时是存在累计误差的，它所描述的是机器人的预估位姿，并不绝对准确。
节点主要逻辑

所以我们需要初始化nav_msgs::msg::Odometry并发布出去。

所以你需要从MCU单片机上去获取这些数据，或者计算这些数据，并以nav_msgs::msg::Odometry类型的消息发送出去。

注意，如果你没有正儿八经的里程计，那么里程计的数据可以由底盘电机编码器数据计算而成。

除了发布话题的代码，别忘记发布odom与base_link之间的动态坐标变换。

下方是运动学正解算出来的odom数据:

#include "odom_mg513.h"
#include "mg513_gmr500ppr.h"
//#include <cmath>
#include "arm_math.h"

ODOM_Motor odom_motor_;

// 坐标系满足右手坐标系

//（单位：米）
//轮距
extern fp32 Wheel_Spacing;
//轴距
extern fp32 Alex_Spacing;
//轮子半径
extern fp32 Wheel_Radius;

void ODOM_Motor::Analysis(fp32 dt)
{
  this->dt = dt;
  for(int16_t i = 0;i < 4;i++)
    {
        // 将 RPM 转换为 m/s
      encoder_wheel_velocities_[i] = mg513_gmr500ppr_motor[i].encoder.motor_data.motor_speed  * 2.0f * 3.14159265358979f * Wheel_Radius / 60.0f;
    }
    // 计算机器人前进的线速度和角速度，公式不需要轮半径
    //线速度，四个轮子在机器人的运动学模型中贡献相同，所以要除以4
    this->vx = ( encoder_wheel_velocities_[0] + encoder_wheel_velocities_[1] - encoder_wheel_velocities_[2] - encoder_wheel_velocities_[3]) / 4.0f;
    this->vy = ( encoder_wheel_velocities_[0] - encoder_wheel_velocities_[1] - encoder_wheel_velocities_[2] + encoder_wheel_velocities_[3]) / 4.0f;

    //线速度，轮距（wheel_spacing） 决定了左右轮对旋转的贡献程度，轴距（alex_spacing） 决定了前后轮对旋转的贡献程度，
    //所以要除以底盘尺寸，alex_spacing + wheel_spacing 是底盘尺寸。
    this->vw = (-encoder_wheel_velocities_[0] - encoder_wheel_velocities_[1] - encoder_wheel_velocities_[2] - encoder_wheel_velocities_[3]) / (4.0f * (Wheel_Spacing + Alex_Spacing));

    // 更新机器人的位置（假设机器人沿着y轴移动）
//     this->x_position += this->vx * std::__math::cos(this->yaw) * this->dt;  
//     this->y_position += this->vy * std::__math::sin(this->yaw) * this->dt;
    this->x_position += this->vx * arm_cos_f32(this->yaw) * this->dt;  
    this->y_position += this->vy * arm_sin_f32(this->yaw) * this->dt;
    this->yaw += this->vw * this->dt;

    // 保证 yaw 始终在 -PI 到 PI 之间
    if(this->yaw > 3.14159265358979f) 
    {
      this->yaw -= 2.0 * 3.14159265358979f;
    }
    if(this->yaw < -3.14159265358979f) 
    {
      this->yaw += 2.0 * 3.14159265358979f;
    }
    
//    this->yaw_deg = this->yaw *  180.0f / 3.14159265358979f;
   
    
}

ROS2总代码如下：

#include "rclcpp/rclcpp.hpp"
#include "serial_driver/serial_driver.hpp"
#include <chrono>
#include <functional>
#include <rclcpp/logging.hpp>
#include <string>
#include "sensor04_odom_kc/serial_pack.h"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>

extern std::vector<uint8_t> tx_data_buffer;

class Odom_KC_Node : public rclcpp::Node
{
public:
  Odom_KC_Node() : Node("Odom_KC_Node")
  {
    // 声明参数（带默认值）
    // 串口设备默认名（根据实际设备调整）
    this->declare_parameter("device_name", "/dev/ttyACM1");
    //串口默认波特率
    this->declare_parameter("baud_rate", 115200);

    // 获取参数值
    const std::string device_name = this->get_parameter("device_name").as_string();
    const uint32_t baud_rate = this->get_parameter("baud_rate").as_int();

    RCLCPP_INFO(this->get_logger(), "Serial port Node Open!");
    // 创建串口配置对象
    // 波特率默认115200；不开启流控制；无奇偶效验；停止位1。
    drivers::serial_driver::SerialPortConfig config(
        baud_rate,
        drivers::serial_driver::FlowControl::NONE,
        drivers::serial_driver::Parity::NONE,
        drivers::serial_driver::StopBits::ONE);
    
    // 初始化串口
    try
    {
      io_context_ = std::make_shared<drivers::common::IoContext>(1);
      // 初始化 serial_driver_
      serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
      serial_driver_->init_port(device_name, config);
      serial_driver_->port()->open();
      
      RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
      RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
      RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
    }
    catch (const std::exception &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
      return;
    }

    //串口发布方
    odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom",10);

    //初始化tf广播器
    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    //初始化cmd_vel消息订阅
    cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>("cmd_vel",10,std::bind(&Odom_KC_Node::cmd_vel_sub_callback,this,std::placeholders::_1));

    async_receive_message();   //进入异步接收
  }

private:
  std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
  std::shared_ptr<drivers::common::IoContext> io_context_;
  
  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;

  // // 四个轮子的速度(对应单片机的顺序)（单位：rpm）
  // fp32 received_encoder_wheel_velocities_[4] = {0.0, 0.0, 0.0, 0.0};
  // // 四个轮子的速度(对应单片机的顺序)（单位：m/s）
  // fp32 encoder_wheel_velocities_[4] = {0.0, 0.0, 0.0, 0.0};  

  // // 四个轮子的速度(对应单片机的顺序)（单位：2000pc）
  // fp32 received_encoder_wheel_angle_[4] = {0.0, 0.0, 0.0, 0.0};

  //麦克纳姆轮底盘参数
  const fp32 wheel_spacing = 0.093; // 轮间距（单位：米）
  const fp32 alex_spacing = 0.085; // 轮距（单位：米）
  const fp32 wheel_radius_ = 0.0375; // 轮子半径（单位：米）
  

  fp32 vy=0.0,vx=0.0,vw=0.0;
  fp32 yaw_ = 0.0; // 机器人航向角（单位：弧度）
  fp32 dt_;

  fp32 x_position_ = 0.0; // x位置
  fp32 y_position_ = 0.0; // y位置
  

  void async_receive_message()  //创建一个函数更方便重新调用
  {
    auto port = serial_driver_->port();

    // 设置接收回调函数
    port->async_receive([this](const std::vector<uint8_t> &data,const size_t &size) 
    {
      //创建odom消息类型
      auto odom_msg = nav_msgs::msg::Odometry();

        if (size > 0)
        {
          for(int16_t i = 0;i < (int16_t)size;++i)
          {
            uint8_t data_buffer = data[i];
            // 处理接收到的数据
            serial_pack_.rx.Data_Analysis(
              &data_buffer,
              0x01,
            0,
            0,
            0,
            0,
            8);
          }
          //由于在ROS2中，node是局部变量，所以发布方只能在node类里，故Data_Apply不写任何东西，直接在接收下面的回调函数里实现功能。
          if(serial_pack_.rx.data.cmd == 0x01)
          {
            RCLCPP_DEBUG(this->get_logger(), "\n");
            RCLCPP_DEBUG(this->get_logger(), "以下是电机编码器的odom数据：");

            // // 存储电机速度数据
            // received_encoder_wheel_velocities_[0] = serial_pack_.rx.data.fp32_buffer[0];  // 电机 0 速度
            // received_encoder_wheel_velocities_[1] = serial_pack_.rx.data.fp32_buffer[1];  // 电机 1 速度
            // received_encoder_wheel_velocities_[2] = serial_pack_.rx.data.fp32_buffer[2];  // 电机 2 速度
            // received_encoder_wheel_velocities_[3] = serial_pack_.rx.data.fp32_buffer[3];  // 电机 3 速度

            // // 存储电机位置数据
            // received_encoder_wheel_angle_[0] = serial_pack_.rx.data.fp32_buffer[4];  // 电机 0 位置
            // received_encoder_wheel_angle_[1] = serial_pack_.rx.data.fp32_buffer[5];  // 电机 1 位置
            // received_encoder_wheel_angle_[2] = serial_pack_.rx.data.fp32_buffer[6];  // 电机 2 位置
            // received_encoder_wheel_angle_[3] = serial_pack_.rx.data.fp32_buffer[7];  // 电机 3 位置

            // vx = serial_pack_.rx.data.fp32_buffer[8];
            // vy = serial_pack_.rx.data.fp32_buffer[9];
            // vw = serial_pack_.rx.data.fp32_buffer[10];
            // yaw_ = serial_pack_.rx.data.fp32_buffer[11];
            // dt_ = serial_pack_.rx.data.fp32_buffer[12];
            // x_position_ = serial_pack_.rx.data.fp32_buffer[13];
            // y_position_ = serial_pack_.rx.data.fp32_buffer[14];

            vx = serial_pack_.rx.data.fp32_buffer[0];
            vy = serial_pack_.rx.data.fp32_buffer[1];
            vw = serial_pack_.rx.data.fp32_buffer[2];
            yaw_ = serial_pack_.rx.data.fp32_buffer[3];
            dt_ = serial_pack_.rx.data.fp32_buffer[4];
            x_position_ = serial_pack_.rx.data.fp32_buffer[5];
            y_position_ = serial_pack_.rx.data.fp32_buffer[6];

            // 打印电机速度和位置（角度）
            for (int i = 0; i < 4; ++i) 
            {
                // RCLCPP_DEBUG(this->get_logger(), "%d号电机的速度: %.6f RPM, 位置: %.6f (2000pc)",
                //             i, received_encoder_wheel_velocities_[i], received_encoder_wheel_angle_[i]);

                RCLCPP_DEBUG(this->get_logger(),"线速度:x:%.6f,y:%.6f,z:%.6f",vx,vy,0.0f);
                RCLCPP_DEBUG(this->get_logger(),"角速度:x:%.6f,y:%.6f,z:%.6f",0.0f,0.0f,vw);
                RCLCPP_DEBUG(this->get_logger(),"欧拉角(逆正顺负):r:%.6f,p:%.6f,y:%.6f",0.0f,0.0f,yaw_);
                RCLCPP_DEBUG(this->get_logger(),"积分间隔:%.6f",dt_);
                RCLCPP_DEBUG(this->get_logger(),"右手坐标系X坐标(前正后负):%.6f",x_position_);
                RCLCPP_DEBUG(this->get_logger(),"右手坐标系Y坐标(左正右负):%.6f",y_position_);
            }

            //时间戳
            odom_msg.header.stamp = this->get_clock()->now();

            //位姿信息所参考的坐标系
            odom_msg.header.frame_id = "odom";

            // 设置child_frame_id（底盘坐标系）
            odom_msg.child_frame_id = "base_link";  // 设置子坐标系为机器人底盘坐标系

            //DOF平动位置
            odom_msg.pose.pose.position.x = x_position_;
            odom_msg.pose.pose.position.y = y_position_;
            odom_msg.pose.pose.position.z = 0.0;

            //从欧拉角转换为四元数
            tf2::Quaternion q;
            q.setRPY(0,0,yaw_);
            //DOF转动（四元数）
            odom_msg.pose.pose.orientation.x = q.x();
            odom_msg.pose.pose.orientation.y = q.y();
            odom_msg.pose.pose.orientation.z = q.z();
            odom_msg.pose.pose.orientation.w = q.w();

            //线速度
            odom_msg.twist.twist.linear.x = vx;
            odom_msg.twist.twist.linear.y = vy;
            odom_msg.twist.twist.linear.z = 0.0;

            //角速度
            odom_msg.twist.twist.angular.x = 0.0;
            odom_msg.twist.twist.angular.y = 0.0;
            odom_msg.twist.twist.angular.z = vw;

            // 发布消息
            odom_pub_->publish(odom_msg);

            // 打印位置（XYZ）
            RCLCPP_DEBUG(
              this->get_logger(),
              "位置Position(XYZ): %.6f, %.6f, %.6f",
              odom_msg.pose.pose.position.x,
              odom_msg.pose.pose.position.y,
              odom_msg.pose.pose.position.z
            );

            // 打印姿态（四元数WXYZ）
            RCLCPP_DEBUG(
              this->get_logger(),
              "姿态Orientation(WXYZ): %.6f, %.6f, %.6f, %.6f",
              odom_msg.pose.pose.orientation.w,  // 注意顺序：WXYZ
              odom_msg.pose.pose.orientation.x,
              odom_msg.pose.pose.orientation.y,
              odom_msg.pose.pose.orientation.z
            );

            // 打印姿态（欧拉角RPY）
            RCLCPP_DEBUG(
              this->get_logger(),
              "欧拉角Euler(RPY): %.6f, %.6f, %.6f",
              0.0,
              0.0,
              yaw_
            );

            // 打印线速度（XYZ）
            RCLCPP_DEBUG(
              this->get_logger(),
              "线速度LinearVel(XYZ): %.6f, %.6f, %.6f",
              odom_msg.twist.twist.linear.x,
              odom_msg.twist.twist.linear.y,
              odom_msg.twist.twist.linear.z
            );

            // 打印角速度（XYZ）
            RCLCPP_DEBUG(
              this->get_logger(),
              "角速度AngularVel(XYZ): %.6f, %.6f, %.6f",
              odom_msg.twist.twist.angular.x,
              odom_msg.twist.twist.angular.y,
              odom_msg.twist.twist.angular.z
            );

            /* 接下来发布tf */
            auto transform = geometry_msgs::msg::TransformStamped();
            transform.header.stamp = odom_msg.header.stamp; // 时间戳与odom同步
            transform.header.frame_id = "odom";
            transform.child_frame_id = "base_link";
            transform.transform.translation.x = x_position_;
            transform.transform.translation.y = y_position_;
            transform.transform.translation.z = 0.0;
            transform.transform.rotation = odom_msg.pose.pose.orientation; // 复用四元数
            tf_broadcaster_->sendTransform(transform);
          }

        }
        // 继续监听新的数据
        async_receive_message();
    }
    );
  }

  void cmd_vel_sub_callback(const geometry_msgs::msg::Twist &cmd_msg)
  {
    // bool bool_buffer[] = {1, 0, 1, 0};
    // int8_t int8_buffer[] = {0x11,0x22};
    // int16_t int16_buffer[] = {2000,6666};
    // int32_t int32_buffer[] = {305419896};
    fp32 fp32_buffer[] = {(fp32)cmd_msg.linear.x,(fp32)cmd_msg.linear.y,(fp32)cmd_msg.linear.z,
                          (fp32)cmd_msg.angular.x,(fp32)cmd_msg.angular.y,(fp32)cmd_msg.angular.z};

    //由于ROS2中node为局部变量，所以只能在node中调用send函数，所以Serial_Transmit只负责处理data_buffer。
    serial_pack_.tx.Data_Pack(0x01, 
                                 nullptr, 0,
                                 nullptr, 0,
                                 nullptr, 0,
                                 nullptr, 0,
                                 fp32_buffer, sizeof(fp32_buffer) / sizeof(fp32));

    auto port = serial_driver_->port();

    try
    {
      size_t bytes_size = port->send(tx_data_buffer);

      RCLCPP_DEBUG(this->get_logger(), "平动XYZ：%.6f,%.6f,%.6f", fp32_buffer[0],fp32_buffer[1],fp32_buffer[2]);
      RCLCPP_DEBUG(this->get_logger(), "转动XYZ：%.6f,%.6f,%.6f", fp32_buffer[3],fp32_buffer[4],fp32_buffer[5]);
      RCLCPP_DEBUG(this->get_logger(), "(%ld bytes)", bytes_size);
    }
    catch(const std::exception &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
    }
  }
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  auto node = std::make_shared<Odom_KC_Node>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

最终odom有效数据应该在250Hz左右,4ms发布一次,对于导航也是完全够用的.

惯性计IMU
简介

概念

IMU是惯性测量单元（Inertial Measurement Unit）的缩写，它是一种集成了多个惯性传感器如加速度计、陀螺仪、磁力计等的导航传感器。IMU可以实时地测量和记录其所在物体的加速度、角速度和方向等信息，并通过信号处理和数学模型来计算出物体的姿态、位置和运动状态等。

IMU（惯性测量单元）主要由以下几种类型组成：

加速度计（Accelerometer）：用于测量物体的加速度，通常以三个轴向（X、Y、Z）提供加速度数据。加速度计可以帮助判断物体的线性运动和动作姿态变化。
陀螺仪（Gyroscope）：用于测量物体的角速度或角度变化率。陀螺仪可以提供关于物体旋转的信息，判断物体的角度、方向和转动动作。
磁力计（Magnetometer）：用于测量物体周围的磁场强度和方向（即测量场强）。磁力计可以帮助判断物体的方向和导航，尤其在地磁定位和航向控制中起重要作用。

这些传感器通常集成在一起形成IMU，并通过数据融合算法将它们的输出结合起来，提供综合的姿态、位置和运动信息。

作用

IMU是一种重要的传感器组件，它在姿态感知、行驶状态监测、非GPS定位中发挥着重要的作用。

特点

惯性测量单元（IMU）作为一种常见的传感器，在机器人领域中也有许多应用，下面是IMU在机器人领域的一些优点和缺点：

（1）优点：

实时性高： IMU能够以很高的采样频率获取姿态和加速度数据，提供实时的运动信息。
精度较高： IMU可以测量物体的加速度和角速度，经过合成和滤波处理后，可提供较高精度的姿态估计和运动跟踪。
不受光照条件限制： IMU不依赖于光照条件，适用于各种环境，如室内、室外和低光条件。
尺寸小巧： IMU通常具有小型、轻量级和紧凑的设计，适合嵌入式系统和对空间要求较高的机器人平台。
低功耗： IMU的功耗相对较低，适合资源受限或需要长时间运行的应用。

（2）缺点：

累积误差： 由于积分计算的误差会随时间累积，IMU的姿态估计随着时间推移可能会出现漂移，需要与其他传感器进行融合或采用校准方法进行补偿。
受外部干扰影响： IMU的测量结果容易受到振动、震动和温度变化等外界干扰的影响，可能导致姿态估计不稳定或不准确。
不适用于绝对定位： 单独使用IMU无法提供物体的绝对位置信息，需要结合其他传感器（如GNSS）来实现绝对定位。
无法感知环境信息： IMU只能提供自身的加速度和角速度数据，无法直接感知环境或检测周围物体。

综上所述，IMU在机器人领域具有实时性高、精度较高、不受光照条件限制、尺寸小巧和低功耗等优点。然而，它也存在累积误差、受外部干扰影响、不适用于绝对定位和无法感知环境信息等缺点。因此，在设计机器人系统时需综合考虑应用需求，并与其他传感器（如相机、激光雷达等）进行融合，以提升机器人的感知、定位和控制能力。

消息接口

在ROS2中imu消息被封装为了sensor_msgs/msg/Imu接口，可通过指令ros2 interface show sensor_msgs/msg/Imu查看该接口的具体数据结构，结果如下：

# 这是一个从惯性测量单元（Inertial Measurement Unit，简称IMU）获取数据的消息。
#
# 加速度应该以米/秒^2为单位（不是以g为单位），旋转速度应该以弧度/秒为单位。
#
# 如果测量的协方差已知，则应该填写相应值。全零的协方差矩阵将被解释为“协方差未知”，必须从其他源获取协方差才能使用协方差数据。
#
# 如果没有其中一个数据元素的估计值（例如，你的IMU不会产生方向估计），请将相关协方差矩阵的第 0 个元素设置为 -1。
# 如果你正在解析此消息，请检查每个协方差矩阵的第一个元素中是否有 -1 的值，并忽略相关的估计值。

std_msgs/Header header # 标头
    builtin_interfaces/Time stamp # 时间戳
        int32 sec
        uint32 nanosec
    string frame_id # imu坐标系

geometry_msgs/Quaternion orientation # 四元数姿态
    float64 x 0
    float64 y 0
    float64 z 0
    float64 w 1
float64[9] orientation_covariance # 姿态协方差矩阵

geometry_msgs/Vector3 angular_velocity # 三轴角速度
    float64 x
    float64 y
    float64 z
float64[9] angular_velocity_covariance # 角速度协方差矩阵

geometry_msgs/Vector3 linear_acceleration # 三轴线性加速度
    float64 x
    float64 y
    float64 z
float64[9] linear_acceleration_covariance # 线性加速度协方差矩阵

功能包创建

首先先创建功能包

ros2 pkg create imu_plumb --build-type ament_cmake --dependencies rclcpp sensor_msgs --node-name imu_node

先编译一下

colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON

节点主要逻辑

#include "rclcpp/rclcpp.hpp"
#include "serial_driver/serial_driver.hpp"
#include "cyber_imu/WitStandardProtocol.h"
#include "sensor_msgs/msg/imu.hpp"
#include <cstdint>
#include <rclcpp/logging.hpp>
#include <sensor_msgs/msg/detail/imu__struct.hpp>
#include <tf2/LinearMath/Quaternion.h>

class ImuNode: public rclcpp::Node
{
  public:
    ImuNode():Node("ImuNode_node_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"惯性计imu节点启动!");
      
      //创建odom话题通信发布方
      imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("/imu",10);

    // 声明参数（带默认值）
    // 串口默认设备名（根据实际设备调整）
    this->declare_parameter("device_name", "/dev/ttyUSB0");
    //串口默认波特率
    this->declare_parameter("baud_rate", 115200);

    // 获取参数值
    const std::string device_name = this->get_parameter("device_name").as_string();
    const uint32_t baud_rate = this->get_parameter("baud_rate").as_int();

      // 创建串口配置对象
      // 波特率默认115200；不开启流控制；无奇偶效验；停止位1。
      drivers::serial_driver::SerialPortConfig config(
          baud_rate,
          drivers::serial_driver::FlowControl::NONE,
          drivers::serial_driver::Parity::NONE,
          drivers::serial_driver::StopBits::ONE);
      
      // 初始化串口
      try
      {
        io_context_ = std::make_shared<drivers::common::IoContext>(1);
        // 初始化 serial_driver_
        serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
        serial_driver_->init_port(device_name, config);
        serial_driver_->port()->open();
        
        RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
        RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
        RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
      }
      catch (const std::exception &ex)
      {
        RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
        return;
      }

      async_receive_message();   //进入异步接收
    }

  private:
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;

    std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
    std::shared_ptr<drivers::common::IoContext> io_context_;

    void async_receive_message()  //创建一个函数更方便重新调用
    {
      auto port = serial_driver_->port();

      // 设置接收回调函数
      port->async_receive([this](const std::vector<uint8_t> &data,const size_t &size) 
      {
          if (size > 0)
          {
            RCLCPP_DEBUG(this->get_logger(),"\n");
            RCLCPP_DEBUG(this->get_logger(),"以下是接收到的IMU惯性计数据:");
            //创建imu消息类型
            auto imu_msg = sensor_msgs::msg::Imu();
            bool valid_frame = false;  // 添加有效帧标志
            bool valid_data = false;  // 添加有效帧标志

            for(int16_t i = 0;i < (int16_t)size;++i)
            {
              uint8_t data_buffer = data[i];
              // RCLCPP_INFO(this->get_logger(),"接收到的字节：%x",data_buffer);
              // 处理接收到的数据
              if(wit_imu_.rx.Data_Analysis(&data_buffer) == true)
              {
                valid_frame = true;  // 标记有有效帧需要处理
              }

              if(valid_frame == true)
              {
                if(wit_imu_.rx.data.cmd == 0x51)
                {
                  // 存储加速度计数据
                  wit_imu_.rx.data.imu.Accel.X = wit_imu_.rx.data.int16_buffer[0];
                  wit_imu_.rx.data.imu.Accel.Y = wit_imu_.rx.data.int16_buffer[1];
                  wit_imu_.rx.data.imu.Accel.Z = wit_imu_.rx.data.int16_buffer[2];
                  wit_imu_.rx.data.imu.Temp    = wit_imu_.rx.data.int16_buffer[3];
                  
                  //加速度单位m/s2，温度单位℃摄氏度
                  wit_imu_.rx.data.imu.Accel.X = wit_imu_.rx.data.imu.Accel.X / 32768.0f * 16.0f * 9.8f;
                  wit_imu_.rx.data.imu.Accel.Y = wit_imu_.rx.data.imu.Accel.Y / 32768.0f * 16.0f * 9.8f;
                  wit_imu_.rx.data.imu.Accel.Z = wit_imu_.rx.data.imu.Accel.Z / 32768.0f * 16.0f * 9.8f;
                  wit_imu_.rx.data.imu.Temp    =  wit_imu_.rx.data.imu.Temp / 100.0f;

                  // 打印加速度计数据
                  RCLCPP_DEBUG(this->get_logger(), "加速度(XYZ): %.6f, %.6f, %.6f",
                              wit_imu_.rx.data.imu.Accel.X, wit_imu_.rx.data.imu.Accel.Y, wit_imu_.rx.data.imu.Accel.Z);
                  //减少时间戳带来的延迟
                  imu_msg.header.stamp = this->get_clock()->now();
                }

                else if(wit_imu_.rx.data.cmd == 0x52)
                {
                  // 存储陀螺仪数据
                  wit_imu_.rx.data.imu.Gyro.X = wit_imu_.rx.data.int16_buffer[0];
                  wit_imu_.rx.data.imu.Gyro.Y = wit_imu_.rx.data.int16_buffer[1];
                  wit_imu_.rx.data.imu.Gyro.Z = wit_imu_.rx.data.int16_buffer[2];
                  
                  //角速度单位rad/s
                  wit_imu_.rx.data.imu.Gyro.X = wit_imu_.rx.data.imu.Gyro.X / 32768.0f * 2000.0f * (M_PI / 180.0f);
                  wit_imu_.rx.data.imu.Gyro.Y = wit_imu_.rx.data.imu.Gyro.Y / 32768.0f * 2000.0f * (M_PI / 180.0f);
                  wit_imu_.rx.data.imu.Gyro.Z = wit_imu_.rx.data.imu.Gyro.Z / 32768.0f * 2000.0f * (M_PI / 180.0f);

                  // 打印陀螺仪数据
                  RCLCPP_DEBUG(this->get_logger(), "角速度(XYZ): %.6f, %.6f, %.6f",
                              wit_imu_.rx.data.imu.Gyro.X, wit_imu_.rx.data.imu.Gyro.Y, wit_imu_.rx.data.imu.Gyro.Z);
                }

                else if(wit_imu_.rx.data.cmd == 0x54)
                {
                  // 存储磁力计数据(单位lsb)
                  wit_imu_.rx.data.imu.Magnet.X = wit_imu_.rx.data.int16_buffer[0];
                  wit_imu_.rx.data.imu.Magnet.Y = wit_imu_.rx.data.int16_buffer[1];
                  wit_imu_.rx.data.imu.Magnet.Z = wit_imu_.rx.data.int16_buffer[2];
                  
                  //磁场单位uT
                  // wit_imu_.rx.data.imu.Magnet.X = wit_imu_.rx.data.imu.Magnet.X / 1.0f;
                  // wit_imu_.rx.data.imu.Magnet.Y = wit_imu_.rx.data.imu.Magnet.Y / 1.0f;
                  // wit_imu_.rx.data.imu.Magnet.Z = wit_imu_.rx.data.imu.Magnet.Z / 1.0f;

                  // 打印磁力计数据
                  RCLCPP_DEBUG(this->get_logger(), "磁场(XYZ): %.6f, %.6f, %.6f",
                              wit_imu_.rx.data.imu.Magnet.X, wit_imu_.rx.data.imu.Magnet.Y, wit_imu_.rx.data.imu.Magnet.Z);
                }

                else if(wit_imu_.rx.data.cmd == 0x53)
                {
                  // 存储欧拉角数据
                  wit_imu_.rx.data.imu.Euler.roll = wit_imu_.rx.data.int16_buffer[0];
                  wit_imu_.rx.data.imu.Euler.pitch = wit_imu_.rx.data.int16_buffer[1];
                  wit_imu_.rx.data.imu.Euler.yaw = wit_imu_.rx.data.int16_buffer[2];
                  
                  //欧拉角单位dec
                  wit_imu_.rx.data.imu.Euler.roll = wit_imu_.rx.data.imu.Euler.roll / 32768.0f * 180.0f;
                  wit_imu_.rx.data.imu.Euler.pitch = wit_imu_.rx.data.imu.Euler.pitch / 32768.0f * 180.0f;
                  wit_imu_.rx.data.imu.Euler.yaw = wit_imu_.rx.data.imu.Euler.yaw / 32768.0f * 180.0f;

                  // 打印欧拉角数据
                  RCLCPP_DEBUG(this->get_logger(), "欧拉角(XYZ_RPY): %.6f, %.6f, %.6f",
                              wit_imu_.rx.data.imu.Euler.roll, wit_imu_.rx.data.imu.Euler.pitch, wit_imu_.rx.data.imu.Euler.yaw);

                  //欧拉角单位rad
                  wit_imu_.rx.data.imu.Euler.roll = wit_imu_.rx.data.imu.Euler.roll * (M_PI / 180.0f);
                  wit_imu_.rx.data.imu.Euler.pitch = wit_imu_.rx.data.imu.Euler.pitch  * (M_PI / 180.0f);
                  wit_imu_.rx.data.imu.Euler.yaw = wit_imu_.rx.data.imu.Euler.yaw  * (M_PI / 180.0f);
                }

                else if(wit_imu_.rx.data.cmd == 0x59)
                {
                  for(int16_t i = 0 ; i < 4 ; i++)
                  {
                    // 存储四元数数据
                    wit_imu_.rx.data.imu.Quat.q[i] = wit_imu_.rx.data.int16_buffer[i];

                    //四元数是归一化的四元数
                    wit_imu_.rx.data.imu.Quat.q[i] = wit_imu_.rx.data.imu.Quat.q[i] / 32768.0f;
                  }

                  // 打印四元数数据
                  //注意，0对应y，1对应x，-2才对应z。
                  RCLCPP_DEBUG(this->get_logger(), "四元数(xyzw): %.6f, %.6f, %.6f, %.6f",
                              wit_imu_.rx.data.imu.Quat.q[1],wit_imu_.rx.data.imu.Quat.q[0],-wit_imu_.rx.data.imu.Quat.q[2],wit_imu_.rx.data.imu.Quat.q[3]);
                  valid_data = true;
                }
                wit_imu_.rx.data.cmd = 0x00;   //跑过一次就进行清0
                valid_frame = false;
              }
            }

            if(valid_data == true)
            {
              //时间戳
              // imu_msg.header.stamp = this->get_clock()->now();
              //位姿信息所参考的坐标系
              imu_msg.header.frame_id = "imu";

              // tf2::Quaternion q;
              // //DOF欧拉角单位rad
              // q.setRPY(roll_, pitch_, yaw_);
              // //DOF欧拉角（四元数）
              // imu_msg.orientation.x = q.x();
              // imu_msg.orientation.y = q.y();
              // imu_msg.orientation.z = q.z();
              // imu_msg.orientation.w = q.w();

              //DOF欧拉角（四元数）
              //注意对应关系
              // 传感器坐标系 -> ROS坐标系：
              // X -> Y
              // Y -> X
              // Z -> -Z
              imu_msg.orientation.x =   wit_imu_.rx.data.imu.Quat.q[1];
              imu_msg.orientation.y =   wit_imu_.rx.data.imu.Quat.q[0];
              imu_msg.orientation.z = - wit_imu_.rx.data.imu.Quat.q[2];
              imu_msg.orientation.w =   wit_imu_.rx.data.imu.Quat.q[3];

              //加速度
              imu_msg.linear_acceleration.x = wit_imu_.rx.data.imu.Accel.X;
              imu_msg.linear_acceleration.y = wit_imu_.rx.data.imu.Accel.Y;
              imu_msg.linear_acceleration.z = wit_imu_.rx.data.imu.Accel.Z;

              //角速度
              imu_msg.angular_velocity.x = wit_imu_.rx.data.imu.Gyro.X;
              imu_msg.angular_velocity.y = wit_imu_.rx.data.imu.Gyro.Y;
              imu_msg.angular_velocity.z = wit_imu_.rx.data.imu.Gyro.Z;

              imu_pub_->publish(imu_msg);
              valid_data = false;
            }

          }
          // 继续监听新的数据
          async_receive_message();
      }
      );
    }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  auto node = std::make_shared<ImuNode>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

这种框架对于一个250Hz（每4ms输出一次）的陀螺仪来说,最后/imu的发布频率是100Hz（每10ms发布一次）,效率也是还可以接受的.

激光雷达LiDAR
相机Camera
全球定位GNSS
键盘控制节点
简介

要在 ROS2 中使用 keyboard 节点控制底盘，我们需要实现以下步骤：

创建 ROS2 包 创建一个新的 ROS2 包用于实现键盘控制功能。
编写键盘控制节点 编写一个 C++ 节点，用于监听键盘输入并发布速度命令。
与麦轮底盘通信 将发布的速度命令与底盘控制结合。
Twist消息接口

ros2 interface show geometry_msgs/msg/Twist

# This expresses velocity in free space broken into its linear and angular parts.
# 该消息表示自由空间中的速度，分为线速度和角速度两部分。

Vector3  linear
        float64 x  # 线速度的 x 分量（通常表示前后方向的速度）
        float64 y  # 线速度的 y 分量（通常表示左右方向的速度）
        float64 z  # 线速度的 z 分量（通常表示上下方向的速度）

Vector3  angular
        float64 x  # 角速度的 x 分量（绕 x 轴的旋转速度）
        float64 y  # 角速度的 y 分量（绕 y 轴的旋转速度）
        float64 z  # 角速度的 z 分量（绕 z 轴的旋转速度）

线速度（linear） ：描述物体在三维空间中沿直线运动的速度。通常在移动机器人中：
- x 表示前进/后退。
- y 表示左右平移。
- z 通常为 0，因为大部分地面机器人只在平面上运动。
角速度（angular） ：描述物体在三维空间中绕某一轴旋转的速度。通常在移动机器人中：
- z 表示绕垂直轴的旋转（顺时针/逆时针）。
- x 和 y 通常为 0，因为大多数地面机器人只需要在平面上旋转。

这个消息类型广泛用于控制机器人运动，也就是我们常说的DOF自由度，通过发布到 /cmd_vel 话题，可以为机器人提供运动指令。

发布cmd_vel给单片机

//初始化串口消息订阅
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist("cmd_vel",10,std::bind(&Serial_Node::cmd_vel_sub_callback,this,std::placeholders::_1));
  
  void cmd_vel_sub_callback(const geometry_msgs::msg::Twist &cmd_msg)
  {
    // bool bool_buffer[] = {1, 0, 1, 0};
    // int8_t int8_buffer[] = {0x11,0x22};
    // int16_t int16_buffer[] = {2000,6666};
    // int32_t int32_buffer[] = {305419896};
    fp32 fp32_buffer[] = {(fp32)cmd_msg.linear.x,(fp32)cmd_msg.linear.y,(fp32)cmd_msg.linear.z,
                          (fp32)cmd_msg.angular.x,(fp32)cmd_msg.angular.y,(fp32)cmd_msg.angular.z};

    //由于ROS2中node为局部变量，所以只能在node中调用send函数，所以Serial_Transmit只负责处理data_buffer。
    serial_pack_.tx.Data_Pack(0x01, 
                                 nullptr, 0,
                                 nullptr, 0,
                                 nullptr, 0,
                                 nullptr, 0,
                                 fp32_buffer, sizeof(fp32_buffer) / sizeof(fp32));

    auto port = serial_driver_->port();

    try
    {
      size_t bytes_size = port->send(data_buffer);
      // RCLCPP_INFO(this->get_logger(), "Transmitted: ");
      // for (size_t i = 0; i < data_buffer.size(); ++i) 
      // {
        // RCLCPP_INFO(this->get_logger(), "0x%02X ", data_buffer[i]);  //以十六进制格式打印每个字节
      // }
      RCLCPP_INFO(this->get_logger(), "平动XYZ：%.6f,%.6f,%.6f", fp32_buffer[0],fp32_buffer[1],fp32_buffer[2]);
      RCLCPP_INFO(this->get_logger(), "转动XYZ：%.6f,%.6f,%.6f", fp32_buffer[3],fp32_buffer[4],fp32_buffer[5]);
      RCLCPP_INFO(this->get_logger(), "(%ld bytes)", bytes_size);
      // (void)bytes_size;
    }
    catch(const std::exception &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
    }
  }
};

STM32部分代码：

    case CHASSIS_ROS2_CMD: //ROS2接管模式
        //单位就是m/s和rad/s
        this->oriChassis.Speed.vx =   cmd_vel_.Linear.Y;
        this->oriChassis.Speed.vy =   cmd_vel_.Linear.X;
        this->oriChassis.Speed.vw = - cmd_vel_.Angular.Z;
        break;

由于我的STM32以Y为底盘前后方向，ROS2以X为底盘前后方向，所以我这里并不是完全对应的。

官方节点

ros2 run teleop_twist_keyboard teleop_twist_keyboard

i: 向前移动（增加线速度 linear.x）
, (逗号) : 向后移动（减少线速度 linear.x）
j: 左转（增加角速度 angular.z，逆时针）
l: 右转（减少角速度 angular.z，顺时针）
k: 紧急停止（将线速度和角速度归零）
u: 向前左转（组合运动：linear.x + angular.z）
o: 向前右转（组合运动：linear.x + angular.z）
m: 向后左转（组合运动：-linear.x + angular.z）
. (句号) : 向后右转（组合运动：-linear.x + angular.z）
w/x: 增加/减少 线速度 （linear.x 的增量步长）
a/d: 增加/减少 角速度 （angular.z 的增量步长）
s/S: 紧急停止（将线速度和角速度归零）

建议可以使用官方的节点。记得跑之前，先将步长降低。

自定义节点
功能包创建

ros2 pkg create cyber_teleop_twist_keyboard --build-type ament_cmake --dependencies rclcpp geometry_msgs --node-name teleop_twist_keyboard

节点主逻辑

w/s/a/d：控制机器人前进/后退/左平移/右平移。

q/e：控制机器人逆时针/顺时针旋转。

i/,：调整线速度步长（加速/减速步长）。

j/l：调整角速度步长（减速/加速步长）。

k：停止所有运动。

下方是一个核心函数，用于检测按键。

#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
    
    //查键盘输入的函数，它不需要按回车就能返回键盘输入。它通过修改终端设置，使得可以直接获取按键的值。
    int kbhit(void)
    {
      struct termios oldt, newt;
      int ch;
      int oldf;

      tcgetattr(STDIN_FILENO, &oldt);        // 获取当前终端的设置
      newt = oldt;
      newt.c_lflag &= ~(ICANON | ECHO);       // 禁用缓冲模式和回显
      newt.c_cc[VMIN] = 1;
      newt.c_cc[VTIME] = 0;
      tcsetattr(STDIN_FILENO, TCSANOW, &newt); // 设置新的终端设置

      oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
      fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); // 设置为非阻塞模式

      ch = getchar(); // 获取字符
      tcsetattr(STDIN_FILENO, TCSANOW, &oldt);   // 恢复终端设置
      fcntl(STDIN_FILENO, F_SETFL, oldf);         // 恢复文件描述符设置

      if(ch != EOF) { 
          ungetc(ch, stdin);  // 将字符放回标准输入流
          return 1;           // 如果读取了字符，返回 1
      }

      return 0;   // 没有读取到字符
    }

以下代码未经验证，谨慎使用，本人直接采用了官方的节点，并未使用自定义节点。

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <geometry_msgs/msg/detail/twist__struct.hpp>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include "cyber_teleop_twist_keyboard/struct_typedef.h"

class TeleopTwistKeyboardNode: public rclcpp::Node
{
  public:
    TeleopTwistKeyboardNode():Node("CyberTeleopTwistKeyboardNode_cpp")
    {
      RCLCPP_INFO(this->get_logger(),"TeleopTwistKeyboardNode Running!");
      RCLCPP_INFO(this->get_logger(),"键盘控制机器人节点已启动！");
      // 创建一个发布者，类型是 geometry_msgs/msg/Twist，发布到 "cmd_vel" 话题
      keyboard_control_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
        
      // 定时器，周期性调用发布函数
      keyboard_control_pub_timer_ = this->create_wall_timer(
          std::chrono::milliseconds(100), 
          std::bind(&TeleopTwistKeyboardNode::cmd_vel_publish_timer_callback, this)
        );

    }

  private:
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr keyboard_control_pub_;
    rclcpp::TimerBase::SharedPtr keyboard_control_pub_timer_;

    // 步长
    fp64 linear_step_ = 0.1;
    fp64 angular_step_ = 0.1;

    // 当前线速度和角速度
    fp64 linear_speed_ = 0.0;
    fp64 angular_speed_ = 0.0;

    // 最大速度限制
    fp64 max_linear_speed_ = 0.5;
    fp64 max_angular_speed_ = 0.2;

    // 最小速度限制
    fp64 min_linear_speed_ = 0.0;
    fp64 min_angular_speed_ = 0.0;

    geometry_msgs::msg::Twist cmd_msg;

    //查键盘输入的函数，它不需要按回车就能返回键盘输入。它通过修改终端设置，使得可以直接获取按键的值。
    int kbhit(void)
    {
      struct termios oldt, newt;
      int ch;
      int oldf;

      tcgetattr(STDIN_FILENO, &oldt);        // 获取当前终端的设置
      newt = oldt;
      newt.c_lflag &= ~(ICANON | ECHO);       // 禁用缓冲模式和回显
      newt.c_cc[VMIN] = 1;
      newt.c_cc[VTIME] = 0;
      tcsetattr(STDIN_FILENO, TCSANOW, &newt); // 设置新的终端设置

      oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
      fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); // 设置为非阻塞模式

      ch = getchar(); // 获取字符
      tcsetattr(STDIN_FILENO, TCSANOW, &oldt);   // 恢复终端设置
      fcntl(STDIN_FILENO, F_SETFL, oldf);         // 恢复文件描述符设置

      if(ch != EOF) { 
          ungetc(ch, stdin);  // 将字符放回标准输入流
          return 1;           // 如果读取了字符，返回 1
      }

      return 0;   // 没有读取到字符
    }

  void cmd_vel_publish_timer_callback()
  {

    cmd_msg.linear.y = 0.0;
    cmd_msg.linear.x = 0.0;
    cmd_msg.angular.z = 0.0;

    // 当有键盘输入时，处理它
    if (kbhit()) 
    {
      char key = getchar();  // 获取键盘输入的字符

      if(key == EOF)
      {
        return;
      }
      RCLCPP_INFO(this->get_logger(),"按下了:%c",key);
      // 根据按键决定机器人运动方向
      switch(key)
      {
        case 'w': // 前进
          cmd_msg.linear.y = linear_speed_;
          cmd_msg.linear.x = 0.0;
          cmd_msg.angular.z = 0.0;
          break;
        case 's': // 后退
          cmd_msg.linear.y = - linear_speed_;
          cmd_msg.linear.x = 0.0;
          cmd_msg.angular.z = 0.0;
          break;
        case 'a': // 左平移
          cmd_msg.linear.y = 0.0;
          cmd_msg.linear.x = - linear_speed_;
          cmd_msg.angular.z = 0.0;
          break;
        case 'd': // 右平移
          cmd_msg.linear.y = 0.0;
          cmd_msg.linear.x = linear_speed_;
          cmd_msg.angular.z = 0.0;
          break;
        case 'q': // 逆时针旋转
          cmd_msg.linear.y = 0.0;
          cmd_msg.linear.x = 0.0;
          cmd_msg.angular.z = - angular_speed_;
          break;
        case 'e': // 顺时针旋转
          cmd_msg.angular.z = angular_speed_;
          break;
        case 'i': // 增加线速度
          linear_speed_ = std::clamp(linear_speed_ + linear_step_, min_linear_speed_, max_linear_speed_);
          RCLCPP_INFO(this->get_logger(), "Linear increased to: %.2f", linear_speed_);
          break;
        case ',': // 减小线速度
          linear_speed_ = std::clamp(linear_speed_ - linear_step_, min_linear_speed_, max_linear_speed_);
          RCLCPP_INFO(this->get_logger(), "Linear decreased to: %.2f", linear_speed_);
          break;
        case 'l': // 增加角速度
          angular_speed_ = std::clamp(angular_speed_ + angular_step_, min_angular_speed_, max_angular_speed_);
          RCLCPP_INFO(this->get_logger(), "Angular increased to: %.2f", angular_speed_);
          break;
        case 'j': // 减小角速度
          angular_speed_ = std::clamp(angular_speed_ - angular_step_, min_angular_speed_, max_angular_speed_);
          RCLCPP_INFO(this->get_logger(), "Angular decreased to: %.2f", angular_speed_);
          break;
        case 'k': // 停止
          cmd_msg.linear.y = 0.0;
          cmd_msg.linear.x = 0.0;
          cmd_msg.angular.z = 0.0;
          break;
        default:
          RCLCPP_WARN(this->get_logger(), "Unknown key pressed!");
      }
    }
    keyboard_control_pub_->publish(cmd_msg);
  }

};

int main(int argc, char ** argv)
{
  rclcpp::init(argc,argv);

  auto node = std::make_shared<TeleopTwistKeyboardNode>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

坐标系与话题关系

odom坐标系 不是固定不动的，原点是机器人的初始位置。

base_link坐标系 是机器人本体的坐标系，固定在机器人中心（如底盘）。当机器人移动时，/odom话题发布机器人运动数据，base_link坐标系会相对于odom坐标系移动。

laser_link和imu_link等坐标系 和base_link之间的位置是静态的，所以当base_link移动了，其他子坐标系也会跟随移动。

odom坐标系是根据机器人电机编码器或者里程计反馈回来的数据预估的机器人出发时的位置，一般都会浮动。

map坐标系是通过算法用激光雷达等传感器预估出来的机器人出发时的位置。

但是由于建议使用树形TF，所以我们通常将map坐标系作为根坐标系，所以此时map在rviz2里是不动的，odom作为map坐标系的下行坐标系。（可以听一下赵老师ROS1的课程，这里讲的很好）

硬件平台进阶
轮式里程计标定与融合

场景

里程计在ROS中扮演着重要的角色，能够为机器人的定位、路径规划和运动控制等任务提供重要的数据支持。然而在实际应用中，里程计并不能准确地反映机器人的位置和运动状态，出现的问题包括：

1.里程计中的线速度、角速度和机器人实际的线速度、角速度明显有差异，而且里程计中提供的机器人位姿与真实位姿不一致；

2.当机器人车轮打滑或受到外力影响而出现姿态改变时，这种姿态的变化无法通过轮式里程计正确反馈。

在这些场景中，我们可以通过进行里程计标定和融合来优化解决这些问题。

概念

里程计标定： 里程计标定是指通过对机器人里程计进行精确的校准和调整，以减小或消除里程计测量中的误差和不准确性。例如，可以通过测量调整轮胎直径和轴距等参数来校准机器人的线速度和角速度，从而使里程计测量结果更加准确。里程计标定的主要目标是确保里程计测量结果与实际机器人的运动轨迹一致，从而提高机器人的定位精度和路径规划准确性。

里程计融合： 里程计融合是利用多个来源的里程计数据进行融合，以获得更准确可靠的机器人定位和运动信息。例如，将车轮编码器和惯性测量单元（IMU）的数据融合，可以提高定位的准确性。通过结合不同的传感器和算法，能够提高机器人在导航和控制任务中的准确性和鲁棒性，从而实现更高级别的自主移动能力。

作用

里程计标定和融合是机器人领域中的两个重要技术，通过利用这些技术，机器人可以更加准确地了解自身位置和运动状态，提高机器人的定位和导航精度，为机器人的自主导航、路径规划等任务提供基础。

轮式里程计标定

轮式里程计标定是校准机器人轮子的旋转和移动，以提供机器人运动中的准确位置和姿态信息的方法。通过标定，可以修正因轮子尺寸和轮距等因素引起的误差，获得更准确的运动轨迹。本节将以MyCar导航机器人为例演示标定过程，其标定步骤如下：

准备测量场地；
线性位移标定；
转角位移标定；
验证和调整。

另外，该过程还需要使用尺子作为测量工具。

1.准备测量场地

选择一个开放而平坦的区域，确保没有障碍物和其他干扰因素。

2.线性位移标定

线性位移标定是用来标定机器人在直线移动时行进的距离。该标定与车轮直径参数密切相关，当车轮直径测量不准确或轮胎磨损时，会导致轮式里程计的线性位移出现误差。以下是线性位移标定的主要步骤：

（1）实际数据采集

启动机器人底盘和键盘控制节点，使用键盘控制机器人向前直线运动一段距离，并测量线性位移数据。

（2）里程计消息采集

机器人停止后，输出里程计消息，并获取里程计消息中的机器人的位移数据。

（3）参数计算以及修改

收集实际位移数据（w1）和里程计消息中的位移数据（w2），并结合驱动包配置文件中的原轮胎直径值（d1），即可计算修正后的轮胎直径值（d）。计算公式如下：

d = w1 / w2 * d1;

将计算出的结果写入 MyCar 机器人的配置文件对应的参数，并重新构建。

为了获得最佳结果，可以多次执行上述流程。每次执行后，测量并记录新的实际位移数据（w1）和里程计消息中的位移数据（w2），然后根据新的数据再次计算修正后的轮胎直径值（d）。不断迭代这个过程，可以逐步优化轮胎直径的估计值，减小里程计的线性位移误差。

小提示：

如果使用的是以Arduino为主控的MyCar两轮差速机器人，那么需要修改ros2_arduino_bridge功能包下的params/arduino.yaml文件，该文件中有一个名为wheel_diameter参数即为车轮直径；

如果使用的是以Stm32为主控的MyCar两轮差速机器人，那么需要修改ros2_stm32_bridge功能包下的params/stm32_2w.yaml文件，该文件中有一个名为wheel_diameter参数即为车轮直径；

如果使用的是以Stm32为主控的MyCar四轮差速机器人，那么需要修改ros2_stm32_bridge功能包下的params/stm32_4w.yaml文件，该文件中有一个名为wheel_diameter参数即为车轮直径。

3.转角位移标定

转角位移标定是用来标定机器人在转弯时的角度变化。该标定与底盘的旋转半径参数密切相关，当旋转半径设置不当时，会导致转角位移的测量误差。另外需要注意的是，如果是两轮差速机器人，那么旋转半径和轮间距基本一致。这是因为两轮差速机器人在旋转时没有造成车体的侧滑运动，所以可以使用轮间距来计算旋转角度，并且旋转半径可以近似等于轮间距。然而，对于四轮差速机器人，在旋转时会产生侧滑，轮间距无法直接用于计算旋转角度。四轮差速机器人的旋转半径需要通过实验获取，因为它的旋转半径受到侧滑运动的影响。此外，由于机器人的左右侧轮胎差异或车辆重心偏移等原因，左旋转和右旋转时计算旋转角度所使用的旋转半径可能不同，因此需要进行分别实验来获取左右旋转时的旋转半径。

以下是转角位移标定的主要步骤：

（1）实际数据采集

启动机器人底盘和键盘控制节点，使用键盘控制机器人原地旋转，并测量转角位移数据，比如可以原地旋转一周。

（2）里程计消息采集

机器人停止后，打开rviz2，并添加tf插件，以显示机器人基坐标系（一般是base_link或base_footprint）与里程计坐标系（一般是odom）相对关系。

（3）参数计算以及修改

在机器人原地旋转一周时，如果机器人基坐标系与里程计坐标系相吻合，那么旋转半径参数无需调整；如果机器人基坐标系与里程计坐标系不吻合且rviz2中机器人的旋转角度大于实际旋转角度，那么请将旋转半径调大；如果机器人基坐标系与里程计坐标系不吻合且rviz2中机器人的旋转角度小于实际旋转角度，那么请将旋转半径调小。将计算出的结果写入 MyCar 机器人的配置文件对应的参数，并重新构建。

为了获得最佳结果，可以多次执行上述流程。每次执行后，测量并记录新的实际转角位移数据和rviz2中的坐标系相对关系，然后根据新的数据再次修正旋转半径。通过不断迭代这个过程，可以逐步优化旋转半径的估计值，减小转角位移的测量误差。

小提示：

如果使用的是以Arduino为主控的MyCar两轮差速机器人，那么需要修改ros2_arduino_bridge功能包下的params/arduino.yaml文件，该文件中有一个名为wheel_track参数即为车轮直径，在ros2_arduino_bridge中并未对左旋转和右旋转做区分；

如果使用的是以Stm32为主控的MyCar两轮差速机器人，那么需要修改ros2_stm32_bridge功能包下的params/stm32_2w.yaml文件，在机器人左旋转时，要标定的是名为model_param_acw的参数，在机器人右旋转时，要标定的是名为model_param_cw的参数；

如果使用的是以Stm32为主控的MyCar四轮差速机器人，那么需要修改ros2_stm32_bridge功能包下的params/stm32_4w.yaml文件，和两轮差速类似的，在机器人左旋转时，要标定的是名为model_param_acw的参数，在机器人右旋转时，要标定的是名为model_param_cw的参数。

4.验证和调整

将标定后的轮式里程计应用于实际场景中，并进行验证和调整。观察车辆的实际位置和运动与估计值的差异，并进行必要的调整和校准。另外，当车辆使用环境发生改变，或持续使用较长一段时间后，建议对里程计进行重新标定。

轮式里程计与IMU融合

轮式里程计可以通过监测轮子转动来估计机器人的位置和姿态变化，但容易受到滑动和不均匀地面等特殊情况的影响。IMU可以测量加速度和角速度，提供高频率的运动信息，但容易受到积分漂移和噪声的影响。将它们的数据融合可以弥补彼此的不足，提供更可靠和准确的定位结果，充分利用各自的优势，减少局限性。在ROS2中，提供了robot_localization功能包，可以方便的帮助开发人员实现二者的融合。

1.robot_localization简介

robot_localization是ROS2中的一个功能包，用于实现多传感器数据融合的机器人定位和导航。它提供了一个解决方案，可以将来自不同传感器（如轮式里程计、IMU、GPS、激光雷达等）的数据进行融合，以获得更准确和鲁棒的定位结果。在该功能包中，融合算法已经通过节点封装完毕，调用者只需在调用节点时注入参数即可。

请调用如下指令安装robot_localization：

sudo apt install ros-<distro>-robot-localization

请将<distro>替换为正在使用的ROS2发行版代号。

2.robot_localization节点说明

在robot_localization功能包中提供了ekf_node、ukf_node和navsat_transform_node多个节点来实现机器人的状态估计和定位。不同节点介绍如下：

ekf_node是基于增强卡尔曼滤波器（EKF）的节点，用于融合多个传感器数据来进行状态估计。它接收来自里程计、IMU、激光雷达等传感器的数据，并将它们融合在一起以提供更准确的机器人状态估计。EKF节点使用机器人的运动模型和传感器测量模型来估计机器人的位置、姿态和速度等状态变量。
ukf_node是基于无迹卡尔曼滤波器（UKF）的节点，用于实现机器人的状态估计和本地化。UKF节点采用无迹变换（Unscented Transform）来处理非线性系统，通过估计机器人在空间中的位置和姿态，提供更准确的状态估计。与EKF相比，UKF节点可以更好地处理非线性系统的估计问题。
navsat_transform_node用于将全球定位系统（GNSS）导航卫星数据与惯性测量单元（IMU）数据进行融合，以提供精确的位置估计。该节点校正GNSS信号的误差，并将其转化为带有高精度的位置信息。这有助于改善在室内或具有GNSS信号不稳定性的环境中的机器人定位精度。

这些节点是robot_localization功能包中的核心组件，每个节点都可以融合任意数量的传感器（惯性测量单元（IMU），里程计，室内定位系统，全球定位系统接收器等），它们支持 nav_msgs/msg/Odometry（位置、方向和线性、角速度）、geometry_msgs/msg/PoseWithCovarianceStamped（位置和方向）、geometry_msgs/msg/TwistWithCovarianceStamped（线性和角速度）和sensor_msgs/msg/Imu（方向、角速度和线性加速度）消息，以跟踪机器人的15维（x、y、z、滚动、俯仰、偏航、x˙、y˙、z˙、滚动˙、俯仰˙、偏航˙、x¨、y¨、z¨）状态。

基于这些测量数据，状态估计节点将过滤后的位置、方向和线性、角速度（nav_msgs/Odometry）发布到 /odometry/filtered 话题上，并在启用时将过滤后的加速度发布到 /accel/filtered 话题上。

此外，它们可以（默认启用）将相应的变换作为 tf2 变换进行发布，无论是 odom → base_link 变换还是 map → odom 变换。

3.robot_localization使用示例

假设要融合轮式里程计与IMU，那么请先保证机器人底盘可以发布里程计消息以及IMU消息。在MyCar导航机器人中，Stm32底盘是满足条件的，但是Arduino底盘不包含IMU传感器，需要自行安装。融合时，可以使用ekf_node或ukf_node节点。本案例中使用的是stm32 4w底盘结合ekf_node实现。其大致步骤如下：

编写luanch文件；
编写配置文件；
编译；
执行并查看结果。

由于robot_localization对融合实现已经做了很好的封装，所以整个实现流程是比较简单的。

（1）编写launch文件

在ros2_stm32_bridge功能包的launch目录下新建文件driver_ekf.launch.py，并输入以下内容：

from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
import launch_ros.actions
import os

def generate_launch_description():
    MYCAR_MODEL = os.environ['MYCAR_MODEL']return LaunchDescription([
        launch_ros.actions.Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_filter_node',
            output='screen',
            parameters=[os.path.join(get_package_share_directory("ros2_stm32_bridge"), 'params', 'ekf.yaml')],
           ),
        launch_ros.actions.Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments=['--frame-id', 'base_footprint', '--child-frame-id', 'imu_link', '--x', '-0.15']
        ),
        launch_ros.actions.Node(
            package="ros2_stm32_bridge",
            executable="base_controller",
            parameters=[
                os.path.join(get_package_share_directory("ros2_stm32_bridge"), "params", MYCAR_MODEL + "_ekf.yaml"),],
        )
    ])

该launch文件中，启动了机器人底盘驱动，启动了robot_localization包中ekf_node节点，并且还发布了base_footprint与imu_link的坐标变换。

（2）编写配置文件

在ros2_stm32_bridge功能包的params目录下新建文件stm32_4w_ekf.yaml，并输入以下内容：

/mini_driver:
# /**:  ros__parameters:    base_frame: base_footprint
    baud_rate: 115200    control_rate: 10    encoder_resolution: 44.0    kd: 130    ki: 0    kp: 100    maximum_encoding: 100.0    model_param_acw: 0.45    model_param_cw: 0.45    odom_frame: odom
    port_name: /dev/mycar
    publish_tf: false    qos_overrides:
      /parameter_events:
        publisher:          depth: 1000          durability: volatile
          history: keep_last
          reliability: reliable
      /tf:
        publisher:          depth: 100          durability: volatile
          history: keep_last
          reliability: reliable
    reduction_ratio: 90.0    use_sim_time: false    wheel_diameter: 0.080

在该文件中，一个重要操作是将publish_tf参数设置为false，设置为false后，底盘驱动不再发布base_footprint与odom的坐标变换，而是由ekf_node取而代之。

在ros2_stm32_bridge功能包的params目录下新建文件ekf.yaml，并输入以下内容：

### ekf config file ###
ekf_filter_node:
    ros__parameters:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
        frequency: 30.0

# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
# filter will generate new output. Defaults to 1 / frequency if not specified.
        sensor_timeout: 0.1

# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
        two_d_mode: false

# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
# unspecified.
        transform_time_offset: 0.0

# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. 
# Defaults to 0.0 if unspecified.
        transform_timeout: 0.0

# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
# unhappy with any settings or data.
        print_diagnostics: true

# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
# effects on the performance of the node. Defaults to false if unspecified.
        debug: false

# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
        debug_out_file: /path/to/debug/file.txt

# Whether we'll allow old measurements to cause a re-publication of the updated state
        permit_corrected_publication: false

# Whether to publish the acceleration state. Defaults to false if unspecified.
        publish_acceleration: false

# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
        publish_tf: true

# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
# Here is how to use the following settings:
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
#         odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
#   "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
# from landmark observations) then:
#     3a. Set your "world_frame" to your map_frame value
#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
#         estimation node from robot_localization! However, that instance should *not* fuse the global data.
        map_frame: map              # Defaults to "map" if unspecified
        odom_frame: odom            # Defaults to "odom" if unspecified
        base_link_frame: base_footprint  # Defaults to "base_link" if unspecified
        world_frame: odom           # Defaults to the value of odom_frame if unspecified

# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
# default values, and must be specified.
        odom0: odom

# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
# if unspecified, effectively making this parameter required for each sensor.
        odom0_config: [false,  false,  false,
                       false, false, false,
                       true, false, false,
                       false, false, false,
                       false, false, false]

# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
# the size of the subscription queue so that more measurements are fused.
        odom0_queue_size: 2

# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
# algorithm.
        odom0_nodelay: false

# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
# for twist measurements has no effect.
        odom0_differential: false

# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
        odom0_relative: false

# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
# the thresholds.
        odom0_pose_rejection_threshold: 5.0
        odom0_twist_rejection_threshold: 1.0


        imu0: imu
        imu0_config: [false, false, false,
                      false, false,  true,
                      false, false, false,
                      false, false,  true,
                      false, false,  false]
        imu0_nodelay: false
        imu0_differential: false
        imu0_relative: true
        imu0_queue_size: 5
        imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
        imu0_twist_rejection_threshold: 0.8                #
        imu0_linear_acceleration_rejection_threshold: 0.8  #

# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
        imu0_remove_gravitational_acceleration: true

# [ADVANCED]  The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
# for the velocity variable in question, or decrease the  variance of the variable in question in the measurement
# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
# inputs, the control term will be ignored.
# Whether or not we use the control input during predicition. Defaults to false.
        use_control: true
# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
# false.
        stamped_control: false
# The last issued control command will be used in prediction for this period. Defaults to 0.2.
        control_timeout: 0.2
# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
        control_config: [true, false, false, false, false, true]
# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
        acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
# Acceleration and deceleration limits are not always the same for robots.
        deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
# gains
        acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
# gains
        deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
# However, if users find that a given variable is slow to converge, one approach is to increase the
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
# unspecified.
        process_noise_covariance: [0.05, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.05, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.06, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.03, 0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.03, 0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.06, 0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.025, 0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.025, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.04, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.01, 0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.01, 0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.02, 0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.01, 0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.01, 0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.015]
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
# question. Users should take care not to use large values for variables that will not be measured directly. The values
# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
#if unspecified.
        initial_estimate_covariance: [1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,  0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     1e-9,  0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     1e-9,  0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     1e-9, 0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    1e-9, 0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    1e-9]

在该文件中，关键配置如下：

#......
        odom0_config: [false,  false,  false,
                       false, false, false,
                       true, false, false,
                       false, false, false,
                       false, false, false]
#......
        imu0_config: [false, false, false,
                      false, false,  true,
                      false, false, false,
                      false, false,  true,
                      false, false,  false]

上述配置主要是融合了里程计消息中的x方向速度与imu消息中的yaw角度生成新的里程计数据。

（3）编译

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

colcon build --packages-select ros2_stm32_bridge

（4）执行

终端中进入当前工作空间，执行launch文件：

. install/setup.bash
ros2 launch ros2_stm32_bridge driver_ekf.launch.py

再启动rviz2以及键盘控制节点，在rviz2中将参考坐标系设置为odom，添加tf插件，再添加两个odometry插件并将话题分别设置为odom和odometry/filtered。使用键盘控制机器人前进，两个里程计插件显示的数据和机器人的实际运行基本相符，前进一段时间后，手动搬动机器人让其产生一定角度的偏航，该动作也能在融合后的里程计以及坐标变换中准确显示，而轮式里程计由于电机并未旋转而没有改变（如下图所示）。

激光雷达工具
相机使用进阶
机器人导航Navigation2(实体篇)
准备工作
实物
已经跑过一遍机器人导航Navigation2(仿真篇)
本章只讲大体思路实现，一般只要你仿真篇搞明白了，实体篇看看大体思路就懂怎么实现了。
本章非赵虚左教程，是自己的实现思路，仅供参考，可能赵老师有更好的办法，不过他还没出实体篇教程。
本章用的是4轮麦克纳姆轮实现的，仅供参考。

以下代码都在下方这个github仓库里：

https://github.com/CyberNaviRobot/CyberRobot_ROS2_WS

下方的教程只有实现思路，不会放源码，所以建议克隆一下这个仓库，看看源码。

导航参数参考

https://docs.nav2.org/configuration/index.html

SLAM 定位与建图
slam_toolbox

根据上方的节点说明，我们要订阅/scan和/tf。

一般激光雷达的说明书都会提供源码去发布/scan,所以这个请看你硬件的说明书。

/tf则需要我们发布odom_frame到base_frame的转换，我们必须使用C++代码去动态发布odom的坐标变换。

但是这里你需要发布/odom,以便于知道机器人的位置和姿态，这样才能够够推算出机器人在map中的位置。

确保slam toolbox各项参数没有设置错，特别是坐标系等等，其他参数可以看着说明微调。

确保激光雷达发布的话题是/scan。

先启动激光雷达

再启动urdf模型，同时发布tf。

最后开启slam

ros2 launch mycar_slam_slam_toolbox online_sync_launch.py use_sim_time:=false

cartographer

根据cartographer说明，我们需要/scan和/odom即可。

先打开串口接收节点，接收stm32传过来的数据。

然后再打开里程计节点，发布odom话题

再发布一下TF，可以直接用launch开启robot_state_publisher和joint_state_publisher,并打开urdf模型来发布TF。

打开激光雷达的节点，发布scan话题

等/odom,/scan和TF全发了之后，再打开cartographer建图，然后可以检查map的TF是否发布。

检查TF树如下：

建图如下

地图服务
保存地图(序列化)

mkdir ./map
ros2 run nav2_map_server map_saver_cli -f map/my_map

读取地图(反序列化)

ros2 launch mycar_map_server map_server.launch.py

AMCL自适应蒙特卡洛定位

首先需要地图的数据，发布/map话题。

其次需要激光雷达数据，即/scan话题。

然后需要坐标变换消息，即/tf话题。

然后那个/initial_pose话题，是2D地图上的初始位置，可以用rviz2发布，也可以用C++代码发布。

然后需要修改一下参数：

这个是参数的官方网站：

https://docs.nav2.org/configuration/packages/configuring-amcl.html#

修改完Launch后，再修改params参数。

这里的OmniChassis不止指全向轮底盘，而是广义的全向轮底盘，像全向轮底盘，麦轮底盘都是全向轮底盘。当然也可以自定义底盘类型。

这个配置文件最顶上的那个use_sim_time设置为False。

导航服务器

涉及的话题太多了，所以我们列出来了一个表：

订阅的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 接口 | 描述 | |:—|:—|:—| | /goal_pose | geometry_msgs/msg/PoseStamped | 导航目标点，用于触发导航任务 | | /tf | tf2_msgs/msg/TFMessage | 坐标变换消息，用于不同坐标系之间的转换 | | /odom | nav_msgs/msg/Odometry | 里程计数据，提供机器人位置和运动信息 | | 话题 | 接口 | 描述 | | /global_costmap/footprint | geometry_msgs/msg/Polygon | 机器人（或任何移动平台）的足迹（footprint）信息。足迹是机器人在地图上占据的空间形状，通常用多边形表示。 | | /map | nav_msgs/msg/OccupancyGrid | 发布环境地图，特别是用于导航的占用网格图（Occupancy Grid Map）。 | | /scan | sensor_msgs/msg/LaserScan | 激光扫描数据。 | | 话题 | 接口 | 描述 | | /odom | nav_msgs/msg/Odometry | 机器人的里程计信息，包含位置、速度和姿态 | | /speed_limit | nav2_msgs/msg/SpeedLimit | 导航过程中的速度限制信息，用于动态调整机器人的移动速度 | | 话题 | 接口 | 描述 | | /local_costmap/footprint | geometry_msgs/msg/Polygon | 机器人或移动平台的足迹多边形，用于本地代价地图的计算 | | /scan | sensor_msgs/msg/LaserScan | 激光扫描仪的扫描数据，用于环境感知和避障 | | 话题 | 接口 | 描述 | | /clock | rosgraph_msgs/msg/Clock | ROS系统时间 | | /cmd_vel_teleop | geometry_msgs/msg/Twist | 遥操作命令，用于控制机器人的线性和角速度 | | /local_costmap/costmap_raw | nav2_msgs/msg/Costmap | 局部代价地图的原始数据 | | /local_costmap/published_footprint | geometry_msgs/msg/PolygonStamped | 机器人在局部代价地图中的已发布足迹 | | /preempt_teleop | std_msgs/msg/Empty | 遥操作抢占信号，用于中断当前遥操作 | | 话题 | 接口 | 描述 | | /global_costmap/costmap_raw | nav2_msgs/msg/Costmap | 全局代价地图的原始数据，用于路径规划 | | /global_costmap/published_footprint | geometry_msgs/msg/PolygonStamped | 机器人在全局代价地图中的足迹表示 | | 话题 | 接口 | 描述 | | /cmd_vel_nav | geometry_msgs/msg/Twist | 接收来自其他节点的速度控制指令的话题 |

发布的话题

td {white-space:nowrap;border:0.5pt solid #dee0e3;font-size:10pt;font-style:normal;font-weight:normal;vertical-align:middle;word-break:normal;word-wrap:normal;} | 话题 | 接口 | 描述 | |:—|:—|:—| | /plan | nav_msgs/msg/Path | 当前位置到目标点的全局路径 | | 话题 | 接口 | 描述 | | /global_costmap/costmap | nav_msgs/msg/OccupancyGrid | 发布全局代价地图的当前状态。 | | /global_costmap/costmap_raw | nav2_msgs/msg/Costmap | 未经进一步处理的原始代价地图数据。 | | /global_costmap/costmap_updates | map_msgs/msg/OccupancyGridUpdate | 全局代价地图的更新，该消息可以高效更新地图。 | | /global_costmap/published_footprint | geometry_msgs/msg/PolygonStamped | 发布机器人的足迹（footprint），即机器人在地图上占据的空间形状。 | | 话题名称 | 消息类型 | 描述 | | /cmd_vel_nav | geometry_msgs/msg/Twist | 发布控制命令，包括线性和角速度，用于控制机器人按照规划路径移动。 | | /cost_cloud | sensor_msgs/msg/PointCloud2 | 发布成本地图中的点云数据，用于避障和路径规划。 | | /local_plan | nav_msgs/msg/Path | 发布局部路径规划结果，即机器人应如何到达当前目标点附近的一个点。 | | /marker | visualization_msgs/msg/MarkerArray | 发布可视化标记，用于在RViz等可视化工具中显示路径、障碍物等信息。 | | /received_global_plan | nav_msgs/msg/Path | 发布从全局规划器接收到的全局路径，即当前位置到目标点的路径。 | | /transformed_global_plan | nav_msgs/msg/Path | 发布经过坐标变换的全局路径，确保路径与机器人的当前坐标系一致。 | | 话题 | 接口 | 描述 | | /local_costmap/clearing_endpoints | sensor_msgs/msg/PointCloud2 | 清除成本图上的障碍物点云数据，通常用于动态障碍物处理 | | /local_costmap/costmap | nav_msgs/msg/OccupancyGrid | 本地成本图，表示机器人周围环境的可通行性 | | /local_costmap/costmap_raw | nav2_msgs/msg/Costmap | 未经处理的本地成本图，可能包含更详细的信息 | | /local_costmap/costmap_updates | map_msgs/msg/OccupancyGridUpdate | 本地成本图的更新信息，包括哪些区域发生了变化 | | /local_costmap/published_footprint | geometry_msgs/msg/PolygonStamped | 发布的机器人足迹多边形，时间戳表示发布时间 | | /local_costmap/voxel_grid | nav2_msgs/msg/VoxelGrid | 体素网格数据，用于成本图生成中的空间划分和优化 | | 话题 | 接口 | 描述 | | /cmd_vel | geometry_msgs/msg/Twist | 发送给底层控制器的速度命令 | | 话题 | 接口 | 描述 | | /plan_smoothed | nav_msgs/msg/Path | 经过平滑处理后的全局路径 | | 话题 | 接口 | 描述 | | /cmd_vel | geometry_msgs/msg/Twist | 发布经过处理或平滑后的速度控制指令的话题 |

由于赵虚左是把官方的源码重新写在了WS里，这样对于初学者来说会比较麻烦，对于初学者来说建议使用官方写好的bringup节点，以下是我根据官方Wiki总结出来的使用方法：（这里选择使用官方的bringup节点，而不是赵虚左老师的节点。）

以下是配置的nav2.launch.py文件：

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
# from launch_ros.actions import Node



def generate_launch_description():
    navigation2_dir = get_package_share_directory('nav05_navigation2')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')

    # launch的参数的优先级比yaml的参数优先级高
    use_sim_time = LaunchConfiguration('use_sim_time', default='flase')
    map_yaml_path = LaunchConfiguration('map',default=os.path.join(navigation2_dir,'map','house.yaml'))
    nav2_param_path = LaunchConfiguration('params_file',default=os.path.join(navigation2_dir,'params','nav2.yaml'))

    return LaunchDescription([
        DeclareLaunchArgument('use_sim_time',default_value=use_sim_time,description='Use simulation (Gazebo) clock if true'),
        DeclareLaunchArgument('map',default_value=map_yaml_path,description='Full path to map file to load'),
        DeclareLaunchArgument('params_file',default_value=nav2_param_path,description='Full path to param file to load'),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([nav2_bringup_dir,'/launch','/bringup_launch.py']),
            launch_arguments={
                'map': map_yaml_path,
                'use_sim_time': use_sim_time,
                'params_file': nav2_param_path}.items(),
        ),
    ])

以下是rviz2.launch.py：

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node



def generate_launch_description():
    navigation2_dir = get_package_share_directory('nav05_navigation2')

    use_sim_time = LaunchConfiguration('use_sim_time', default='false')

    rviz_config_dir = os.path.join(navigation2_dir,'rviz','nav2.rviz')

    return LaunchDescription([
        DeclareLaunchArgument('use_sim_time',default_value=use_sim_time,description='Use simulation (Gazebo) clock if true'),

        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'),
    ])

以下是nav2.yaml配置文件（差速模型+DWE局部规划器）：

amcl:
  ros__parameters:
    use_sim_time: false
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_link"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan

amcl_map_client:
  ros__parameters:
    use_sim_time: false

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: false

bt_navigator:
  ros__parameters:
    use_sim_time: false
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_compute_path_through_poses_action_bt_node
    - nav2_smooth_path_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_drive_on_heading_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_globally_updated_goal_condition_bt_node
    - nav2_is_path_valid_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_truncate_path_local_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_path_expiring_timer_condition
    - nav2_distance_traveled_condition_bt_node
    - nav2_single_trigger_bt_node
    - nav2_is_battery_low_condition_bt_node
    - nav2_navigate_through_poses_action_bt_node
    - nav2_navigate_to_pose_action_bt_node
    - nav2_remove_passed_goals_action_bt_node
    - nav2_planner_selector_bt_node
    - nav2_controller_selector_bt_node
    - nav2_goal_checker_selector_bt_node
    - nav2_controller_cancel_bt_node
    - nav2_path_longer_on_approach_bt_node
    - nav2_wait_cancel_bt_node
    - nav2_spin_cancel_bt_node
    - nav2_back_up_cancel_bt_node
    - nav2_drive_on_heading_cancel_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: false

controller_server:
  ros__parameters:
    use_sim_time: false
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    #precise_goal_checker:
    #  plugin: "nav2_controller::SimpleGoalChecker"
    #  xy_goal_tolerance: 0.25
    #  yaw_goal_tolerance: 0.25
    #  stateful: True
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.26
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.26
      min_speed_theta: 0.0
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      acc_lim_x: 2.5
      acc_lim_y: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: false

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: false
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True

  local_costmap_client:
    ros__parameters:
      use_sim_time: false
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: false

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: false
      robot_radius: 0.22
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      always_send_full_costmap: True

  global_costmap_client:
    ros__parameters:
      use_sim_time: false
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: false

map_server:
  ros__parameters:
    use_sim_time: false
    yaml_filename: "house.yaml"

map_saver:
  ros__parameters:
    use_sim_time: false
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: false
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: false

smoother_server:
  ros__parameters:
    use_sim_time: false
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: True

behavior_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors/DriveOnHeading"
    wait:
      plugin: "nav2_behaviors/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_tolerance: 0.1
    use_sim_time: false
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

robot_state_publisher:
  ros__parameters:
    use_sim_time: false

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200

velocity_smoother:
  ros__parameters:
    use_sim_time: false
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.26, 0.0, 1.0]
    min_velocity: [-0.26, 0.0, -1.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

多车编队
Moveit2工业机器人机械臂

https://moveit.ros.org/

机器人学

https://www.bilibili.com/video/BV1v4411H7ez

https://www.bilibili.com/video/av59243185

理论基础
DOF(自由度)

简单来说，自由度(以下统称为dof)指的是 物体在空间里面的基本运动方式 ，总共有6种。任何运动都可以拆分成这6种基本运动方式，而这6种基本运动方式又可以分为两类：位移和旋转。

位移：X轴、Y轴、Z轴的平动

旋转：Roll横滚角(绕X转动)、Pitch俯仰角(绕Y转动)、Yaw航向角(绕Z转动)

数理基础
位姿（位置与姿态）的表示

倘若在一个空间里有一个刚体（frame），我们如何确定刚体在这个空间里的位姿呢？

首先要建立一个世界坐标系（world frame），然后要在刚体上建立刚体坐标系（body frame）.

位置的描述

描述刚体的质心(一个点)在世界中的位置，就可以用一个3X1向量来表示.

这样就知道了平动的三个DOF。

方位的描述

设世界坐标系为A，刚体坐标系为B。

上面这个矩阵就叫旋转矩阵（Rotation Matrix），是一个3*3的正交矩阵，ABR描述的是A为参考坐标系，B相对于A的方向。

每一个列向量，都代表B的对应坐标轴各自指向的方向。

每一列向量都是B的对应的坐标轴相对于A的方向余弦（Direct Cosines）。

旋转矩阵的每个元素 r ij代表 B 的第 j 轴与 A 的第 i 轴的方向余弦.

实在看不懂，先来看下面来看例子：

B的X轴在A中怎么表示？可以看出来，B的X正好是A的Z轴的负半轴，那就是0，0，-1.

B的Y轴在A中怎么表示？可以看出来，B的Y正好是A的Y轴的正半轴，那就是0，1，0.

B的Z轴在A中怎么表示？可以看出来，B的Z正好是A的X轴的正半轴，那就是1，0，0.

这个例子里，AB的Z重合了，所以我们只看上视图就可以了。

就是把XB这个单位向量，投影到A的X和Y上看分量即可。

同理YB也一样。

ZB和ZA重合，比较简单。

答案是B。

位姿的描述

通过BF在WF的状态，就可以知道刚体在世界中的位姿。

运动的描述

红色是刚体运动的轨迹线，

轨迹（平动DOF）对时间的微分(导数)，就是刚体线速度。

刚体速度再对时间的微分(导数)，就是刚体线加速度。

同理,转动DOF对时间的微分(导数)，就是刚体的角速度。

角速度再对时间的微分(导数)，就是刚体角加速度。

旋转矩阵

特性：

由于旋转矩阵R里每个元素都是两个向量内积，内积是可以交换位置且最后结果数值不变的。

所以我们选择交换位置。

原始的R是每一列都是B的某一轴在A系的分量。

交换位置后的R是每一行都是A的某一轴在B系的分量。

结论：所以说Rab = Rba的T。

他俩明显是转置关系。

RT*R=I3（3*3单位阵）(正交阵orthogonal matrix的性质)

还有个性质就是

虽然有9个数字，但是啊，因为正交阵的性质，所以是有约束条件的，这9个数字里有6个数字是随着其他数字变化而变化的，所以这9个数字实际上只有3个参数可以任意选择，也就是旋转矩阵实际上只有3个自由度。(转动DOF)

旋转矩阵的一个功能如下：

比如说另一个坐标系B相对于A坐标系绕X,Y,Z轴各自逆时针转动theta度的旋转矩阵。

在中国大陆教材中，常用R(X,theta)来代替图中的RXA(theta)。图中这个A指的是原坐标系，得出来的AP`也是原坐标下的坐标。

这样的话，AP左乘一个R就得出来了P转动后在A系的坐标。（一定要与下面讲的旋转坐标变换分清楚，很容易混淆）

P逆时针转动30度后，在原坐标系中的坐标为002.

总结：旋转矩阵主要是三种用法，如下图：

坐标变换
平移坐标变换
旋转坐标变换

这里的APX是个数值，XA等是矢量，加法是矢量加法，最后得出来的是AP向量。

重要结论就是AP = ABR*BP.(注意是矩阵的左乘)

AP就是P在A系的坐标。

BP就是P在B系的坐标。

ABR就是A为参考坐标系，B相对于A的旋转矩阵。

物体的变换及逆变换

物体平动的顺序可以互相颠倒,但是物体转动的顺序不能互相颠倒,否则姿态会不一样.

主要是两种拆解方式,一个是设一个固定的坐标系,一直按这个坐标系转动,

另一个方式是假设物体的坐标系.

机械臂描述方式

Link 0一般也叫base_Link

先看好相对关系，Axis i-1的后面才是Link i - 1(当然其他描述也成立)

描述各关节之间的关系

也就是公垂线(唯一解)，其长度为Link Length连杆长度

现在只是限制住了两个轴的距离，两个轴还是可以转动的，所以需要下一个参数，Link Twist连杆扭角。

这个角就是沿中垂线，把后一个轴线沿中垂线往当前轴移动，然后形成的夹角叫Link Twist连杆扭角。

也就是说，针对空间中任意两个转轴，我们需要两个参数来进行描述，也就是Link Length和Link Twist。

如果是多个串起来的转轴，我们就无法找到对应关系了，比如说

上面这个图，我没法表示ai-1与ai在轴线Axis i上的相对关系以及相对姿态是什么样子的。

所以还需要其他参数。

首先肯定需要一个长度，两个公垂线在Axis i上的距离，叫做Link Offset，连杆偏距。

然后还需要一个角，Joint Angle连杆夹角，也叫关节角。

其实发现，这四个参数，只有一个参数是变化的，其他都是固定的。

如果ioint type是revolute joint，那么thetai变化，其他不变。

如果joint type是prismatic joint，那么di变化，其他不变。

描述两个轴需要2个参数，连杆长度与连杆扭角。

描述多个轴串在一起需要4个参数(每两两杆件都需要4个参数)，连杆长度Link Length，连杆扭角Link Twist，连杆偏距Link Offset，连杆夹角Joint Angle。

在joint上建立frame

咱们一般把Z方向定义成和转轴的方向一样，Z朝上或朝下是看怎么朝向，这两个轴的夹角最小，这样就能够确定Z的方向了。

Xi的方向是沿着ai的方向。

Xi与Zi+1和Zi都垂直。

右手定则，判断Y方向。

原点是Z和X的交点。

若是建立base_link(link0)与link1的话，则是特殊情况。（base_link是immobile不动的）

frame0和frame1重叠(重合)。通常，比如说是个旋转关节，虽然规定theta是arbitrary任意的，但是我把theta固定成0，然后让frame0和frame1(theta

= 0)重合。如果是平动关节，那么同理也取d=0的时候的frame1。注意，这里是重叠重合，并不是形状相同，而是完全重叠的坐标系。

最后一个杆件，也差不多，因为Xn和Xn-1都要垂直于Axis n(Zn)，所以最简单的方法就是让Xn与Xn-1方向一致。

Xn取Xn-1的方向。也就是framen和framen-1是延长的。

下面是重点中的重点：(有好几种方法判断，如有错误请讨论后修改)

需要知道的常识：

一般连杆扭角按逆时针是正值。
我们常说的转向，是从逆着的方向去看，也就是让箭头指向眼睛的方向去看的。
从轴的逆方向去看和顺着方向去看转向，是完全相反的方向，但是在平面上容易产生视觉错觉，难以理解。可以拿支笔或者电机，转动一下试试。

①alphai-1是正值还是负值，要看Zi-1到Zi的角是顺时针还是逆时针，伸出右手，让拇指沿Xi-1的方向，如果alphai-1顺着四指方向则为逆时针，正值，反之为顺时针，负值。

所以如图，alphai-1是逆时针，所以是正值。

②ai-1的长度因为是长度，所以永远是正值，然后值为Z轴间的相对距离。

③thetai的角度也基本同理，将右手大拇指沿着Zi的方向，若thetai顺着四指方向则为逆时针，逆着则为顺时针。

④di的大小方向要看从ai-1沿着zi的方向到ai，则是正值，反之为负值，大小即为距离。

Link Transformations
理论

我们两个关节都有俩轴，一个Axisi-1一个是Axisi，我们也有俩frame，一个是framei-1一个是framei。

我们需要找到两个frame之间的关系式是什么，也就是找到变换矩阵Transformation Matrix。

然后将Trans Matrix量化即可。

假设说有一个点P，他在frame i下的表达是Pi，如果我们找到了Ti-1 i的矩阵，那么就有办法，获得P在frame i-1下的表达了。

所以我们现在需要，用刚才找到的四个参数，转化成我们的Trans Matrix。

这四个参数，也就是ai-1，alphai-1，di和thetai，足以可以表达framei-1到framei了。

①首先在Axis i-1上，

先描述alpha，就把framei-1的Zi-1旋转到差不多Zi的方向，生成FrameR（只旋转Z，X不动，然后右手定则判断Y）

②然后描述a，

就把FrameR沿着ai-1的方向移动到Zi上，

③再描述theta

我们转动frameR中的ZR，使其与ai方向相同，生成frameP（X动，Z不动，右手定则判断Y）

这样搞完之后，Xp的方向与Xi是相同的，Zp的方向也与Zi相同。

④再描述d，也就是把FrameP往上拉，最后会与Framei重合。

也就是从Framei - 1到Frame R，然后再到Frame Q，然后再到Frame P，最后到Frame i。一共四次转化。

刚才我们演示的是从Pi-1到Pi，现在我们要求的是从我们的Pi要到Pi-1，那么就是倒着左乘，先乘Tp i接着往下以此类推。

从连杆i到连杆i-1的坐标系间的齐次变换矩阵T i-1 i=Rot(X，aplhai-1)Trans(ai-1,0,0)Rot(Z,thetai)Trans(0,0,di)

左上角是3X3的旋转矩阵，所以只有角度theta和alpha参数，

右上角3X1的矩阵，也就是frame i的原点相对于frame i - 1的原点的向量。然后是从frame i - 1去看。所以他是长度与角度的复合。

最后一行的1X4的矩阵，是0001是固定数不动的。

对于连续的杆件，我们可以从base_link一直算到linkx，x想是几就是几。

比如说，现在有三个杆件，我们找到T23（3对2的），T12（1对2的）T01（地对1的），就可以找到T03（地对3的）

Example
平面RRR类型

①先找到关节转轴Joint Axes

三个转轴是三个红点，是点的话，也就是出纸面的方向。（因为，他是个平面的，所以说，每个Z轴之间的角度都是0，所以说Link Twist Alpha是0，所以Z的方向都随便取，但是咱们这里，假设关节都是逆时针旋转，按右手螺旋定则来看，Z就都朝上）

②再找到公垂线Common Perpendiculars

但是由于Axis都是互相平行的，所以说，这个公垂线有无数多条，所以我们就在一个平面内表达即可。

③下一步定义Zi向量（Z方向与转轴方向相同所以也是向上。）

④然后判断中间的Xi向量

⑤然后判断中间的Yi向量

⑥然后判断头和尾，也就是frame 0 和frame n

其实，我们可以把frame0和frame1建的完全重合，就是让alpha和theta都为0，如图并没有重合，所以有theta大小。

最后一个杆件也一样，建议让X3的方向和X2方向重合，这样的话，theta和alpha都是0。

因为每个轴都是平行的，所以alpha是0，由于ai都是同平面的，所以d也为0.

因为frame0和frame1重合，所以a0 = 0，然后a1 = L1，a2 = L2

由于全是RRR，所以，theta都是变化的角。

P点末端执行器，也可以算出，沿X3方向走，获得P在frame n的表达，然后就可以推出P在frame 0中的表达了。

如图的，P点在Frame3里的坐标是（L3，0，0）

RPR类型

①先找到转轴

按右手螺旋来。

②找公垂线

因为frame1的Z1和frame2的Z2相交，所以，没有公垂线。

frame2的Z2和frame3的Z3重合，所以也没有公垂线。

也就是说a全是0.

③建立Zi向量

Zi方向与转轴方向相同。

④Xi的向量

当Z1和Z2相交时，我们挑X的方向就挑和Z1和Z2都垂直的，有两种方案，要么X往前，要么往后，如图是往后的。

X1和X2必须平行，因为是个P类型的关节

⑤Y轴

⑥Frame 0和frame n

frame 0 和frame1重合

让X3方向与X2方向相同

如图，驱动的关节参数分别是theta1，d2，theta3

由于frame0和frame1重合，那么alpha0 = 0，

从Z1到Z2的角，伸右手大拇指指向X1，然后四指与Z1到Z2方向相同，所以是逆时针，为正值，所以是90度。

然后fame2到frame3，Z共线，所以alpha2=0

然后由于Zi有的相交，有的共线，所以a全是0

然后d1是0，因为frame0和frame1重合，

d2是d2，d3是L2（d是X在Axis上的距离）

P点在Frame3上的坐标为（0，0，L3）

其实Z方向有俩选择，X也有俩选择，一共四种选择，选择自己好理解，好计算的方案即可。

中国台积电晶圆机器人(PRRR类型4个自由度)

SCARA机器人(RRRP类型4个自由度)

该机器人最后一个关节是既可以R又可以P的，所以是个RP关节，既可以先算R也可以先算P。

RP类型

选D，因为俩自由度，所以有俩驱动参数。

执行器关节与笛卡尔空间(Actuator Joint and Cartesian Spaces)

我们这里的驱动是theta1-3，

当我们知道theta1-3的值之后，我们就会知道P点在世界坐标系上的表达。

这被叫做正向运动学（Forward Kinematics）。

由P点世界坐标系反算关节角度，那么叫逆向运动学（Inverse Kinematics）。

Actuator Space就是驱动器空间，比如一个电机怎么操控能转joint space下的固定角（经过一系列转换）。

有转动有移动的部分（所以需要两个电机来达到这两个自由度）

同步带机构使其旋转。（第一个电机）

齿轮齿条机构达到上下移动。（第二个电机）

但是，这两个并不是独立的，因为是同轴驱动的，所以有些负荷。

两个转动变成一个转动一个移动。

正运动学

定义：已知机器人各个关节（或轮子等驱动单元）的运动参数（如角度、位移、速度等），计算末端执行器的位置和姿态。

ads=dv/dt * ds = ds/dt *dv = vdv

牛顿第二定律，能量守恒，冲量与动量

Wp就是P点在世界坐标系下的坐标

可以根据该公式，得知末端执行器在世界坐标系中的坐标。

逆运动学

定义：已知末端执行器的目标位置和姿态，计算需要让各个关节（或轮子等驱动单元）运动到什么角度或速度才能达到该目标。

先知道末端执行器的某个点P在世界坐标系中的表达，也就是给出Pw或者末端执行器某个点上的frameH，

通过Pw求出theta。

这样手臂就有6个未知数

16个数字，转动的部分占了9个数字，也就是左上角的3X3的旋转矩阵，然后右上角3X1的向量表示相对于原点的位移量是什么。（也就是frame6的原点相对于frame0的原点位移量是什么）

下面的0001是整数，固定的，不变的。

这个旋转矩阵里，有3个长度条件，3个互相垂直条件，所以9个数字里，就剩3个自由度。（也就是向量长度为1限制3个，向量两两垂直限制3个，所以是平移矩阵，3个自由度）

然后右上角3X1的向量中，相对原点的坐标X,Y,Z,那么就是3个自由度。

所以总共有6个自由度。

这12个方程式就是除了低下的0001，上面的参数都可以列一个式子。我们要做的，就是从12个式子中求出6个未知数。

灵活工作空间Dexterous workspace是可达工作空间Reachable workspace的子集。

它的可达工作空间是个圆环。

对于某个点，在这个例子中，只有1种或2种姿态可以达到。这个机器人只有RWS，没有DWS。

手臂一样长的话，那样工作空间就是个圆了，

有一个点就是DWS,就是原点，当手臂内折，那么可以以360度任意一个角度来达到这个点，所以该点就是DWS。

MicroROS

https://micro.ros.org/

没啥必要，串口够用了。

除非你想极低成本跑ROS2,比如说车上只放一个ESP32或者STM32跑MicroROS，然后电脑跑ROS2在旁边运行其他需要大型计算的节点。

ROS2_Control

官网:

https://control.ros.org/humble/index.html

仓库:https://github.com/ros-controls/ros2_control

第三方功能包（开发者:@brainyuan:https://b23.tv/L0sEXPL）:https://github.com/Factor-Robotics/odrive_ros2_control

Webots仿真平台

这个平台用来测运动学很舒服。

可以SolidWorks转URDF（ROS官方提供sw2udfo工具），再URDF转Webots（webots官方提供urdf2webots工具）。

https://github.com/ros/solidworks_urdf_exporter

https://github.com/cyberbotics/urdf2webots

这样的话，连电机甚至都给你选好了，你只需要写运动学即可。

运动控制程序可以用C/C++、Python、Java等语言编写，控制的时候只需要注意一下控制电机角度和速度的方式以及单位即可。

如果你给电机setPosition(INFINITY)的话，那么这个电机将变成角度电机，而原本设置速度的函数将会变成设置角速度。

如果你没给电机设置setPosition(INFINITY)，那么这个电机就是个速度电机。

角度单位是rad（存疑）

速度单位是rad/s（存疑）

        for (int i = 0;i < 4;i++)
        {
                //舵电机仿真中默认顺时针为正
                swerve_motor[i] = robot->getMotor("swerve_joint" + std::to_string(i));
                //swerve_motor[i]->setPosition(INFINITY);       // 注释掉：不启用速度控制
                swerve_motor[i]->setPosition(0.0);       // 初始角度为0
                swerve_motor[i]->setVelocity(50.0);       // 设置角速度

                //驱动电机仿真中默认向后滚为正
                drive_motor[i] = robot->getMotor("drive_joint" + std::to_string(i));
                drive_motor[i]->setPosition(INFINITY);   // 启用速度控制
                drive_motor[i]->setVelocity(-0.0);        // 初始速度为0
        }

OpenCV
OpenCV

基础视觉算法-OpenCV实现

CV_Bridge

cv_bridge维基百科介绍:

https://wiki.ros.org/cv_bridge

https://index.ros.org/p/cv_bridge/

ROS2Humble的cv_bridge仓库链接(注意选择对应版本的分支branches):

https://github.com/ros-perception/vision_opencv/tree/humble

安装

提前自己编译好带CUDA的OpenCV4,详见电控组环境搭建大全

apt安装(不建议)

由于ros自带的cv_bridge自动链接ros自带的oepncv版本,所以我们一般不会用ros2自带的cv_bridge,一般都需要自己手动编译一个cv_bridge.

# 通用命令
sudo apt install ros-<ros2-distro>-vision-opencv
# ROS2 Humble
sudo apt install ros-humble-vision-opencv
# ROS2 Jazzy
sudo apt install ros-jazzy-vision-opencv

源码编译安装(建议)

本教程以jazzy为例子.

首先克隆仓库,克隆jazzy,humble,rolling都可以,只要是ROS2的基本都没啥大变化.但是官方暂时没出jazzy,我就直接克隆默认的rolling了.

新建一个文件夹

mkdir ~/ros2_ws/src
cd ~/ros2_ws/src
# 克隆源码
git clone https://github.com/ros-perception/vision_opencv.git
cd vision_opencv
# 如果是humble建议:
git checkout humble

安装依赖:

sudo apt install python3-numpy
sudo apt install libboost-python-dev

修改cv_bridge的CMakeLists

将原本的find_package(OpenCV 4 QUIET)改为精确匹配版本，并添加EXACT参数：

EXACT是未找到精确版本时，CMake会报错并终止构建.

find_package(OpenCV 4.11 EXACT QUIET
  COMPONENTS
    opencv_core
    opencv_imgproc
    opencv_imgcodecs
  CONFIG
)

cd ~/ros2_ws
# 下面这三个根据情况三选一,一般是第一个colcon build --symlink-install

# 如果你曾经没编译过
colcon build --symlink-install
# 如果你只想编译cv_bridge
colcon build --symlink-install --packages-select cv_bridge
# 如果你曾经编译过一遍,则需要下列命令
colcon build --symlink-install --packages-select cv_bridge --allow-overriding cv_bridge

验证:

# 列出cv_bridge链接的opencv版本
ldd ./install/cv_bridge/lib/libcv_bridge.so | grep opencv 

如下图,我的成功链接到411了,也就是opencv4.11版本.

接下来配置环境:

vim ~/.bashrc

在source /opt/ros/jazzy/setup.bash的下一行加入下面这句

source ~/ros2_ws/install/setup.bash

输入:wq保存

完成安装与环境配置结束.

ROS2介绍

ROS2简介

ROS2框架

ROS2参考资料

官方资料

推荐的视频资料与配套文档

学习ROS2需要用到的知识储备

安装ROS2

二进制包安装ROS2(以Kubuntu Jammy 22.04安装 ROS Humble 为例)

进入ROS官网

安装ROS2前期准备工作

换源(国内源)

通过apt安装ROS2

ROS2基础环境变量配置

测试ROS2

从源码安装ROS2(难度较高，Ubuntu用上面那个简单方法即可。)

Fedora或者Rocky

Debian

Docker安装ROS2

ROS2安装进阶(可选)

安装进阶的包

apt安装方式（Ubuntu强烈建议）

源码方式（非Ubuntu）

黄色的包

蓝色的包

常见问题

环境进阶

Apt方式

源码方式

测试ROS2

示例1

示例2

常见问题

Rviz2闪烁

入门操作

简介

终端环境搭建

命令行操作

ROS2 HelloWorld(C++)

ROS2 HelloWorld(Python)

运行优化(bash终端环境)

VScode环境搭建

运行优化(colcon build)

VScode环境进阶

clangd插件代码提示(可选,但是建议)

安装其他工具

ROS2体系框架

工作空间与功能包

工作空间简介

源文件编译

配置文件

常用操作命令

核心模块_通信相关

核心模块_工具相关

功能包

ROS2技术支持

ROS2应用方向

NAV2

OpenCV

MoveIt

The Autoware Foundation

F1 Tenth

microROS

Open Robotics

PX4

ROS-Industrial

四大通信

通信机制简介与代码模板

话题通信_理论

话题通信_实验1(C++)

话题通信_自定义接口信息

话题通信_实验2(C++)

话题通信_rqt查看计算图

服务通信_理论

服务通信_实验1_服务端实现(C++)

服务通信_实验1_客户端实现(C++)

动作通信_理论

动作通信_实验1_服务端实现(C++)

动作通信_实验1_客户端实现(C++)

坐标系变换监听_C++

坐标点变换监听_C++