ROS2的消息发布和订阅&图像发布和订阅

ROS2 的编译

colcon 是 ROS2 的编译工具。ROS2 的工作空间与 ROS1 基本保持相同的目录结构：

<workspace>
├── build        # 编译时自动生成，包含编译中间文件
├── install      # 编译时自动生成，包含编译结果：可执行文件、库文件、message 头文件等
├── log          # 编译日志，方便在编译失败时查找问题（ROS2 新增）
└── src
  ├── ros_pkg1  # 功能包 1
  ├── ros_pkg2
  ├── ros_pkg3
    ......

需要注意的是，ROS2 在执行 colcon 命令编译时，需要位于 workspace 目录下，因为编译时自动生成的 build、install、log 等文件夹会与 colcon 所在目录保持同级。

colcon 编译

编译整个工作空间。默认情况下，编译会优先使用 ninja 作为构建方式，并使用 8 线程进行代码编译。编译结果会在 install 文件夹中按 package 为单位存放。

colcon build

编译工作空间，并建立软连接。这样在 launch 文件修改后通常不必重新编译。如果 install 中的文件需要拷贝到另外一台机器使用，就不要使用这个参数，否则可能会找不到源文件（Python 场景尤其要注意）。

colcon build --symlink-install

编译工作空间指定 pkg

colcon build --symlink-install --packages-select packages1

编译工作空间多个 pkg

colcon build --symlink-install --packages-select packages1 packages2 packages3

忽略某个包。编译除指定忽略 pkg 之外的其他所有 pkg。同样也可以在 package 目录下创建一个 COLCON_IGNORE 空文件，以忽略该包的编译。

colcon build --packages-ignore packages2

清除已有的编译缓存。如果不加 --cmake-clean-cache 参数，系统在发现 build、install、log 这三个文件夹中已经存在相关包的信息后，通常会跳过该包的重新编译。

colcon build --cmake-clean-cache

合并安装。这个功能会将编译结果合并安装。比如头文件会统一放在 install/include 目录下，库文件会统一放在 install/lib 文件夹下。

colcon build --merge-install

rosdep

rosdep 是一种依赖管理工具，可以与功能包和外部库一起工作。

它本身不是一个独立的包管理器，而是一个元包管理器。它会根据系统平台和依赖信息，查找最适合当前平台安装的包，真正的安装仍然由系统包管理器完成。

rosdep 的作用是在编译和部署时解决软件包的编译依赖与运行依赖问题。最常见的场景，就是解决 xxxConfig.cmake not found 这一类问题。

通常在构建工作空间之前调用 rosdep，用于安装工作空间内包的依赖项。

rosdep init 初始化远程服务器地址。
rosdep update 将 ROS package 的依赖关系缓存在本地。
rosdep install --from-paths src --ignore-src src -y 查询每个 ROS package 下 package.xml 文件中的内容，确定需要下载的依赖包并进行安装。

工作空间中的 package.xml 是 rosdep 查找依赖集合的重要文件，因此其中依赖项列表是否完整且正确非常关键，可以理解为 ROS 生态的 requirements.txt。

<depend> 表示构建时和运行时和导出时都需要的依赖。对于 C++ 项目，如果不确定，通常可以优先使用这个标签。纯 Python 项目没有 build 阶段，通常改用 <exec_depend>。
<build_depend> 仅在构建时使用特定的依赖。
<exec_depend> 仅在运行时需要的依赖。

bloom

bloom 是一个打包工具，可以将代码制作成 deb 或 rpm 包，然后安装到目标机器上进行测试和部署。

安装：

sudo apt-get install python3-bloom fakeroot

工作方式：

读取 ROS package 中 CMakeLists.txt 的 install 规则，确定安装位置。
读取 ROS package 中 package.xml 文件，确定版本号及依赖项。
基于这些信息完成软件打包。

使用参考：

cd <ros package 的目录下>
bloom-generate rosdebian --os-name <系统名字> --ros-distro <ros 版本名字>
fakeroot debian/rules binary

ROS2 节点

下面是 ROS Graph。ROS 的 Node 可以向任意数量的 Topic 发布数据，同时也可以订阅任意数量的 Topic。

创建工作空间

如果是创建 C++ 工作空间：

ros2 pkg create --build-type ament_cmake msg_demo --dependencies rclcpp example_interfaces

如果是创建 Python 工作空间：

ros2 pkg create --build-type ament_python msg_demo --dependencies rclcpp example_interfaces

其中 --dependencies 可以在创建时指定，也可以后续手动在 package.xml 中添加。

当然，也可以直接下载 ROS 官方 Demo：

git clone https://github.com/ros2/examples -b foxy

进入 examples/rclcpp/topics 路径后，运行colcon build。

然后对构建出来的软件包运行测试。此时不需要额外引用工作空间，colcon test 会确保测试在正确的环境中运行，并且能够访问它们的依赖项：

colcon test

引用工作空间：

source install/setup.bash
或者
. install/setup.bash

分别在两个终端中运行：

ros2 run examples_rclcpp_minimal_subscriber subscriber_member_function
ros2 run examples_rclcpp_minimal_publisher publisher_member_function

图像的发布和订阅

Publisher

这是一个简单的静态图像发布节点（由 ChatGPT 生成）。

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>

class ImagePublisher : public rclcpp::Node
{
public:
    ImagePublisher() : Node("image_publisher"), count_(0)
    {
        publisher_ = this->create_publisher<sensor_msgs::msg::Image>("image", 10);
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&ImagePublisher::timer_callback, this));
    }

private:
    void timer_callback()
    {
        auto message = sensor_msgs::msg::Image();
        cv::Mat image = cv::imread("/home/jetson/Documents/CV/ros2_demo/image_demo/hand-landmarks.png");
        cv::putText(image, "Frame: " + std::to_string(count_),
                    cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2);
        cv_bridge::CvImage cv_image;
        cv_image.header.stamp = this->get_clock()->now();
        cv_image.header.frame_id = "camera";
        cv_image.encoding = "bgr8";
        cv_image.image = image;
        cv_image.toImageMsg(message);
        publisher_->publish(message);
        count_++;
    }
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
    size_t count_;
};

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

Subscriber

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"
#include <opencv2/opencv.hpp>

class ImageSubscriberNode : public rclcpp::Node
{
public:
    ImageSubscriberNode() : Node("image_subscriber")
    {
        subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
            "image", 10,
            std::bind(&ImageSubscriberNode::image_callback, this, std::placeholders::_1));
    }

private:
    void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
    {
        cv_bridge::CvImagePtr cv_ptr;
        try
        {
            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        }
        catch (const cv_bridge::Exception& e)
        {
            RCLCPP_ERROR(this->get_logger(), "Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
            return;
        }
        cv::imshow("Received Image", cv_ptr->image);
        cv::waitKey(10);
    }

    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
};

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

CMakeLists.txt

下面是图像发布和订阅对应的 CMakeLists.txt：

cmake_minimum_required(VERSION 3.5)
project(image_demo)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

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

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(image_transport REQUIRED)
find_package(std_msgs REQUIRED)  # 确保 std_msgs 被找到
find_package(cv_bridge REQUIRED)  # 确保 cv_bridge 被找到

set(OpenCV_DIR "/home/nvidia/opencv-4.5.4/build")
find_package(OpenCV)
message(STATUS "OpenCV library status:")
message(STATUS "    config: ${OpenCV_DIR}")
message(STATUS "    version: ${OpenCV_VERSION}")
message(STATUS "    libraries: ${OpenCV_LIBS}")
message(STATUS "    include path: ${OpenCV_INCLUDE_DIRS}")
include_directories(${OpenCV_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS})

add_executable(image_publisher src/image_publisher.cpp)
add_executable(image_subscriber src/image_subscriber.cpp)

ament_target_dependencies(image_publisher
  rclcpp
  sensor_msgs
  image_transport
  cv_bridge
  OpenCV
)
ament_target_dependencies(image_subscriber
  rclcpp
  sensor_msgs
  image_transport
  cv_bridge
  OpenCV
)

install(TARGETS
  image_publisher
  image_subscriber
  DESTINATION lib/${PROJECT_NAME})

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

ament_package()

ROS2 的 ament_cmake 是基于 CMake 改进而来的。下面把这个 CMakeLists 拆开看一下。

第一行指定 CMake 的最低版本，第二行是功能包名称。这里的名称应当与 package.xml 中保持一致。

cmake_minimum_required(VERSION 3.5)
project(image_demo)

查找依赖项时，如果依赖项是非 ROS2 功能包，通常需要把头文件路径写到 include_directories 中；而如果依赖项本身是 ROS2 功能包，则一般不需要额外这样做。

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(image_transport REQUIRED)
find_package(std_msgs REQUIRED)  # 确保 std_msgs 被找到
find_package(cv_bridge REQUIRED)  # 确保 cv_bridge 被找到

set(OpenCV_DIR "/home/nvidia/opencv-4.5.4/build")
find_package(OpenCV)
message(STATUS "OpenCV library status:")
message(STATUS "    config: ${OpenCV_DIR}")
message(STATUS "    version: ${OpenCV_VERSION}")
message(STATUS "    libraries: ${OpenCV_LIBS}")
message(STATUS "    include path: ${OpenCV_INCLUDE_DIRS}")
include_directories(${OpenCV_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS})

add_executable 用于构建可执行文件。

add_executable(image_publisher src/image_publisher.cpp)
add_executable(image_subscriber src/image_subscriber.cpp)

ament_target_dependencies 是官方推荐的依赖添加方式。它会让依赖项的库、头文件以及依赖项自身的依赖被正确找到。

如果依赖项是 ROS2 功能包，通常优先使用 ament_target_dependencies。如果一个功能包包含多个库，也会一起被正确纳入。

此外，ament_target_dependencies 只能链接 find_package() 找到的包；如果是自定义库，仍然需要使用 target_link_libraries 的方式进行链接。

ament_target_dependencies(image_publisher
  rclcpp
  sensor_msgs
  image_transport
  cv_bridge
  OpenCV
)
ament_target_dependencies(image_subscriber
  rclcpp
  sensor_msgs
  image_transport
  cv_bridge
  OpenCV
)

install 这里安装的是可执行文件，安装路径是 image_demo/install/image_demo/lib/image_demo。

install(TARGETS
  image_publisher
  image_subscriber
  DESTINATION lib/${PROJECT_NAME})

下面这部分是编译单元测试文件时使用的配置：

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

项目安装通过 ament_package() 完成，并且每个软件包必须只调用一次。

ament_package() 会安装 package.xml 文件，在 ament 索引中注册该软件包，并安装 CMake 配置文件，以便其他软件包后续通过 find_package() 找到它。

ament_package() 会从 CMakeLists.txt 中收集大量信息，通常应该放在 CMakeLists.txt 的最后。

ament_package()

参考文章

[1] ROS2 编译入门_ros2编译-CSDN博客

[2] ROS2入门教程-colcon build使用 - 爱折腾-创客智造实验室

[3] fishros ros2 tutorials