ROS2自定义Float32MultiArray子类转C时断言失败错误求助

问题场景 #

我尝试为ROS2的Float32MultiArray对象写一个封装类，用来存储物体的x、y、z三维坐标，代码如下：

class LocationMsg(Float32MultiArray):
    def __init__(self, x_c, y_c, z):
        super().__init__()
        self.data = [x_c, y_c, z]

启动ROS2项目时触发了Python到C的数据转换错误，错误信息如下：

python3: /tmp/binarydeb/ros-galactic-std-msgs-2.2.4/obj-aarch64-linux-gnu/rosidl_generator_py/std_msgs/msg/_float32_multi_array_s.c:55: std_msgs__msg__float32_multi_array__convert_from_py: Assertion `strncmp("std_msgs.msg._float32_multi_array.Float32MultiArray", full_classname_dest, 51) == 0' failed.
Stack trace (most recent call last):
#31   Object "/usr/bin/python3", at 0x503667, in _PyEval_EvalFrameDefault
#30   Object "/usr/bin/python3", at 0x5935e3, in _PyObject_MakeTpCall
#29   Object "/usr/bin/python3", at 0x534e87, in
#28   Object "/usr/bin/python3", at 0x59637f, in _PyObject_Call_Prepend
#27   Object "/usr/bin/python3", at 0x5960af, in _PyObject_FastCallDict
#26   Object "/usr/bin/python3", at 0x59684f, in _PyFunction_Vectorcall
#25   Object "/usr/bin/python3", at 0x4feda3, in _PyEval_EvalFrameDefault
#24   Object "/usr/bin/python3", at 0x48bf8f, in
#23   Object "/usr/bin/python3", at 0x487b37, in
#22   Object "/usr/bin/python3", at 0x5002db, in _PyEval_EvalFrameDefault
#21   Object "/usr/bin/python3", at 0x487b37, in
#20   Object "/usr/bin/python3", at 0x5002db, in _PyEval_EvalFrameDefault
#19   Object "/usr/bin/python3", at 0x487b37, in
#18   Object "/usr/bin/python3", at 0x5001c7, in _PyEval_EvalFrameDefault
#17   Object "/usr/bin/python3", at 0x592bb7, in PyObject_Call
#16   Object "/usr/bin/python3", at 0x493e03, in
#15   Object "/usr/bin/python3", at 0x59684f, in _PyFunction_Vectorcall
#14   Object "/usr/bin/python3", at 0x4feda3, in _PyEval_EvalFrameDefault
#13   Object "/usr/bin/python3", at 0x59684f, in _PyFunction_Vectorcall
#12   Object "/usr/bin/python3", at 0x50340f, in _PyEval_EvalFrameDefault
#11   Object "/usr/bin/python3", at 0x49438b, in
#10   Object "/usr/bin/python3", at 0x5935e3, in _PyObject_MakeTpCall
#9    Object "/usr/bin/python3", at 0x593247, in PyCFunction_Call
#8    Object "/opt/ros/galactic/lib/python3.8/site-packages/rclpy/_rclpy_pybind11.cpython-38-aarch64-linux-gnu.so", at 0xffffbbe1ae8f, in
#7    Object "/opt/ros/galactic/lib/python3.8/site-packages/rclpy/_rclpy_pybind11.cpython-38-aarch64-linux-gnu.so", at 0xffffbbddc05f, in
#6    Object "/opt/ros/galactic/lib/python3.8/site-packages/rclpy/_rclpy_pybind11.cpython-38-aarch64-linux-gnu.so", at 0xffffbbde2983, in
#5    Object "/opt/ros/galactic/lib/librclpy_common.so", at 0xffffbbd98d3f, in rclpy_convert_from_py
#4    Object "/opt/ros/galactic/lib/libstd_msgs__python.so", at 0xffffa6dc17c3, in std_msgs__msg__float32_multi_array__convert_from_py
#3    Object "/lib/aarch64-linux-gnu/libc.so.6", at 0xffffbcc1e4f3, in __assert_fail
#2    Object "/lib/aarch64-linux-gnu/libc.so.6", at 0xffffbcc1e48f, in
#1    Object "/lib/aarch64-linux-gnu/libc.so.6", at 0xffffbcc11aab, in abort
#0    Object "/lib/aarch64-linux-gnu/libc.so.6", at 0xffffbcc24d78, in gsignal
Aborted (Signal sent by tkill() 9066 1000)

我使用的是ROS2 Galactic版本，试过用np.float32()转换坐标值但没用，而且发现self.data需要父类转换后的float类型。下面是最小复现代码：

#ROS2 Imports
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32MultiArray

import numpy as np

#Publisher class for 3 tuple of blob X Y Z coordinates
class LocationMsg(Float32MultiArray):
    def __init__(self, x_c, y_c, z):
        super().__init__()
        self.data = [x_c, y_c, z]
        

class TestNode(Node):

    def __init__(self):
        super().__init__('TestNode')

        self.timer_period = 1/12  # frequency to call timer_callback

        self.publisher_ = self.create_publisher(LocationMsg, "/Location", 10)
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

    def timer_callback(self):
        x_c = 5.0
        y_c = 5.0
        z = 5.0
        msg = LocationMsg(x_c,y_c,z)
        self.LocationPublisher_.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    publisher = TestNode()
    rclpy.spin(publisher)
    publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

错误原因分析 #

ROS2的Python消息类由代码生成器自动生成，底层与C++实现强绑定。转换函数std_msgs__msg__float32_multi_array__convert_from_py会严格检查传入对象的类名，必须完全匹配std_msgs.msg._float32_multi_array.Float32MultiArray，自定义的LocationMsg类名不符合要求，因此断言失败。

此外，创建Publisher时指定LocationMsg作为消息类型，但ROS2通信系统仅识别官方生成的消息类型，自定义子类无法被正确识别和转换。

解决方案 #

不要直接继承ROS2的消息类，改用组合模式或静态工厂方法封装坐标创建逻辑：

方案1：静态工厂方法 #

在自定义类中实现静态方法，返回标准的Float32MultiArray对象：

class LocationMsg:
    @staticmethod
    def create(x_c, y_c, z):
        msg = Float32MultiArray()
        msg.data = [float(x_c), float(y_c), float(z)]
        return msg

使用方式：

msg = LocationMsg.create(x_c, y_c, z)
self.publisher_.publish(msg)

创建Publisher时需指定标准消息类型：

self.publisher_ = self.create_publisher(Float32MultiArray, "/Location", 10)

方案2：组合类 #

将Float32MultiArray作为实例属性，提供获取消息对象的方法：

class LocationMsg:
    def __init__(self, x_c, y_c, z):
        self._msg = Float32MultiArray()
        self._msg.data = [float(x_c), float(y_c), float(z)]
    
    def get_msg(self):
        return self._msg
    
    # 可选：添加坐标访问与修改方法
    @property
    def x(self):
        return self._msg.data[0]
    
    @x.setter
    def x(self, value):
        self._msg.data[0] = float(value)

使用方式：

msg = LocationMsg(x_c, y_c, z)
self.publisher_.publish(msg.get_msg())

额外修复：复现代码中的拼写错误 #

TestNode类中self.LocationPublisher_未定义，需改为self.publisher_，否则运行时会触发属性错误。

内容的提问来源于stack exchange，提问作者John B