Python 中 Drake Visualizer 中的点云可视化

Question

我想使用 python 绑定在 drake-visualizer 中可视化点云。

我模仿了here, and checked out these two issues (14985, 14991）如何通过lcm发布图片。片段如下：

point_cloud_to_lcm_point_cloud = builder.AddSystem(PointCloudToLcm())

point_cloud_to_lcm_point_cloud.set_name('pointcloud_converter')
builder.Connect(
    station.GetOutputPort('camera0_point_cloud'),
    point_cloud_to_lcm_point_cloud.get_input_port()
)

point_cloud_lcm_publisher = builder.AddSystem(
    LcmPublisherSystem.Make(
        channel="DRAKE_POINT_CLOUD_camera0",
        lcm_type=lcmt_point_cloud,
        lcm=None,
        publish_period=0.2,
        # use_cpp_serializer=True
    )
)
point_cloud_lcm_publisher.set_name('point_cloud_publisher')
builder.Connect(
    point_cloud_to_lcm_point_cloud.get_output_port(),
    point_cloud_lcm_publisher.get_input_port()
)

但是，我收到以下运行时错误：

RuntimeError: DiagramBuilder::Connect: Mismatched value types while connecting output port lcmt_point_cloud of System pointcloud_converter (type drake::lcmt_point_cloud) to input port lcm_message of System point_cloud_publisher (type drake::pydrake::Object)

当我设置'use_cpp_serializer=True'时，错误变为

LcmPublisherSystem.Make(
File "/opt/drake/lib/python3.8/site-packages/pydrake/systems/_lcm_extra.py", line 71, in _make_lcm_publisher
  serializer = _Serializer_[lcm_type]()
File "/opt/drake/lib/python3.8/site-packages/pydrake/common/cpp_template.py", line 90, in __getitem__
  return self.get_instantiation(param)[0]
File "/opt/drake/lib/python3.8/site-packages/pydrake/common/cpp_template.py", line 159, in get_instantiation
  raise RuntimeError("Invalid instantiation: {}".format(
RuntimeError: Invalid instantiation: _Serializer_[lcmt_point_cloud]

我看到了 cpp 示例 here，所以这个问题可能特定于 python 绑定。我也看到了 this python example，但认为使用 'PointCloudToLcm' 可能更方便。

P.S.

我知道最近在 MeshcatVisualizerCpp 和 MeshcatPointCloudVisualizerCpp 上提交的开发，但我仍在使用 drake-dev stable build 0.35.0-1 并希望继续使用drake visualizer 直到 meshcat c++ 更加成熟。
pydrake.systems.meshcat_visualizer.MeshcatVisualizer 中的旧版本在我当前的用例（多个对象掉落）上有点太慢了。我可以用这个可视化设置来可视化点云，但是它占用了太多的机器资源。

Answer 1

只有在 lcm_py_bind_cpp_serializers.cc 中明确绑定的消息类型才能用于 C++ 和 Python 之间的 LCM 消息 input/output 端口连接。对于所有其他 LCM 消息类型，input/output 端口连接必须是从 Python 系统到 Python 系统或从 C++ 系统到 C++ 系统。

那里列出了 lcmt_image_array，但没有列出 lcmt_point_cloud。

如果您无法使用 Drake 的 v0.35.0 功能，那么我看不到任何好的解决方案。部分选项：

(1) 在 Python 中编写您自己的 PointCloudToLcm 系统（通过将 C++ 代码重新编写为 Python，为简单起见，可能使用一组更窄的支持功能/通道).

(2) 编写自己的小C++辅助函数MakePointCloudPublisherSystem(...)调用C++中的LcmPublisherSystem::Make<lcmt_point_cloud>函数，绑定到Python。然后您的 Python 代码可以调用 MakePointCloudPublisherSystem() 并成功将其连接到现有的 C++ PointCloudToLcm.

Python 中 Drake Visualizer 中的点云可视化

Pointcloud Visualization in Drake Visualizer in Python

drake

P.S.