ROS2 学习笔记

一些理论

自动驾驶三层架构

感知层

传感器数据

激光雷达
IMU（惯导）
GPS
摄像头
...

决策层

决策算法

路线规划
路径规划
运动规划
控制
....

执行层

底盘控制

线控驱动
线控制动
线控转向
...

ROS2 主题

ROS2概念总览

工作空间(Workspace)：开发过程的大本营

在ROS机器人开发中，我们针对机器人某些功能进行代码开始时，各种编写的代码、参数、脚本等文件，也需要放置在某一个文件夹里进行管理，这个文件夹在ROS系统中就叫做工作空间。

工作空间目录

目录名	中文	解释
src	代码空间	存放编写的代码、脚本
doc	文档空间	存放编写的文档
build	编译空间	保存编译过程中产生的中间文件
install	安装空间	保存编译得到的可执行文件和脚本
log	日志空间	编译和运行过程中，保存各种警告、错误、信息等日志

一些命令

安装ROS2开发工具

shell
sudo apt install -y ros-dev-tools
sudo rosdep init && rosdep update

安装依赖

shell
rosdep update && rosdep install -i --from-path src --rosdistro ${ROS_DISTRO} -y

编译工作空间

shell
colcon build

只编译其中一个功能包

shell
colcon build --packages-select YOUR_PKG_NAME

不编译测试单元

shell
colcon build --cmake-args -DBUILD_TESTING=0

跳过测试

shell
colcon build --catkin-skip-building-tests

使用单线程编译

shell
MAKEFLAGS="-j1"
colcon build --executor sequential

允许通过更改src下的部分文件来改变 install（重要）

每次修改 python 脚本时都不必重新 build

shell
colcon build --symlink-install

设置工作空间的环境变量

shell
source install/local_setup.bash

功能包(Package)：功能源码的聚集地

每个机器人可能有很多功能，比如移动控制、视觉感知、自主导航等，如果我们把这些功能的源码都放到一起当然也是可以的，但是当我们想把其中某些功能分享给别人时，就会发现代码都混合到了一起，很难拆分出来。

创建功能包

shell
# 在工作空间的src目录下执行
ros2 pkg create --build-type <build-type> <package_name>

C++ 版本

shell
ros2 pkg create --build-type ament_cmake learning_pkg_cpp

Python 版本

shell
ros2 pkg create --build-type ament_python learning_pkg_py

功能包的目录文件

C++ 版本


learning_pkg_cpp/
├── CMakeLists.txt
├── include
│   └── learning_pkg_cpp
├── package.xml
└── src

Python 版本


learning_pkg_py/
├── learning_pkg_py
│   └── __init__.py
├── package.xml
├── resource
│   └── learning_pkg_py
├── setup.cfg
├── setup.py
└── test
    ├── test_copyright.py
    ├── test_flake8.py
    └── test_pep257.py

节点(Node)：机器人的工作细胞

机器人是各种功能的综合体，每一项功能就像机器人的一个工作细胞，众多细胞通过一些机制连接到一起，成为了一个机器人整体。在ROS中，我们给这些 “细胞”取了一个名字，那就是节点。

代码

Python 版本

python
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2节点示例-发布“Hello World”日志信息, 使用面向对象的实现方式
"""

import rclpy                                     # ROS2 Python接口库
from rclpy.node import Node                      # ROS2 节点类
import time

"""
创建一个HelloWorld节点, 初始化时输出“hello world”日志
"""
class HelloWorldNode(Node):
    def __init__(self, name):
        super().__init__(name)                       # ROS2节点父类初始化
        while rclpy.ok():                            # ROS2系统是否正常运行
            self.get_logger().info("Hello World")    # ROS2日志输出
            time.sleep(0.5)                          # 休眠控制循环时间

def main(args=None):                                 # ROS2节点主入口main函数
    rclpy.init(args=args)                            # ROS2 Python接口初始化
    node = HelloWorldNode("node_helloworld_class")   # 创建ROS2节点对象并进行初始化
    node.destroy_node()                              # 销毁节点对象
    rclpy.shutdown()                                 # 关闭ROS2 Python接口

C++ 版本

cpp
/***
@作者: 古月居(www.guyuehome.com)
@说明: ROS2节点示例-发布“Hello World”日志信息, 使用面向对象的实现方式
***/

#include "rclcpp/rclcpp.hpp"
#include <unistd.h>

/***
创建一个HelloWorld节点, 初始化时输出“hello world”日志
***/
class HelloWorldNode : public rclcpp::Node {
public:
  HelloWorldNode() : Node("node_helloworld_class") {  // ROS2节点父类初始化
    while (rclcpp::ok()) {                            // ROS2系统是否正常运行
      RCLCPP_INFO(this->get_logger(), "Hello World"); // ROS2日志输出
      sleep(1);                                       // 休眠控制循环时间
    }
  }
};

// ROS2节点主入口main函数
int main(int argc, char *argv[]) {

  // ROS2 C++接口初始化
  rclcpp::init(argc, argv);

  // 创建ROS2节点对象并进行初始化
  rclcpp::spin(std::make_shared<HelloWorldNode>());

  // 关闭ROS2 C++接口
  rclcpp::shutdown();

  return 0;
}

话题(Topic)：节点间传递数据的桥梁

节点实现了机器人各种各样的功能，但这些功能并不是独立的，之间会有千丝万缕的联系，其中最重要的一种联系方式就是话题，它是节点间传递数据的桥梁。

发布/订阅模型

代码

发布者

Python 版本

python
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2话题示例-发布“Hello World”话题
"""

import rclpy                                     # ROS2 Python接口库
from rclpy.node import Node                      # ROS2 节点类
from std_msgs.msg import String                  # 字符串消息类型

"""
创建一个发布者节点
"""
class PublisherNode(Node):
    
    def __init__(self, name):
        super().__init__(name)                                    # ROS2节点父类初始化
        self.pub = self.create_publisher(String, "chatter", 10)   # 创建发布者对象（消息类型、话题名、队列长度）
        self.timer = self.create_timer(0.5, self.timer_callback)  # 创建一个定时器（单位为秒的周期，定时执行的回调函数）
        
    def timer_callback(self):                                     # 创建定时器周期执行的回调函数
        msg = String()                                            # 创建一个String类型的消息对象
        msg.data = 'Hello World'                                  # 填充消息对象中的消息数据
        self.pub.publish(msg)                                     # 发布话题消息
        self.get_logger().info('Publishing: "%s"' % msg.data)     # 输出日志信息，提示已经完成话题发布
        
def main(args=None):                                 # ROS2节点主入口main函数
    rclpy.init(args=args)                            # ROS2 Python接口初始化
    node = PublisherNode("topic_helloworld_pub")     # 创建ROS2节点对象并进行初始化
    rclpy.spin(node)                                 # 循环等待ROS2退出
    node.destroy_node()                              # 销毁节点对象
    rclpy.shutdown()                                 # 关闭ROS2 Python接口

C++ 版本

cpp
/***
@作者: 古月居(www.guyuehome.com)
@说明: ROS2话题示例-发布“Hello World”话题
***/

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"       // ROS2 C++接口库
#include "std_msgs/msg/string.hpp" // 字符串消息类型

using namespace std::chrono_literals;

class PublisherNode : public rclcpp::Node {
public:
  PublisherNode()
      : Node("topic_helloworld_pub") // ROS2节点父类初始化
  {
    // 创建发布者对象（消息类型、话题名、队列长度）
    publisher_ = this->create_publisher<std_msgs::msg::String>("chatter", 10);
    // 创建一个定时器,定时执行回调函数
    timer_ = this->create_wall_timer(
        500ms, std::bind(&PublisherNode::timer_callback, this));
  }

private:
  // 创建定时器周期执行的回调函数
  void timer_callback() {
    // 创建一个String类型的消息对象
    auto msg = std_msgs::msg::String();
    // 填充消息对象中的消息数据
    msg.data = "Hello World";
    // 发布话题消息
    RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg.data.c_str());
    // 输出日志信息，提示已经完成话题发布
    publisher_->publish(msg);
  }

  rclcpp::TimerBase::SharedPtr timer_;                            // 定时器指针
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_; // 发布者指针
};

// ROS2节点主入口main函数
int main(int argc, char *argv[]) {
  // ROS2 C++接口初始化
  rclcpp::init(argc, argv);

  // 创建ROS2节点对象并进行初始化
  rclcpp::spin(std::make_shared<PublisherNode>());

  // 关闭ROS2 C++接口
  rclcpp::shutdown();

  return 0;
}

订阅者

Python 版本

python
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2话题示例-订阅“Hello World”话题消息
"""

import rclpy                                     # ROS2 Python接口库
from rclpy.node   import Node                    # ROS2 节点类
from std_msgs.msg import String                  # ROS2标准定义的String消息

"""
创建一个订阅者节点
"""
class SubscriberNode(Node):
    
    def __init__(self, name):
        super().__init__(name)                                    # ROS2节点父类初始化
        self.sub = self.create_subscription(\
            String, "chatter", self.listener_callback, 10)        # 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）

    def listener_callback(self, msg):                             # 创建回调函数，执行收到话题消息后对数据的处理
        self.get_logger().info('I heard: "%s"' % msg.data)        # 输出日志信息，提示订阅收到的话题消息
        
def main(args=None):                                 # ROS2节点主入口main函数
    rclpy.init(args=args)                            # ROS2 Python接口初始化
    node = SubscriberNode("topic_helloworld_sub")    # 创建ROS2节点对象并进行初始化
    rclpy.spin(node)                                 # 循环等待ROS2退出
    node.destroy_node()                              # 销毁节点对象
    rclpy.shutdown()                                 # 关闭ROS2 Python接口

C++ 版本

cpp
/***
@作者: 古月居(www.guyuehome.com)
@说明: ROS2话题示例-订阅“Hello World”话题消息
***/

#include <memory>

#include "rclcpp/rclcpp.hpp"       // ROS2 C++接口库
#include "std_msgs/msg/string.hpp" // 字符串消息类型
using std::placeholders::_1;

class SubscriberNode : public rclcpp::Node {
public:
  SubscriberNode()
      : Node("topic_helloworld_sub") { // ROS2节点父类初始化
    subscription_ = this->create_subscription<std_msgs::msg::String>(
        "chatter", 10, std::bind(&SubscriberNode::topic_callback, this, _1)); // 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）
  }

private:
  void topic_callback(const std_msgs::msg::String::SharedPtr msg) const { // 创建回调函数，执行收到话题消息后对数据的处理
    RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());  // 输出日志信息，提示订阅收到的话题消息
  }

  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_; // 订阅者指针
};

// ROS2节点主入口main函数
int main(int argc, char *argv[]) {
  // ROS2 C++接口初始化
  rclcpp::init(argc, argv);

  // 创建ROS2节点对象并进行初始化
  rclcpp::spin(std::make_shared<SubscriberNode>());

  // 关闭ROS2 C++接口
  rclcpp::shutdown();

  return 0;
}

服务(Service)：节点间的你问我答

话题通信可以实现多个ROS节点之间数据的单向传输，使用这种异步通信机制，发布者无法准确知道订阅者是否收到消息，本讲我们将一起学习ROS另外一种常用的通信方法——服务，可以实现类似你问我答的同步通信效果。

客户端/服务器模型

代码

服务器

Python 版本

python
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2服务示例-提供加法器的服务器处理功能
"""

import rclpy                                     # ROS2 Python接口库
from rclpy.node   import Node                    # ROS2 节点类
from learning_interface.srv import AddTwoInts    # 自定义的服务接口

class adderServer(Node):
    def __init__(self, name):
        super().__init__(name)                                                             # ROS2节点父类初始化
        self.srv = self.create_service(AddTwoInts, 'add_two_ints', self.adder_callback)    # 创建服务器对象（接口类型、服务名、服务器回调函数）

    def adder_callback(self, request, response):                                           # 创建回调函数，执行收到请求后对数据的处理
        response.sum = request.a + request.b                                               # 完成加法求和计算，将结果放到反馈的数据中
        self.get_logger().info('Incoming request\na: %d b: %d' % (request.a, request.b))   # 输出日志信息，提示已经完成加法求和计算
        return response                                                                    # 反馈应答信息

def main(args=None):                                 # ROS2节点主入口main函数
    rclpy.init(args=args)                            # ROS2 Python接口初始化
    node = adderServer("service_adder_server")       # 创建ROS2节点对象并进行初始化
    rclpy.spin(node)                                 # 循环等待ROS2退出
    node.destroy_node()                              # 销毁节点对象
    rclpy.shutdown()                                 # 关闭ROS2 Python接口

C++ 版本

cpp
/***
@作者: 古月居(www.guyuehome.com)
@说明: ROS2服务示例-提供加法器的服务器处理功能
***/

#include "learning_interface/srv/add_two_ints.hpp" // 自定义的服务接口
#include "rclcpp/rclcpp.hpp"                       // ROS2 C++接口库

#include <memory>

// 创建回调函数，执行收到请求后对数据的处理
void adderServer(const std::shared_ptr<learning_interface::srv::AddTwoInts::Request> request,
                 std::shared_ptr<learning_interface::srv::AddTwoInts::Response> response) {
  // 完成加法求和计算，将结果放到反馈的数据中
  response->sum = request->a + request->b;
  // 输出日志信息，提示已经完成加法求和计算
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld b: %ld", request->a, request->b);
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
}

// ROS2节点主入口main函数
int main(int argc, char **argv) {
  // ROS2 C++接口初始化
  rclcpp::init(argc, argv);
  // 创建ROS2节点对象并进行初始化
  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("service_adder_server");
  // 创建服务器对象（接口类型、服务名、服务器回调函数）
  rclcpp::Service<learning_interface::srv::AddTwoInts>::SharedPtr service =
      node->create_service<learning_interface::srv::AddTwoInts>("add_two_ints", &adderServer);

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");

  // 循环等待ROS2退出
  rclcpp::spin(node);
  // 关闭ROS2 C++接口
  rclcpp::shutdown();
}

客户端

Python 版本

python
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2服务示例-发送两个加数，请求加法器计算
"""

import sys

import rclpy                                                                      # ROS2 Python接口库
from rclpy.node   import Node                                                     # ROS2 节点类
from learning_interface.srv import AddTwoInts                                     # 自定义的服务接口

class adderClient(Node):
    def __init__(self, name):
        super().__init__(name)                                                    # ROS2节点父类初始化
        self.client = self.create_client(AddTwoInts, 'add_two_ints')              # 创建服务客户端对象（服务接口类型，服务名）
        while not self.client.wait_for_service(timeout_sec=1.0):                  # 循环等待服务器端成功启动
            self.get_logger().info('service not available, waiting again...') 
        self.request = AddTwoInts.Request()                                       # 创建服务请求的数据对象
                    
    def send_request(self):                                                       # 创建一个发送服务请求的函数
        self.request.a = int(sys.argv[1])
        self.request.b = int(sys.argv[2])
        self.future = self.client.call_async(self.request)                        # 异步方式发送服务请求

def main(args=None):
    rclpy.init(args=args)                                                         # ROS2 Python接口初始化
    node = adderClient("service_adder_client")                                    # 创建ROS2节点对象并进行初始化
    node.send_request()                                                           # 发送服务请求
    
    while rclpy.ok():                                                             # ROS2系统正常运行
        rclpy.spin_once(node)                                                     # 循环执行一次节点

        if node.future.done():                                                    # 数据是否处理完成
            try:
                response = node.future.result()                                   # 接收服务器端的反馈数据
            except Exception as e:
                node.get_logger().info(
                    'Service call failed %r' % (e,))
            else:
                node.get_logger().info(                                           # 将收到的反馈信息打印输出
                    'Result of add_two_ints: for %d + %d = %d' % 
                    (node.request.a, node.request.b, response.sum))
            break
            
    node.destroy_node()                                                           # 销毁节点对象
    rclpy.shutdown()                                                              # 关闭ROS2 Python接口

C++ 版本

cpp
/***
@作者: 古月居(www.guyuehome.com)
@说明: ROS2服务示例-发送两个加数，请求加法器计算
***/

#include "learning_interface/srv/add_two_ints.hpp" // 自定义的服务接口
#include "rclcpp/rclcpp.hpp"                       // ROS2 C++接口库

#include <chrono>
#include <cstdlib>
#include <memory>

using namespace std::chrono_literals;

int main(int argc, char **argv) {
  // ROS2 C++接口初始化
  rclcpp::init(argc, argv);

  if (argc != 3) {
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: service_adder_client X Y");
    return 1;
  }

  // 创建ROS2节点对象并进行初始化
  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("service_adder_client");
  // 创建服务客户端对象（服务接口类型，服务名）
  rclcpp::Client<learning_interface::srv::AddTwoInts>::SharedPtr client =
      node->create_client<learning_interface::srv::AddTwoInts>("add_two_ints");

  // 创建服务接口数据
  auto request = std::make_shared<learning_interface::srv::AddTwoInts::Request>();
  request->a = atoll(argv[1]);
  request->b = atoll(argv[2]);

  // 循环等待服务器端成功启动
  while (!client->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
      return 0;
    }
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
  }

  // 异步方式发送服务请求
  auto result = client->async_send_request(request);

  // 接收服务器端的反馈数据
  if (rclcpp::spin_until_future_complete(node, result) ==
      rclcpp::FutureReturnCode::SUCCESS) {
    // 将收到的反馈信息打印输出
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum);
  } else {
    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints");
  }

  // 关闭ROS2 C++接口
  rclcpp::shutdown();
  return 0;
}

通信接口(Interface)：数据传递的标准结构

在ROS系统中，无论话题还是服务，或者我们后续将要学习的动作，都会用到一个重要的概念——通信接口。通信并不是一个人自言自语，而是两个甚至更多个人，你来我往的交流，交流的内容是什么呢？为了让大家都好理解，我们可以给传递的数据定义一个标准的结构，这就是通信接口。

参数(Parameter)：机器人系统的全局字典

话题、服务、动作，不知道这三种通信机制大家是否已经了解清楚，本节我们再来介绍一种ROS系统中常用的数据传输方式——参数。类似C++编程中的全局变量，可以便于在多个程序中共享某些数据，参数是ROS机器人系统中的全局字典，可以运行多个节点中共享数据。

动作(Action)：完整行为的流程管理

机器人是一个复杂的智能系统，并不仅仅是键盘遥控运动、识别某个目标这么简单，我们需要实现的是送餐、送货、分拣等满足具体场景需求的机器人。在这些应用功能的实现中，另外一种ROS通信机制也会被常常用到——那就是动作。从这个名字上就可以很好理解这个概念的含义，这种通信机制的目的就是便于对机器人某一完整行为的流程进行管理。

客户端/服务器模型

分布式通信(DistributedCommunication)：多计算平台的任务分配

智能机器人的功能繁多，全都放在一个计算机里，经常会遇到计算能力不够、处理出现卡顿等情况，如果可以将这些任务拆解，分配到多个计算机中运行岂不是可以减轻压力？这就是分布式系统，可以实现多计算平台上的任务分配。

DDS(Data Distribution Service)：机器人的神经网络

ROS2中最为重大的变化——DDS，话题、服务、动作，他们底层通信的具体实现过程，都是靠DDS（DDS的全称是Data Distribution Service，也就是数据分发服务）来完成的，它相当于是ROS机器人系统中的神经网络。

ROS2 常用工具

Launch：多节点启动与配置脚本

每当我们运行一个ROS节点，都需要打开一个新的终端运行一个命令。机器人系统中节点很多，每次都这样启动好麻烦呀。有没有一种方式可以一次性启动所有节点呢？答案当然是肯定的，那就是Launch启动文件，它是ROS系统中多节点启动与配置的一种脚本。

TF：机器人坐标系管理神器

坐标系是我们非常熟悉的一个概念，也是机器人学中的重要基础，在一个完整的机器人系统中，会存在很多坐标系，这些坐标系之间的位置关系该如何管理？ ROS给我们提供了一个坐标系的管理神器——TF。

遇到的错误

1. 执行ROS2脚本报错：ImportError: librclpy_common.so: cannot open shared object file: No such file or directory


Traceback (most recent call last):
  File "/opt/ros/foxy/lib/teleop_twist_keyboard/teleop_twist_keyboard", line 11, in <module>
    load_entry_point('teleop-twist-keyboard==2.3.2', 'console_scripts', 'teleop_twist_keyboard')()
  File "/opt/ros/foxy/lib/python3.8/site-packages/teleop_twist_keyboard.py", line 135, in main
    rclpy.init()
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/__init__.py", line 70, in init
    context = get_default_context() if context is None else context
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/utilities.py", line 34, in get_default_context
    g_default_context = Context()
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/context.py", line 37, in __init__
    from rclpy.impl.implementation_singleton import rclpy_implementation
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/impl/implementation_singleton.py", line 31, in <module>
    rclpy_implementation = _import('._rclpy')
  File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/impl/__init__.py", line 28, in _import
    return importlib.import_module(name, package='rclpy')
  File "/usr/lib/python3.8/importlib/__init__.py", line 127, in import_module
    return _bootstrap._gcd_import(name[level:], package, level)
ImportError: librclpy_common.so: cannot open shared object file: No such file or directory
The C extension '/opt/ros/foxy/lib/python3.8/site-packages/rclpy/_rclpy.cpython-38-aarch64-linux-gnu.so' failed to be imported while being present on the system. Please refer to 'https://index.ros.org/doc/ros2/Troubleshooting/Installation-Troubleshooting/#import-failing-even-with-library-present-on-the-system' for possible solutions

解决方案

bash
# 检查安装状态
sudo dpkg -L ros-foxy-rclpy

# 当使用 LD_LIBRARY_PATH 时，LD_LIBRARY_PATH 会被 setcap 忽略。因此 python3.8 将无法找到 ros 库，只能找到python包。解决方法：
sudo setcap -r /usr/bin/python3.8

在 Rviz2 中不显示机器人模型并且报错

shell
[ERROR] [1710734213.775509869] [rviz2]: Error retrieving file [package://originbot_description/meshes/base_link.STL]: Package [originbot_description] does not exist
[ERROR] [1710734213.775762403] [rviz2]: Error retrieving file [package://originbot_description/meshes/base_link.STL]: Package [originbot_description] does not exist

解决方案

bash
# cd 对应机器人的空间
source install/local_setup.bash
# 重新运行 rviz2
rviz2