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

FunnyWii
FunnyWii
发布于 2023-05-08 / 19 阅读
0
0

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

每日一问:我学PCL了吗?领导给批显卡了吗?
:没有

为什么要订阅数组型消息

比如在这个场景下: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 loopLimt = 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;
        for (int i = 0; i < loopLimt; 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;
}

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 objID [%ld]",msg->objClass[i]);
        ROS_INFO("The objID [%lf]",msg->xPos[i]);
        ROS_INFO("The objID [%lf]",msg->yPos[i]);
    }
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "funnywiisubscriber");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Subscriber,订阅名为my_topic的消息,注册回调函数 callback
    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]
[ INFO] [1683534532.867678599]: The objID [976.112915]
[ INFO] [1683534532.867688689]: The objID [1443.911743]
[ INFO] [1683534532.867697808]: ------------------ msg ---------------------
[ INFO] [1683534532.867707752]: The objID [2005]
[ INFO] [1683534532.867716578]: The objID [1286]
[ INFO] [1683534532.867726476]: The objID [1798.004028]
[ INFO] [1683534532.867736020]: The objID [156.275818]
[ INFO] [1683534532.867744898]: ------------------ msg ---------------------
[ INFO] [1683534532.867753617]: The objID [1494]
[ INFO] [1683534532.867762170]: The objID [549]
[ INFO] [1683534532.867771531]: The objID [580.149780]
[ INFO] [1683534532.867781842]: The objID [2040.718872]

之所以用C++标准库中的 vector.pushback进去,是因为在 .msg中定义的数组为不定长数组,如果直接用 for循环依次向数组里写值,则会报错,因此使用上述方式依次向空数组尾部添加新的value。

C++ 占位符

同时,在 ROS_INFO("The objID [%ld]",msg->objID[i]);这行代码中出现了占位符,一定要注意占位符的正确使用,比如 int16int32的占位符是不同的,如果混淆使用,即使编译能通过,程序运行时也会报错(如segmentation fault)。

详细占位符信息可以参考如下表格:

符号属性 长度 基本型 位数 取值范围 输入符举例 输出符举例
-- -- char 8 -2^7 ~ 2^7-1 %c %c、%d、%u
signed -- char 8 -2^7 ~ 2^7-1 %c %c、%d、%u
unsigned -- char 8 0 ~ 2^8-1 %c %c、%d、%u
[signed] short [int] 16 -2^15 ~ 2^15-1 %hd %hd
unsigned short [int] 16 0 ~ 2^16-1 %hu %hu 、%ho、%hx
[signed] -- int 32 -2^31 ~ 2^31-1 %d %d
unsigned -- [int] 32 0 ~ 2^32-1 %u %u 、%o、%x
[signed] long [int] 32 -2^31 ~ 2^31-1 %ld %ld
unsigned long [int] 32 0 ~ 2^32-1 %lu %lu 、%lo、%lx
[signed] long long [int] 64 -2^63 ~ 2^63-1 %I64d %I64d
unsigned long long [int] 64 0 ~ 2^64-1 %I64u %I64u、%I64o、%I64x
-- -- float 32 +/- 3.40282e+038 %f、%e、%g %f、%e、%g
-- -- double 64 +/- 1.79769e+308 %lf 、%le、%lg %f、%e、%g
-- long double 96 +/- 1.79769e+308 %Lf、%Le、%Lg %Lf 、%Le、%Lg

评论