使用 ROS2 进行图像传输

尽管在想要实现这个功能之前，我就了解到使用ROS2进行无线图像传输会消耗较大的资源，且很难达到实时显示的效果，但是仍然想尝试去测试一下，看看最终呈现的效果；下面是我在测试ROS2发布未压缩和压缩图像的记录。

发布未压缩图像

地平线官方提供了mipi_cam功能包，可发布sensor_msgs::msg::Image图像消息，通过以下命令启动：

ros2 launch originbot_bringup camera.launch.py

可以在PC端使用命令 ros2 run rqt_image_view rqt_image_view 打开 rqt_image_view 查看对应话题下的图像数据。最终的结果是图像的延时非常高。

通过 ros2 topic hz /image_raw 和 ros2 topic bw /image_raw 两条命令可以分别查看消息的发布频率和带宽使用情况，如下图：

可以看到 /image_raw 话题的消息发布速率为 1.8 Hz左右，即每秒发布约 1.8 条图像消息；平均每条图像消息的大小约为 1.5 MB左右。这个速率显然是远远不能达到实时的需求的。

OriginBot的官方网站上介绍了通过配置DDS为cyclonedds来优化图像传输。下图为小车端DDS的切换过程，PC端也一样。

DDS会默认绑定某一网卡进行数据传输，没有切换的状态下默认为eth0有线网卡，如下图：

将DDS绑定为 wlan0 ，命令如下：

export CYCLONEDDS_URI='<CycloneDDS><Domain><General><NetworkInterfaceAddress>wlan0</NetworkInterfaceAddress></General></Domain></CycloneDDS>'

再次查看速率，如图：

可以看到发布速率由 1.8 Hz左右提升到 3.4 Hz左右，但是这个提升离实时显示依然差的很远，随着小车的持续运行，速率还有可能降到二点几赫兹，x3主板的温度上升很快，侧面印证了ROS2的图像传输会消耗较大的资源。

我也使用有线网卡进行了测试，将DDS切换绑定到有线网卡eth0来进行传输。

  • 两端都使用cyclonedds，速率为21.4Hz左右

  • 两端都使用fastdds，速率为3Hz左右

  • 小车端使用fastdds，PC端使用cyclonedds，速率为29.6Hz左右。

  • 小车端使用cyclonedds，PC端使用fastdds，速率为22.7Hz左右。

小结

  • 使用ROS2的sensor_msgs::msg::Image图像消息进行图像传输会比较耗费资源，在进行图传输的时候X3主板会很快升到一个比较高的温度
  • DDS很大程度上会影响数据的传输速率，传输数据的两端只要有一端设置为cyclonedds就可以让传输速率增大
  • 使用有线网卡进行传输，在设置好DDS后可以达到实时传输的效果

发布压缩图像

下面来看看ROS2的压缩图像传输，从ROS1开始就支持压缩图像传输，配合专用与压缩图像的功能包 image_transport，在ROS2中也同样支持这个功能包，只不过使用起来繁杂一些（也有可能是该功能包还没有设计完整）。

通常 image_transport 相关的功能包在ROS2中需要单独安装，安装命令如下：

sudo apt install ros-<ros2-distro>-image-transport
sudo apt install ros-<ros2-distro>-image-transport-plugins

对于humble 版本，有可能所使用的源不包括 image_transport_plugins ，可以下载到自己的工作空间进行编译：

https://github.com/ros-perception/image_transport_plugins/tree/humble

我在小车端写了一个功能包，主要功能为使用OpenCV加载一个分辨率为1280x720的视频文件，然后使用定时器每30ms发布一次图像数据。当然这些图像消息包括sensor_msgs::msg::Image 、sensor_msgs::msg::CompressedImage以及通过image_transport_plugins插件发布的压缩图像消息。代码如下：

#include <cstdio>
#include <string>

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

using namespace std;
using namespace cv;
using namespace std::chrono_literals; 

class ImagePub : public rclcpp::Node
{

public:
  ImagePub() : Node("image_pub")
  {
    loadParam();
    initNode();
    videoCapture();
  }
  ~ImagePub(){};

  void loadParam()
  {
    this->declare_parameter<string>("image_pub_topic_name", "/image_raw");
    this->declare_parameter<string>("capture_format", "0");

    this->get_parameter("image_pub_topic_name", image_pub_topic_name);
    this->get_parameter("capture_format", capture_format);
  }

  void initNode()
  {
    image_pub_ = this->create_publisher<sensor_msgs::msg::Image>(
        image_pub_topic_name, 3);
     cp_image_pub_ = this->create_publisher<sensor_msgs::msg::CompressedImage>(
        "image_raw_compressed", 3);
    // cp_image_pub_ = this->create_publisher<sensor_msgs::msg::CompressedImage>(
    //    "image_raw_compressed", rclcpp::SensorDataQoS());

    image_transport_pub_ = image_transport::create_publisher(this, "image_raw_transport");

    timer_ = this->create_wall_timer(
        30ms, std::bind(&ImagePub::timerCallback, this));
  }

  void videoCapture()
  {
    if (isNum(capture_format))
    {
      int port = atoi(capture_format.c_str());
      capture = VideoCapture(port);
    }
    else
    {
      capture = VideoCapture(capture_format);
      frame_num = capture.get(cv::CAP_PROP_FRAME_COUNT);
      flag = 0;

      RCLCPP_INFO(this->get_logger(), "total frame number is: '%d'", frame_num);
    }
  }

  void timerCallback()
  {
    if (!isNum(capture_format) && flag == frame_num)
      videoCapture();

    Mat frame;
    capture.read(frame);
    if (!frame.empty())
    {
      sensor_msgs::msg::Image img_msg;
      // cv::resize(frame, frame, cv::Size(640, 480));
      cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame).toImageMsg(img_msg);
      img_msg.header.frame_id = "camera";
      img_msg.header.stamp = this->now();
      image_pub_->publish(img_msg);
      // img_msg.encoding = sensor_msgs::image_encodings::YUV422_YUY2;
      image_transport_pub_.publish(img_msg);

      sensor_msgs::msg::CompressedImage cp_img_msg;
      cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame).toCompressedImageMsg(cp_img_msg);
      cp_img_msg.header.frame_id = "camera";
      cp_img_msg.format = "jpeg"; // png
      cv_bridge::CvImagePtr cv_ptr_compressed =
            cv_bridge::toCvCopy(cp_img_msg, sensor_msgs::image_encodings::BGR8);
      // Mat clone_img = cv_ptr_compressed->image.clone();
      // cv::imshow("clone_img", clone_img);
      // cv::waitKey(1);
      cp_image_pub_->publish(cp_img_msg);
      
    }

    if (!isNum(capture_format))
      flag++;
  }

  bool isNum(string str)
  {
    stringstream sin(str);
    double d;
    char c;
    if (!(sin >> d))
    {
      return false;
    }
    if (sin >> c)
    {
      return false;
    }
    return true;
  }

private:
  string image_pub_topic_name;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
  rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr cp_image_pub_;
  
  image_transport::Publisher image_transport_pub_;
  
  rclcpp::TimerBase::SharedPtr timer_;
  VideoCapture capture;
  int frame_num;
  int flag;
  string capture_format;
};

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

以上程序在已经安装了 image_transport_plugins 插件的ROS2环境下直接运行，最终会发布以下话题：

  • /image_raw 为直接发布的 sensor_msgs::msg::Image 消息格式
  • /image_raw/uncompressed 为直接发布的 sensor_msgs::msg::CompressedImage 消息格式
  • /image_raw_transport 为通过 image_transport 发布的 sensor_msgs::msg::Image 消息格式，基本上和 /image_raw 一致
  • /image_raw_transport/compressed 为通过 image_transport_plugins 插件自动发布的压缩图像消息，通过 ros2 topic info 可以看到它发布的就是sensor_msgs::msg::CompressedImage 消息格式，所以这个话题几乎和 /image_raw/uncompressed 是一致的，只是有可能部分参数设置的不一样
  • /image_raw_transport/compressedDepth 和 /image_raw_transport/theora 也是通过image_transport_plugins 插件自动发布的图像消息，但是此处用不到，暂时不做说明。
  • /image_raw_transport/uncompressed 为 image_transport 解压缩/image_raw_transport/compressed 后的话题，通过以下命令得到：
ros2 run image_transport republish compressed raw --ros-args --remap in/compressed:=image_raw_transport/compressed --remap out:=image_raw_transport/uncompressed
  • /image_raw_uncompressed 同样为 image_transport 解压缩 /image_raw_tcompressed 后的话题，通过以下命令得到：
ros2 run image_transport republish compressed raw --ros-args --remap in/compressed:=image_raw_compressed --remap out:=image_raw_uncompressed

解压缩后的图像消息可以在 rviz2 上显示，如下图：

通过 image_transport_plugins 插件自动发布的压缩的图像可以在 rqt_image_view 上显示，如下图：

但是直接发布的 sensor_msgs::msg::CompressedImage 消息格式并不能通过 rqt_image_view 显示，暂时不知道具体的原因；想使用这个话题可以自己去解压缩，下面提供一个订阅此压缩图像的例程：

#include <cstdio>
#include <string>

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

using namespace std;
using namespace cv;
using namespace std::chrono_literals;
using std::placeholders::_1;

class ImageSub : public rclcpp::Node
{

public:
  ImageSub()
      : Node("image_sub")
  {
    initNode();
  }
  ~ImageSub(){};

  void initNode()
  {
    
    cp_image_sub_ = this->create_subscription<sensor_msgs::msg::CompressedImage>(
        "/image_raw_transport/compressed", 3, std::bind(&ImageSub::imageCallback, this, _1));
    // image_raw_compressed

    // cp_image_sub_ = this->create_subscription<sensor_msgs::msg::CompressedImage>(
    //     "image_raw_compressed", rclcpp::SensorDataQoS(),
    //     std::bind(&ImageSub::imageCallback, this, _1));
    
  }

  void imageCallback(const sensor_msgs::msg::CompressedImage &msg)
  {
    // 第一种方法
    // const cv::Mat_<uchar> in(1, msg.data.size(), const_cast<uchar *>(&msg.data[0]));
    // const cv::Mat uncompressed_img = cv::imdecode(in, cv::IMREAD_ANYCOLOR); // IMREAD_UNCHANGED

    // 第二种方法
    cv_bridge::CvImagePtr cv_ptr_compressed =
          cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    Mat uncompressed_img = cv_ptr_compressed->image.clone();
    
    cv::imshow("uncompressed_img", uncompressed_img);
    cv::waitKey(1);
  }

private:
  rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr cp_image_sub_;
  
};

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

以上例程同时适用于解压缩直接发布以及通过 image_transport_plugins 插件发布的压缩图像。

接下来开始测试无线传输的效率。

  • 两端都使用fastdds，速率以及带宽使用情况如下图：

可以看到速率相对与未压缩的图像有很大的提升，但是离实时依然有很大距离。

  • PC端换成cyclonedds，速率几乎没有变化

  • 小车端换成cyclonedds，速率反而降低了

小结

  • 直接发布 sensor_msgs::msg::CompressedImage 消息和通过 image_transport_plugins 插件自动发布压缩图像消息几乎是同样的效果
  • 通过ROS2的压缩图像进行图像传输，速率会有较大的提升，但是依然很难满足实时传输的需求；不过将图像的大小调整到足够小，会达到实时的效果。比如将图像缩放到320x240时，可达到实时，如下图：

使用地平线官方 websocket 进行图像传输

由于ROS2在大量数据传输上性能欠佳，所以地平线TogetherROS集成了一个web展示功能，目的是为了方便预览图像和算法效果。通过网络将图像和算法检测结果传输到浏览器端，然后进行渲染显示。

下面梳理一下图像数据从摄像头到web端的传输过程，以相关功能包展开：

  1. mipi_cam：启动mipi摄像头，发布nv12类型图像消息

下面是mipi_cam包里面的launch文件mipi_cam.launch.py的输入参数

parameters=[
                {"camera_calibration_file_path": "/opt/tros/lib/mipi_cam/config/F37_calibration.yaml"},
                {"out_format": "nv12"},
                {"image_width": 960},
                {"image_height": 544},
                {"io_method": "shared_mem"},
                {"video_device": "F37"}
            ]
  • camera_calibration_file_path：加载固定的标定文件，用于在节点中发布图像内参
  • out_format：数据的输出编码格式为nv12；主要是因为TogetherROS做图像推理所使用的格式也是nv12
  • image_height、image_width：设置发布图像的长宽为(960x544)，设置为这个长宽主要是因为后续hobot_codec包的编码器限制。
  • io_method：设置发布图像采用的方式为shared_mem高性能内存模式，目的是为了提高图像采集和传输的效率；目前 shared_mem 发布的主题是固定的：hbmem_img
  • video_device：指定mipi设备为F37

特别说明，发布 hbmem_img 主题的消息为TogetherROS自定义的消息包：hbm_img_msgs；默认发布的消息为：HbmMsg1080P.msg，即最高支持发布分辨率为1080p的图像；

HbmMsg1080P.msg字段如下：

int32 index    # 帧序列号
#uint64 time_stamp
builtin_interfaces/Time time_stamp    # 时间戳
uint32 height    # 高
uint32 width     # 宽
uint32 data_size    # 图像数据大小（字节）
uint32 step    # 每行像素数据的字节数
uint8[12] encoding    # 图像编码方式
uint8[6220800] data    # 图像数据

uint32 MAX_SIZE=6220800    # 图像数据的最大大小
  1. hobot_codec：将mipi_cam发布的nv12图像编码为websocket需要的jpeg格式图像
  2. mono2d_body_detection：接收nv12格式数据，进行算法推理，发布人体、人头、人脸、人手框感知消息

mono2d_body_detection是一个人体检测功能包，使用地平线的BPU处理器进行模型推理，类似的功能包在 hobot_perception目录下还有很多；其针对格式为nv12的图像数据做推理，检测结果发布于自定义的消息格式ai_msgs，包括人/物/车等检测框roi，跟踪track id，抓拍，特征，手势识别等结果。

这里也可以自己开发检测的功能包，以及设计相关的消息格式。

  1. hobot_websocket：建立WebSocket 服务器连接，将图像消息、检测消息、系统信息添加到一个名为 x3::FrameMessage 的对象中，然后进行序列化处理，最后将序列化后的数据发送到 WebSocket 客户端

hobot_websocket 包订阅所有需要显示的消息，同时编写了一个简单的网页来显示这些数据。

最后使用一个lunch文件启动

ros2 launch websocket hobot_websocket.launch.py

在经过上面的步骤之后，在PC端使用谷歌或Edge浏览器，输入http://IP:8000，即可查看图像和算法渲染效果（IP为设备IP地址）；可以看到即使是通过无线传输，摄像头传回的图像也可以实时显示。

既然通过这种方式在PC端可以实时显示，那么在Android端肯定也可以实时了；而且Android端只需要使用一个网页控件webview即可轻松实现浏览器的功能，代码如下：

import android.webkit.WebView;
WebView mWebView;

private void webViewPlay(){
        mWebView = findViewById(R.id.camera_web);
        /*
        启用 WebView 的 JavaScript 支持，
        设置 WebView 的 useWideViewPort 和 loadWithOverviewMode 属性，以便自适应屏幕大小。
        创建了一个 WebViewClient，在页面加载完成后调用 JavaScript 来重新调整 WebView 的大小。
         */
        WebSettings webSettings = mWebView.getSettings();
        webSettings.setJavaScriptEnabled(true);
        mWebView.getSettings().setUseWideViewPort(true);
        mWebView.getSettings().setLoadWithOverviewMode(true);
        mWebView.setWebViewClient(new WebViewClient() {
            @Override
            public void onPageFinished(WebView view, String url) {
                super.onPageFinished(view, url);
                view.loadUrl("javascript:(function() { " +
                        "var e = document.createEvent('Event');" +
                        "e.initEvent('resize',true,true);" +
                        "document.dispatchEvent(e);" +
                        "})()");
            }
        });

        mWebView.setWebChromeClient(new WebChromeClient()); // 需要设置 WebChromeClient 来支持视频播放
//        mWebView.loadUrl("https://www.baidu.com/?tn=02003390_19_hao_pg");
        mWebView.loadUrl("http://192.168.31.189/modules/"); // 直接加载图像画面

        // 设置 WebView 的 OnTouchListener 接口
        mWebView.setOnTouchListener(new View.OnTouchListener() {
            private long lastClickTime = 0;

            @Override
            public boolean onTouch(View v, MotionEvent event) {
                // 判断触摸事件类型是否为双击
                if (event.getAction() == MotionEvent.ACTION_DOWN) {
                    long currentTime = System.currentTimeMillis();
                    if (currentTime - lastClickTime < 500) {
                        // 如果用户双击屏幕，调用 WebView 的 reload() 方法刷新页面
                        mWebView.reload();
                    }
                    lastClickTime = currentTime;
                }
                return false;
            }
        });
    }

在上面的代码中，我添加了双击刷新Web页面的功能，目的是为了在图像传输重启后进行重新连接；如果不需要可以取消或者改为其他方式实现。

一些错误及解决方法

  • 在x3pi上安装ROS2 Fox 版本的image_transport 和 image_transport_plugins 包时出现以类似下错误：
E: Failed to fetch http://packages.ros.org/ros2/ubuntu/pool/main/r/ros-foxy-image-transport/ros-foxy-image-transport_2.4.0-1focal.20221013.013318_arm64.deb  404  Not Found [IP: 64.50.233.100 80]

这个错误可能是软件源已经过期造成的，执行以下命令解决：

sudo apt-get update 
// 或者
sudo apt-get update --fix-missing // 该选项可以帮助 apt-get 在下载软件包时忽略丢失的文件，并尝试自动修复软件包依赖关系。
  • 在运行 image_transport 相关功能包后出现以下错误：
Could not find a package configuration file provided by "image_transport"
  with any of the following names:

    image_transportConfig.cmake
    image_transport-config.cmake

  Add the installation prefix of "image_transport" to CMAKE_PREFIX_PATH or
  set "image_transport_DIR" to a directory containing one of the above files.
  If "image_transport" provides a separate development package or SDK, be
  sure it has been installed.

安装好image_transport 相关功能包，需要在终端更新一下：source /opt/ros/foxy/setup.bash

结果展示

总结

如果只是考虑在移动端实时显示视频流，那么必定还有一些其他的关于无线图像传输的方法来实现；但是在这方面我也暂时没有什么积累，后续也会考虑去学习实现一下其他方法。

虽然地平线开发的这个web展示功能能够解决无线通讯下实时显示图像的问题，但是实现过程还是略显繁杂，而且限制也比较多，在开发过程中远不及直接使用ROS2的图像传输来的方便。但是当前ROS2也存在大数据量传输性能不佳的问题，取舍之下只能优先选择地平线的实现方式。