CODEWITHSUNDEEP
drake
milLII3yway
milLII3yway

Reputation: 15

Pointcloud Visualization in Drake Visualizer in Python

I would like to visualize pointcloud in drake-visualizer using python binding.

I imitated how to publish images through lcm from here, and checked out these two issues (14985, 14991). The snippet is as follows :

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()
)

However, I got the following runtime error:

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)

When I set 'use_cpp_serializer=True', the error becomes

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]

I saw the cpp example here, so maybe this issue is specific to python binding. I also saw this python example, but thought using 'PointCloudToLcm' might be more convenient.

P.S.

Upvotes: 0

Views: 284

Answers (1)

jwnimmer-tri
jwnimmer-tri

Reputation: 2449

Only the message types that are specifically bound in lcm_py_bind_cpp_serializers.cc can be used on an LCM message input/output port connection between C++ and Python. For all other LCM message types, the input/output port connection must be from a Python system to a Python system or a C++ System to a C++ System.

The lcmt_image_array is listed there, but not the lcmt_point_cloud.

If you're stuck using Drake's v0.35.0 capabilities, then I don't see any great solutions. Some options:

(1) Write your own PointCloudToLcm system in Python (by re-working the C++ code into Python, possibly with a narrower set of supported features / channels for simplicity).

(2) Write your own small C++ helper function MakePointCloudPublisherSystem(...) that calls LcmPublisherSystem::Make<lcmt_point_cloud> function in C++, and bind it into Python. Then your Python code can call MakePointCloudPublisherSystem() and successfully connect that to the existing C++ PointCloudToLcm.

Upvotes: 1

Related Questions