Блог

Я хотел бы визуализировать pointcloud в drake-визуализаторе, используя привязку python.

Я имитировал, как публиковать изображения через lcm отсюда, и проверил эти два выпуска (14985, 14991). Фрагмент выглядит следующим образом :

 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 здесь, так что, возможно, эта проблема специфична для привязки python.
Я также видел этот пример python, но подумал, что использование «PointCloudToLcm» может быть более удобным.

P.S.

• Я в курсе последних изменений в MeshcatVisualizerCpp и MeshcatPointCloudVisualizerCpp, но я все еще нахожусь в стабильной сборке drake-dev 0.35.0-1 и хочу оставаться на визуализаторе drake, пока meshcat c не станет более зрелым.
• Старая версия в pydrake.systems.meshcat_visualizer.MeshcatVisualizer работает слишком медленно в моем текущем случае использования (отбрасывается несколько объектов). Я могу визуализировать облако точек с помощью этой настройки визуализации, но для этого потребовалось слишком много ресурсов машины.

Только типы сообщений, которые специально привязаны в lcm_py_bind_cpp_serializers.cc может использоваться для подключения порта ввода/вывода сообщений LCM между C и Python. Для всех других типов сообщений LCM подключение порта ввода/вывода должно осуществляться из системы Python в систему Python или из системы C в систему C .

То lcmt_image_array указано там, но не то lcmt_point_cloud .

Если вы застряли, используя возможности Drake v0.35.0, то я не вижу никаких отличных решений. Некоторые варианты:

(1) Напишите свою собственную PointCloudToLcm систему на Python (переработав код C на Python, возможно, с более узким набором поддерживаемых функций / каналов для простоты).

(2) Напишите свою собственную небольшую вспомогательную функцию C MakePointCloudPublisherSystem(...) , которая вызывает LcmPublisherSystem::Make<lcmt_point_cloud> функцию на C , и свяжите ее с Python. Затем ваш код Python может вызвать MakePointCloudPublisherSystem() и успешно подключить его к существующему C PointCloudToLcm .