ROS2创建自定义数组类消息并进行发布

说明

自定义创建一个数组类型的消息，使用python创建ROS2节点，并将消息进行发布
参考链接：https://blog.csdn.net/qq_32618327/article/details/128793362

1.创建自定义消息

初始化工作空间

mkdir -p myros_ws/src

1.1 创建自定义消息

创建独立的功能包来保存自定义消息，自定义消息的说明如下：

1. 发布的topic是一个包含了多个元素的数组，针对同一次输入有多个格式相同的输入(如同一张图像有多个目标检测结果)
2. 每个数组元素包含多个数据类型

cd myros_ws/src
ros2 pkg create --build-type ament_cmake box_interfaces
cd box_interfaces/
mkdir msg && cd msg
touch Box3d.msg Box3dArray.msg

创建成功会有LOG打印：

将以下内容添加到对应的消息文件：
# Box3d.msg是一个基础的检测框的信息，包括了检测目标的类别、得分、中心点位置、长宽高、yaw角等信息

# Box3d.msg
int8 class_id
float32 score
geometry_msgs/Point center
float32 dx
float32 dy
float32 dz
float32 yaw

Box3dArray.msg是一个数组类消息，用于存放同一帧数据的多个检测结果，数组中的数据类型是上面定义的OBJBox3d,数组的名词为obj_box3d_array

# Box3dArray.msg
std_msgs/Header header
Box3d[] obj_box3d_array

1.2 修改CMakeLists.txt和package.xml

在CMakeLists.txt的 find dependencies下添加相应的依赖和自定义消息目录

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Box3d.msg"
  "msg/Box3dArray.msg"
  DEPENDENCIES geometry_msgs
)

在package.xml中添加相应依赖

    <buildtool_depend>rosidl_default_generators</buildtool_depend>
    <member_of_group>rosidl_interface_packages</member_of_group>

  <export>
    <build_type>ament_cmake</build_type>
    <depend>rclpy</depend>
    <depend>std_msgs</depend>
    <depend>geometry_msgs</depend>
    <build_depend>message_generation</build_depend>
    <exec_depend>message_runtime</exec_depend>
  </export>

2.创建发布者

2.1 创建发布者并编写发布节点

cd myros_ws/src
# 初始化发布者的package,ament_python表示使用python编码，pub_box3d表示package的名字
ros2 pkg create --build-type ament_python pub_box3d

package创建后终端会有日志进行打印

编写发布者代码,在myros_ws/src/pub_box3d/pub_box3d目录下新建一个文件publish_box3d.py

import rclpy
from rclpy.node import Node 
from std_msgs.msg import Header
from std_msgs.msg import String
# 引用自定义的消息格式，Box3d, OBJBox3dArray即消息文件名
from box_interfaces.msg import Box3d, Box3dArray

# 创建一个发布器节点类
class MinimalPublisher(Node):
    '''
    发布器节点类
    '''
    def __init__(self):
        # 初始化节点名、发布器、每0.5s回调的定时器和计数器
        super().__init__('minimal_publisher')
        # 创建发布者，发布的TOPIC类型为Box3dArray，名称为'/velodyne/box3ds'
        self.publisher_ = self.create_publisher(Box3dArray, '/velodyne/box3ds', 10)
        # 为了测试添加定时器，实际上发布频率会由数据频率和处理速度控制
        timer_period = 0.5
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        '''
        定时器回调函数
        '''
		# 声明一个OBJBox3dArray消息，并初始化Header
        header = Header()
        now = self.get_clock().now().to_msg()
        header.stamp = now  # 获取当前时间戳  
        header.frame_id = "velodyne"  # 设置frame_id，这里只是一个示例
        msg_box = Box3dArray()
        msg_box.header = header
        # 做循环填充信息，实际上OBJBox3dArray中的元素个数以及内容由检测结果决定
        for i in range(3):
        	# 创建一个OBJBox3d类型的消息并进行初始化
            msg = Box3d()
            msg.class_id = int(self.i)
            msg.center.x = 1.0
            msg.center.y = 1.0
            msg.center.z = 1.0
            msg.dx = 1.0
            msg.dy = 1.0
            msg.dz = 1.0
            msg.yaw = 1.0
            # 将OBJBox3d消息添加到数组中，obj_box3d_array是上面自定义消息格式中的数组消息名称
            msg_box.obj_box3d_array.append(msg)
        # 发布消息
        self.publisher_.publish(msg_box)
        self.i += 1


def main(args=None):
    # 初始化ROS2
    rclpy.init(args=args)

    # 创建节点
    minimal_publisher = MinimalPublisher()

    # 运行节点
    rclpy.spin(minimal_publisher)

    # 销毁节点，退出ROS2
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

2.2 修改配置文件

1. 修改package.xml,添加依赖信息

  <export>
    <build_type>ament_python</build_type>
    <depend>rclpy</depend>
    <depend>std_msgs</depend>
    <depend>geometry_msgs</depend>
    <!-- box_interfaces 是自定义消息的package -->
    <exec_depend>box_interfaces</exec_depend>
    <build_depend>message_generation</build_depend>
    <exec_depend>message_runtime</exec_depend>
    <buildtool_depend>rosidl_default_generators</buildtool_depend>
    <member_of_group>rosidl_interface_packages</member_of_group>
  </export>

2. 修改setup.py文件，添加node节点信息

    entry_points={
        'console_scripts': [
            'pub_box3d_node = pub_box3d.publish_box3d:main',
        ],
    },

3.编译和运行

编译

cd myros_ws
colcon build
source install/setup.bash
ros2 run pub_box3d pub_box3d_node

新打开一个终端

source install/setup.bash
# 查看当前的topic
ros2 topic list
# 打印topic信息
ros2 topic echo /velodyne/box3ds

结果显示如下，这是一条完整的topic，类型是obj_box3d_array数组，每个数组自定义了3个结果

再看一下发布频率

ros2 topic hz /velodyne/box3ds

打印日志，因为再发布节点中设置了0.5s的定时器，所以这里的频率的2hz：

average rate: 2.001
        min: 0.499s max: 0.500s std dev: 0.00048s window: 3
average rate: 2.001
        min: 0.499s max: 0.500s std dev: 0.00037s window: 6
average rate: 2.000
        min: 0.499s max: 0.500s std dev: 0.00034s window: 8
average rate: 2.000