ROS2 std_msg转PyDrake兼容值报错：SerializerInterface调用问题

问题 #

我正在开发一个运行PyDrake仿真的ROS2节点，用于模拟机器人动力学、测试现有ROS2控制栈。采用drake_ros的RosSubscriberSystem订阅ROS话题发布的指令扭矩值，再通过带有两个AbstractValue输入端口的LeafSystem，将Float32消息转换并组合为MultibodyPlant适用的2D向量。但运行节点时，仿真开始步进后反复出现错误：

Tried to call pure virtual function "SerializerInterface::Deserialize"

我怀疑错误源于AbstractValue.Make(Float32())的使用不当。以下是最小复现代码示例：

转换器LeafSystem代码 #

class ControlTorquesConverter(LeafSystem):
    def __init__(self):
        super().__init__()
        self.inputFloat32drive = self.DeclareAbstractInputPort(
            "cmd_drive_torques", AbstractValue.Make(Float32())
        )
        self.inputFloat32steer = self.DeclareAbstractInputPort(
            "cmd_steer_torques", AbstractValue.Make(Float32())
        )

        self.DeclareVectorOutputPort("drive_steer_torques", 2, self.ConvertValues)

    def ConvertValues(self, context, output):
        drive_torque_msg = self.inputFloat32drive.Eval(context)
        steer_torque_msg = self.inputFloat32steer.Eval(context)

        drive_val = drive_torque_msg.data
        steer_val = steer_torque_msg.data

        output.SetFromVector([drive_val, steer_val])

节点中调用代码 #

drive_control_sub = self.builder.AddSystem(RosSubscriberSystem.Make(Float32, 
                                                                     "/cmd/drive_torque", 
                                                                     qos, 
                                                                     self.ros_interface_system.get_ros_interface()))
steer_control_sub = self.builder.AddSystem(RosSubscriberSystem.Make(Float32, 
                                                                     "/cmd/steer_torque", 
                                                                     qos, 
                                                                     self.ros_interface_system.get_ros_interface()))
control_converter = self.builder.AddSystem(ControlTorquesConverter())

# connect control sub to plant
self.builder.Connect(drive_control_sub.get_output_port(0),
                     control_converter.inputFloat32drive)
self.builder.Connect(steer_control_sub.get_output_port(0),
                     control_converter.inputFloat32steer)
# self.builder.Connect(control_converter.get_output_port(),
#                      self.plant.get_actuation_input_port())

已确认图构建完成且仿真初始化成功，仅在步进时崩溃。请问在LeafSystem中使用AbstractValue.Make(Float32())对接RosSubscriberSystem输出是否有效？或有更优实现方式？

解答 #

问题根源 #

你遇到的错误核心原因是：AbstractValue.Make(Float32())没有为ROS2消息类型绑定正确的序列化/反序列化逻辑。Drake的普通AbstractValue不会自动处理ROS2消息的序列化接口，而RosSubscriberSystem输出的AbstractValue是带有ROS专属序列化器的，两者类型不匹配，导致运行时找不到Deserialize的具体实现。

正确实现方式 #

不要用AbstractValue.Make(Float32())声明输入端口，而是使用Drake为ROS2消息提供的专用抽象值类型RosValue。

优化后的转换器代码

from drake_ros.core import RosValue

class ControlTorquesConverter(LeafSystem):
    def __init__(self):
        super().__init__()
        # 使用RosValue声明ROS2消息类型的抽象输入端口
        self.inputFloat32drive = self.DeclareAbstractInputPort(
            "cmd_drive_torques", RosValue[Float32]()
        )
        self.inputFloat32steer = self.DeclareAbstractInputPort(
            "cmd_steer_torques", RosValue[Float32]()
        )

        self.DeclareVectorOutputPort("drive_steer_torques", 2, self.ConvertValues)

    def ConvertValues(self, context, output):
        # Eval直接得到Float32消息实例
        drive_torque_msg = self.inputFloat32drive.Eval(context)
        steer_torque_msg = self.inputFloat32steer.Eval(context)

        drive_val = drive_torque_msg.data
        steer_val = steer_torque_msg.data

        output.SetFromVector([drive_val, steer_val])

简化方案：用Lambda省略自定义LeafSystem

如果只是简单的消息转数值，可直接用AbstractValueConverter配合Lambda函数，省去自定义LeafSystem的步骤：

from drake.systems.framework import AbstractValueConverter, Multiplexer, BasicVector

# 定义Float32消息转标量的函数
def float32_to_scalar(context, msg):
    return msg.data

# 为订阅器输出添加转换，得到标量端口
drive_scalar_port = self.builder.AddSystem(
    AbstractValueConverter(
        drive_control_sub.get_output_port(0), 
        float32_to_scalar, 
        BasicVector(1)
    )
).get_output_port()

steer_scalar_port = self.builder.AddSystem(
    AbstractValueConverter(
        steer_control_sub.get_output_port(0), 
        float32_to_scalar, 
        BasicVector(1)
    )
).get_output_port()

# 合并两个标量为2D向量
mux = self.builder.AddSystem(Multiplexer([1, 1]))
self.builder.Connect(drive_scalar_port, mux.get_input_port(0))
self.builder.Connect(steer_scalar_port, mux.get_input_port(1))

# 连接到MultibodyPlant的驱动端口
self.builder.Connect(mux.get_output_port(), self.plant.get_actuation_input_port())

关键说明 #

• RosValue[T]是Drake专为ROS2消息设计的AbstractValue子类，内置了ROS序列化/反序列化逻辑，能完美匹配RosSubscriberSystem的输出端口类型。
• 简单转换场景下，使用AbstractValueConverter可以减少冗余代码，提升开发效率。

内容的提问来源于stack exchange，提问作者Micah O.