0

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.
  • I am aware of the development in recent commits on MeshcatVisualizerCpp and MeshcatPointCloudVisualizerCpp, but I am still on the drake-dev stable build 0.35.0-1 and want to stay on drake visualizer until the meshcat c++ is more mature.
  • The old version in pydrake.systems.meshcat_visualizer.MeshcatVisualizer is a bit too slow on my current use-case (multiple objects drop). I can visualize the pointcloud with this visualization setting, but it took too much machine resources.
milLII3yway
  • 15
  • 4

1 Answers1

1

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.

jwnimmer-tri
  • 1,994
  • 2
  • 5
  • 6
  • Thank you for your reply and suggestions. I am currently using Drake's v0.35.0, but will upgrade when the new stable version released. However, probably gonna stick with DrakeVisualization for a little while. Since there is PointCloudToLcm() binding in pydrake, will `lcmt_point_cloud` be included in the binding in the future version? I guess I can also try to build my own version of drake as the alternative solution (last time I tried, it failed. probably need to spend more time getting familiar with bazel). – milLII3yway Nov 01 '21 at 15:25
  • I've filed https://github.com/RobotLocomotion/drake/issues/16022 as the feature request. I think we should be able to do it for in the next release. (I discussed with other Drake developers. In past versions, we had an important reason why only some messages were available. As of today though, that reason no longer applies, so we can add them all.) – jwnimmer-tri Nov 01 '21 at 16:41
  • Wow, awesome!!! Thank you very much. Looking forward for the next release. – milLII3yway Nov 01 '21 at 18:11
  • I tested with the nightly build. The serialization works! Thank you. Got small issue on the frame of the pointcloud though. DepthImageToPointCloud in python doesn't support camera_pose_input_port(). Is there the System that can transform the pointcloud? or should I file a feature request on the main repo? MeshcatPointCloudVisualizerCpp_ seems to support it via pose_input_port(). Related issues [10080](https://github.com/RobotLocomotion/drake/pull/10080), [10657](https://github.com/RobotLocomotion/drake/issues/10657). – milLII3yway Nov 04 '21 at 18:38
  • Looks like that input port was added to C++ in https://github.com/RobotLocomotion/drake/pull/10080 but we forgot to add it to Python bindings for its getter. For now, you can call `depth_to_cloud_system.GetInputPort("camera_pose")` instead to fetch it by string name, instead of with the dedicated method. – jwnimmer-tri Nov 04 '21 at 23:17
  • Oh, didn't thought it can be done though that parent method. Work like a charm. Thank you! – milLII3yway Nov 05 '21 at 03:43