ROS入门 - 如何订阅数组型消息

为什么要订阅数组型消息

比如在这个场景下：YOLO 在场景中检测到多个目标，并通过其他算法得到了目标的 ID、class、位置、速度等信息。这些信息会作为避障或局部路径规划的依据。一台自动驾驶汽车或机器人上的其他算法模块也可能需要这些信息，这种时候会用 ROS 来进行通讯。

假设这是需要发布的 msg，其中包含 int64 数组和 float64 数组，均为不定长数组：

int64[] objID
int64[] objClass
float64[] xPos
float64[] yPos
float64[] dist
float64[] xSpeed
float64[] ySpeed
int64 objNum

Publisher 和 Subscriber

Publisher

//
// Created by funnywii on 2023/3/10.
// The main function for testing other functions.
//
#include <iostream>
#include "src/MyHeaders.h"
#include <opencv2/opencv.hpp>
#include "src/YoloV5.h"
#include "src/CppLearning.h"

#include <ros/ros.h>
#include <std_msgs/String.h>
#include "funnywiitest/funnywii_msg.h"

#include <cstdlib>  // rand() 和 srand()

int main(int argc, char **argv)
{
    ros::init(argc, argv, "funnywiipublisher");
    ros::NodeHandle n;
    ros::Publisher funny_pub = n.advertise<funnywiitest::funnywii_msg>("test_topic", 100);
    ros::Rate loop_rate(1);

    int loopLimit = 10;

    std::vector<int64_t> objID;
    std::vector<int64_t> objClass;
    std::vector<double> xPos;
    std::vector<double> yPos;

    while (ros::ok())
    {
        funnywiitest::funnywii_msg msg;

        objID.clear();
        objClass.clear();
        xPos.clear();
        yPos.clear();

        for (int i = 0; i < loopLimit; i++)
        {
            objID.push_back(int(rand() / 1e6));
            std::cout << objID[i] << std::endl;
            objClass.push_back(int(rand() / 1e6));
            xPos.push_back(float(rand() / 1e6));
            std::cout << xPos[i] << std::endl;
            yPos.push_back(float(rand() / 1e6));
        }

        msg.objID = objID;
        msg.objClass = objClass;
        msg.xPos = xPos;
        msg.yPos = yPos;

        funny_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }

    cout << "Test Done" << endl;
    return 0;
}

Subscriber

#include <ros/ros.h>
#include "funnywiitest/funnywii_msg.h"
#include <std_msgs/String.h>

void callback(const funnywiitest::funnywii_msg::ConstPtr & msg)
{
    for (int i = 0; i < msg->objID.size(); i++) {
        ROS_INFO("------------------ msg ---------------------");
        ROS_INFO("The objID    [%ld]", msg->objID[i]);
        ROS_INFO("The objClass [%ld]", msg->objClass[i]);
        ROS_INFO("The xPos     [%lf]", msg->xPos[i]);
        ROS_INFO("The yPos     [%lf]", msg->yPos[i]);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "funnywiisubscriber");
    ros::NodeHandle n;
    ros::Subscriber funny_sub = n.subscribe<funnywiitest::funnywii_msg>("test_topic", 100, callback);
    ros::spin();
    return 0;
}

运行 rosrun funnywiitest funnywiisubscriber 之后，可以看到运行结果：

[ INFO] [1683534532.867650458]: ------------------ msg ---------------------
[ INFO] [1683534532.867660196]: The objID [2106]
[ INFO] [1683534532.867669170]: The objID [1780]
...

之所以用 C++ 标准库中的 vector.push_back()，是因为 .msg 中定义的是不定长数组。

若直接用 for 循环按下标赋值，可能因数组未初始化而报错，因此需先向空 vector 尾部逐个添加元素。

C++ 占位符

在 ROS_INFO("The objID [%ld]", msg->objID[i]); 中使用了 printf 风格占位符。

占位符必须与数据类型匹配，例如 int16 和 int32 的占位符不同。若混用，即使编译通过，运行时也可能报错（如 segmentation fault）。

int64_t 在 Linux 上通常使用 %ld，float64（double）使用 %lf。