ROS2 Float32MultiArray 从 python 转换到 c 时出错

Question

我正在尝试为 ROS2 中包含的 Float32MultiArray 对象创建一个包装类。包装类设置为包含 3 个值，对应于对象的 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)

这里我提供了MRE。我正在使用 ROS2 Galactic。使用 np.float32() 转换 x_c、y_c 和 z 不起作用，因为 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()

Answer 1

这与将 Python 转换为 C 无关，而是您的错误发生在 Python 使用的某些 C 绑定中。该错误是因为您继承了 Float32 数组消息。为了简单起见，当发生内部类型检查时，断言失败，因为您的消息不是 strictly 原始

Float32MultiArray

消息。您传递的内容包括

LocationMsg

类型的数据。无论如何，这不是您处理自定义消息类型的方式。 All ROS 消息应该在消息文件中明确定义。 官方文档 有你应该采取的步骤。对于您的示例，您应该只包含一个消息文件，其中包含：

float32 x
float32 y
float32 z

综上所述，对于您似乎要问的内容，也已经存在消息定义。我强烈建议使用已经可用的接口而不是尝试重新创建轮子。