Ubuntu 22.04/ROS2 Humble下使用Intel RealSense D435i相机

1. 安装RealSense SDK和ROS2驱动

1.1 安装依赖

bash 复制代码

sudo apt update
sudo apt install -y git cmake libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev
sudo apt install -y libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev
sudo apt install -y python3-dev python3-pip

1.2 安装Intel RealSense SDK

打开终端，创建工作空间和src/目录：

bash 复制代码

mkdir -p ~/realsense_ws
cd ~/realsense_ws

克隆librealsense仓库

bash 复制代码

git clone https://github.com/IntelRealSense/librealsense.git
cd librealsense

安装依赖

bash 复制代码

./scripts/setup_udev_rules.sh
./scripts/patch-realsense-ubuntu-lts.sh

编译安装

bash 复制代码

mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_EXAMPLES=true
make -j$(nproc)
sudo make install

1.3 安装ROS2 RealSense包

打开终端，创建工作空间

bash 复制代码

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

克隆realsense-ros2仓库

bash 复制代码

git clone https://github.com/IntelRealSense/realsense-ros.git -b humble

返回工作空间根目录并安装依赖

bash 复制代码

cd ~/ros2_ws
rosdep install -i --from-path src --rosdistro humble -y

编译

bash 复制代码

colcon build
source install/setup.bash

2. 创建自定义ROS2包

2.1 创建包

进入工作空间，创建自定义ROS2包：

bash 复制代码

cd ~/ros2_ws/src
ros2 pkg create d435i_camera --build-type ament_cmake --dependencies rclcpp sensor_msgs cv_bridge image_transport geometry_msgs tf2 tf2_ros

2.2 项目结构

复制代码

d435i_camera/
├── CMakeLists.txt
├── package.xml
├── include/
├── src/
│   ├── d435i_publisher.cpp
│   └── d435i_subscriber.cpp
└── launch/
    ├── d435i_launch.py
    └── rviz_config.rviz

3. 配置文件

3.1 package.xml

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>d435i_camera</name>
  <version>0.1.0</version>
  <description>D435i Camera ROS2 Package</description>
  <maintainer email="your-email@example.com">Your Name</maintainer>
  <license>Apache-2.0</license>

  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>
  <depend>cv_bridge</depend>
  <depend>image_transport</depend>
  <depend>geometry_msgs</depend>
  <depend>tf2</depend>
  <depend>tf2_ros</depend>
  <depend>realsense2_camera</depend>
  <depend>vision_msgs</depend>
  <depend>std_msgs</depend>

  <exec_depend>rviz2</exec_depend>
  <exec_depend>ros2launch</exec_depend>

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

3.2 CMakeLists.txt

cmake 复制代码

cmake_minimum_required(VERSION 3.8)
project(d435i_camera)

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

# 查找依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(image_transport REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(vision_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(OpenCV REQUIRED)

# 添加可执行文件
add_executable(d435i_publisher src/d435i_publisher.cpp)
ament_target_dependencies(d435i_publisher
  rclcpp
  sensor_msgs
  cv_bridge
  image_transport
  geometry_msgs
  tf2
  tf2_ros
  vision_msgs
  std_msgs
)

add_executable(d435i_subscriber src/d435i_subscriber.cpp)
ament_target_dependencies(d435i_subscriber
  rclcpp
  sensor_msgs
  cv_bridge
  std_msgs
)

# 安装目标
install(TARGETS
  d435i_publisher
  d435i_subscriber
  DESTINATION lib/${PROJECT_NAME}
)

# 安装启动文件
install(DIRECTORY
  launch
  DESTINATION share/${PROJECT_NAME}
)

# 安装rviz配置
install(FILES
  launch/rviz_config.rviz
  DESTINATION share/${PROJECT_NAME}/launch
)

ament_package()

4. 源代码文件

4.1 src/d435i_publisher.cpp

cpp 复制代码

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.hpp>
#include <opencv2/opencv.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <vision_msgs/msg/detection2_d_array.hpp>

class D435iPublisher : public rclcpp::Node
{
public:
    D435iPublisher() : Node("d435i_publisher")
    {
        // 创建发布者
        color_pub_ = image_transport::create_publisher(this, "camera/color/image_raw");
        depth_pub_ = image_transport::create_publisher(this, "camera/depth/image_raw");
        imu_pub_ = create_publisher<sensor_msgs::msg::Imu>("camera/imu", 10);
        
        // 创建相机信息发布者
        color_info_pub_ = create_publisher<sensor_msgs::msg::CameraInfo>("camera/color/camera_info", 10);
        depth_info_pub_ = create_publisher<sensor_msgs::msg::CameraInfo>("camera/depth/camera_info", 10);
        
        // 创建TF广播器
        tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
        
        // 创建定时器（模拟相机数据）
        timer_ = create_wall_timer(
            std::chrono::milliseconds(33),  // 30Hz
            std::bind(&D435iPublisher::timer_callback, this));
        
        // 创建检测结果发布者
        detection_pub_ = create_publisher<vision_msgs::msg::Detection2DArray>("detections", 10);
        
        RCLCPP_INFO(this->get_logger(), "D435i Publisher started");
    }

private:
    void timer_callback()
    {
        // 模拟发布相机数据
        publish_color_image();
        publish_depth_image();
        publish_imu_data();
        publish_camera_info();
        publish_tf();
        publish_detections();
    }

    void publish_color_image()
    {
        // 创建模拟彩色图像（640x480）
        cv::Mat color_image(480, 640, CV_8UC3, cv::Scalar(100, 100, 255));
        
        // 添加一些视觉特征
        cv::circle(color_image, cv::Point(320, 240), 50, cv::Scalar(0, 255, 0), 3);
        cv::rectangle(color_image, cv::Rect(100, 100, 200, 200), cv::Scalar(255, 0, 0), 2);
        
        // 转换为ROS消息
        auto msg = cv_bridge::CvImage(
            std_msgs::msg::Header(), "bgr8", color_image).toImageMsg();
        msg->header.stamp = now();
        msg->header.frame_id = "camera_color_optical_frame";
        
        color_pub_.publish(msg);
    }

    void publish_depth_image()
    {
        // 创建模拟深度图像（640x480）
        cv::Mat depth_image(480, 640, CV_16UC1);
        
        // 模拟深度数据
        for (int i = 0; i < depth_image.rows; i++) {
            for (int j = 0; j < depth_image.cols; j++) {
                // 模拟距离（毫米）
                int distance = 1000 + (i * 640 + j) % 500;
                depth_image.at<uint16_t>(i, j) = distance;
            }
        }
        
        // 转换为ROS消息
        auto msg = cv_bridge::CvImage(
            std_msgs::msg::Header(), "16UC1", depth_image).toImageMsg();
        msg->header.stamp = now();
        msg->header.frame_id = "camera_depth_optical_frame";
        
        depth_pub_.publish(msg);
    }

    void publish_imu_data()
    {
        auto imu_msg = std::make_unique<sensor_msgs::msg::Imu>();
        imu_msg->header.stamp = now();
        imu_msg->header.frame_id = "camera_imu_optical_frame";
        
        // 模拟IMU数据
        imu_msg->linear_acceleration.x = 0.0;
        imu_msg->linear_acceleration.y = 0.0;
        imu_msg->linear_acceleration.z = 9.81;
        
        imu_msg->angular_velocity.x = 0.01;
        imu_msg->angular_velocity.y = 0.02;
        imu_msg->angular_velocity.z = 0.03;
        
        imu_pub_->publish(std::move(imu_msg));
    }

    void publish_camera_info()
    {
        // 彩色相机参数
        auto color_info = std::make_unique<sensor_msgs::msg::CameraInfo>();
        color_info->header.stamp = now();
        color_info->header.frame_id = "camera_color_optical_frame";
        color_info->height = 480;
        color_info->width = 640;
        color_info->distortion_model = "plumb_bob";
        color_info->d = {0.0, 0.0, 0.0, 0.0, 0.0};
        color_info->k = {615.0, 0.0, 320.0, 0.0, 615.0, 240.0, 0.0, 0.0, 1.0};
        color_info->r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
        color_info->p = {615.0, 0.0, 320.0, 0.0, 0.0, 615.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0};
        
        // 深度相机参数
        auto depth_info = std::make_unique<sensor_msgs::msg::CameraInfo>();
        *depth_info = *color_info;
        depth_info->header.frame_id = "camera_depth_optical_frame";
        
        color_info_pub_->publish(std::move(color_info));
        depth_info_pub_->publish(std::move(depth_info));
    }

    void publish_tf()
    {
        geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp = now();
        transform.header.frame_id = "camera_link";
        transform.child_frame_id = "camera_color_optical_frame";
        
        transform.transform.translation.x = 0.0;
        transform.transform.translation.y = 0.0;
        transform.transform.translation.z = 0.0;
        
        transform.transform.rotation.x = 0.0;
        transform.transform.rotation.y = 0.0;
        transform.transform.rotation.z = 0.0;
        transform.transform.rotation.w = 1.0;
        
        tf_broadcaster_->sendTransform(transform);
    }

    void publish_detections()
    {
        auto detection_msg = std::make_unique<vision_msgs::msg::Detection2DArray>();
        detection_msg->header.stamp = now();
        detection_msg->header.frame_id = "camera_color_optical_frame";
        
        // 添加模拟检测结果
        vision_msgs::msg::Detection2D detection;
        detection.bbox.center.position.x = 320.0;
        detection.bbox.center.position.y = 240.0;
        detection.bbox.size_x = 100.0;
        detection.bbox.size_y = 100.0;
        
        detection_msg->detections.push_back(detection);
        
        detection_pub_->publish(std::move(detection_msg));
    }

    // 发布者
    image_transport::Publisher color_pub_;
    image_transport::Publisher depth_pub_;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
    rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr color_info_pub_;
    rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr depth_info_pub_;
    rclcpp::Publisher<vision_msgs::msg::Detection2DArray>::SharedPtr detection_pub_;
    
    // TF广播器
    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    
    // 定时器
    rclcpp::TimerBase::SharedPtr timer_;
};

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

4.2 src/d435i_subscriber.cpp

cpp 复制代码

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

class D435iSubscriber : public rclcpp::Node
{
public:
    D435iSubscriber() : Node("d435i_subscriber")
    {
        // 创建订阅者
        color_sub_ = create_subscription<sensor_msgs::msg::Image>(
            "camera/color/image_raw", 10,
            std::bind(&D435iSubscriber::color_callback, this, std::placeholders::_1));
        
        depth_sub_ = create_subscription<sensor_msgs::msg::Image>(
            "camera/depth/image_raw", 10,
            std::bind(&D435iSubscriber::depth_callback, this, std::placeholders::_1));
        
        imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
            "camera/imu", 10,
            std::bind(&D435iSubscriber::imu_callback, this, std::placeholders::_1));
        
        RCLCPP_INFO(this->get_logger(), "D435i Subscriber started");
    }

private:
    void color_callback(const sensor_msgs::msg::Image::SharedPtr msg)
    {
        try {
            // 转换ROS图像消息到OpenCV格式
            cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
            cv::Mat image = cv_ptr->image;
            
            // 显示图像信息
            RCLCPP_INFO_ONCE(this->get_logger(), 
                "Color Image: %dx%d, Encoding: %s", 
                image.cols, image.rows, msg->encoding.c_str());
            
            // 处理图像（示例：转换为灰度图）
            cv::Mat gray_image;
            cv::cvtColor(image, gray_image, cv::COLOR_BGR2GRAY);
            
            // 显示图像（可选）
            if (show_images_) {
                cv::imshow("Color Image", image);
                cv::imshow("Gray Image", gray_image);
                cv::waitKey(1);
            }
            
        } catch (cv_bridge::Exception& e) {
            RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
        }
    }

    void depth_callback(const sensor_msgs::msg::Image::SharedPtr msg)
    {
        try {
            cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg);
            cv::Mat depth_image = cv_ptr->image;
            
            // 计算深度统计信息
            double min_val, max_val;
            cv::minMaxLoc(depth_image, &min_val, &max_val);
            
            RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
                "Depth Image: Min=%.2fmm, Max=%.2fmm", min_val, max_val);
            
            // 可视化深度图（归一化到0-255）
            if (show_images_ && depth_image.type() == CV_16UC1) {
                cv::Mat depth_visual;
                depth_image.convertTo(depth_visual, CV_8U, 255.0 / max_val);
                cv::applyColorMap(depth_visual, depth_visual, cv::COLORMAP_JET);
                cv::imshow("Depth Image", depth_visual);
                cv::waitKey(1);
            }
            
        } catch (cv_bridge::Exception& e) {
            RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
        }
    }

    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        // 提取IMU数据
        double accel_x = msg->linear_acceleration.x;
        double accel_y = msg->linear_acceleration.y;
        double accel_z = msg->linear_acceleration.z;
        
        double gyro_x = msg->angular_velocity.x;
        double gyro_y = msg->angular_velocity.y;
        double gyro_z = msg->angular_velocity.z;
        
        // 打印IMU数据（限制频率）
        RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
            "IMU - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]",
            accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z);
    }

    // 订阅者
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr color_sub_;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr depth_sub_;
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
    
    bool show_images_ = true;  // 设置为false如果不希望显示图像
};

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

5. 启动文件

5.1 launch/d435i_launch.py

python 复制代码

#!/usr/bin/env python3

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 获取包目录
    pkg_dir = get_package_share_directory('d435i_camera')
    
    # 配置RealSense相机节点
    realsense_node = Node(
        package='realsense2_camera',
        executable='realsense2_camera_node',
        namespace='camera',
        name='realsense2_camera',
        parameters=[{
            'rgb_camera.profile': '640x480x30',
            'depth_module.profile': '640x480x30',
            'enable_color': True,
            'enable_depth': True,
            'enable_imu': True,
            'align_depth': True,
            'enable_pointcloud': True,
            'pointcloud_texture_stream': 'RS2_STREAM_COLOR',
            'initial_reset': True,
            'unite_imu_method': 'linear_interpolation',
        }],
        output='screen'
    )
    
    # 自定义发布者节点
    publisher_node = Node(
        package='d435i_camera',
        executable='d435i_publisher',
        name='d435i_publisher',
        output='screen'
    )
    
    # 自定义订阅者节点
    subscriber_node = Node(
        package='d435i_camera',
        executable='d435i_subscriber',
        name='d435i_subscriber',
        output='screen'
    )
    
    # RViz2节点
    rviz_config_file = os.path.join(pkg_dir, 'launch', 'rviz_config.rviz')
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config_file],
        output='screen'
    )
    
    # TF静态发布器
    static_tf_node = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=['0', '0', '0', '0', '0', '0', 'camera_link', 'camera_color_optical_frame']
    )
    
    return LaunchDescription([
        realsense_node,
        publisher_node,
        subscriber_node,
        static_tf_node,
        rviz_node
    ])

6. 编译和运行

6.1 编译包

在工作空间下，打开终端，输入：

bash 复制代码

cd ~/ros2_ws
colcon build --packages-select d435i_camera
source install/setup.bash

6.2 测试RealSense驱动

测试RealSense相机，打开终端，输入：

bash 复制代码

source ~/ros2_ws/install/setup.bash
ros2 launch realsense2_camera rs_launch.py

查看可用话题，打开终端，输入：

bash 复制代码

ros2 topic list

查看图像话题，打开终端，输入：

bash 复制代码

ros2 topic echo /camera/color/image_raw --once

6.3 运行自定义节点

方法1：使用启动文件

在工作空间下打开终端，输入：

bash 复制代码

source install/setup.bash
ros2 launch d435i_camera d435i_launch.py

方法2：单独运行节点

在工作空间下打开终端，输入：

bash 复制代码

source install/setup.bash
ros2 run d435i_camera d435i_subscriber

在工作空间下打开一个新终端，输入：

bash 复制代码

source install/setup.bash
ros2 run d435i_camera d435i_subscriber

查看发布的话题，打开终端，输入：

bash 复制代码

ros2 topic list
ros2 topic echo /camera/color/image_raw --once
ros2 topic echo /camera/imu --once

7. 常用命令和工具

7.1 查看相机信息

查看所有话题，打开终端，输入：

bash 复制代码

ros2 topic list

查看话题信息，打开终端，输入：

bash 复制代码

ros2 topic info /camera/color/image_raw
ros2 topic info /camera/depth/image_raw

查看IMU数据，打开终端，输入：

bash 复制代码

ros2 topic echo /camera/imu

查看TF树，打开终端，输入：

bash 复制代码

ros2 run tf2_tools view_frames.py

7.2 使用rqt工具

安装rqt，打开终端，输入：

bash 复制代码

sudo apt install ros-humble-rqt*

图像查看，打开终端，输入：

bash 复制代码

rqt_image_view

多工具界面，打开终端，输入：

bash 复制代码

rqt

8. 故障排除

8.1 常见问题解决

权限问题

bash 复制代码

sudo usermod -a -G dialout $USER
sudo usermod -a -G video $USER

UDEV规则问题

bash 复制代码

sudo cp ~/realsense_ws/librealsense/config/99-realsense-libusb.rules /etc/udev/rules.d/
sudo udevadm control --reload-rules && sudo udevadm trigger

重新插拔相机

bash 复制代码

sudo systemctl restart udev

检查相机连接

bash 复制代码

rs-enumerate-devices

测试深度流

bash 复制代码

realsense-viewer

8.2 验证安装

打开终端，检查ROS2环境，输入：

bash 复制代码

printenv | grep ROS

检查包路径：

bash 复制代码

echo $AMENT_PREFIX_PATH

测试消息发布：

bash 复制代码

ros2 topic pub /test std_msgs/msg/String "data: 'Hello ROS2'" -1

9. 高级功能配置

9.1 修改发布频率

在启动文件中添加参数，修改发布频率：

python 复制代码

parameters=[{
    'depth_fps': 30.0,
    'color_fps': 30.0,
    'gyro_fps': 200.0,
    'accel_fps': 63.0,
}]

9.2 启用点云

在启动文件中添加参数，启用点云：

python 复制代码

parameters=[{
    'enable_pointcloud': True,
    'pointcloud_texture_stream': 'RS2_STREAM_COLOR',
}]