Plane Detection in ROS 2

In this tutorial, you will learn how to exploit the Plane Detection capabilities of the ZED SDK to detect the planes in the environment where a ZED camera is operating.

Start a plane detection task #

The ZED ROS 2 nodelet subscribes to the topic /clicked_point of type geometry_msgs/PointStamped, usually published by Rviz2.

When a message on the topic /clicked_point is received, the node searches for the first plane hitten by a virtual ray starting from the camera optical center and virtually passing through the received 3D point.

If a plane is found, its position and orientation are calculated, the 3D mesh is extracted and all the useful plane information is published as a custom zed_interfaces/PlaneStamped message on the topic /<camera_model>/<node_name>/plane.

A second message of type visualization_msgs/Marker with information useful for visualization is published on the topic /<camera_model>/<node_name>/plane_marker in order to display the plane using a Marker display plugin in Rviz.

Planes

Planes no clouds

Logging #

When a plane detection is started, the log of the ROS wrapper will show the following information:

[zed_wrapper-2] 1651505097.533702002 [zed2i.zed_node] [INFO] Clicked 3D point [X FW, Y LF, Z UP]: [2.04603,-0.016467,0.32191]
[zed_wrapper-2] 1651505097.533748287 [zed2i.zed_node] [INFO] 'map' -> 'zed2i_left_camera_optical_frame': {0.061,0.010,0.011} {0.495,-0.534,0.466,0.503}
[zed_wrapper-2] 1651505097.533775284 [zed2i.zed_node] [INFO] Point in camera coordinates [Z FW, X RG, Y DW]: {0.044,-0.436,2.034}
[zed_wrapper-2] 1651505097.533812591 [zed2i.zed_node] [INFO] Clicked point image coordinates: [655.536,231.765]
[zed_wrapper-2] 1651505097.592107647 [zed2i.zed_node] [INFO] Found plane at point [2.046,-0.016,0.322] -> Center: [1.909,-0.760,-0.007], Dims: 1.681x2.519

Rviz2 #

The Rviz2 GUI allows one to easily start a plane detection task and displays the results of the detection.

Start a plane detection #

To publish a /clicked_point point message and start a plane detection the Publish Point button must be enabled and a point of the 3D view or the camera view must be clicked.

Publish Point

Configure Rviz to display the results #

The Marker plugin allows you to visualize the information of the detected planes.

Marker

Key parameters:

  • Topic: The topic that contains the information relative to the detected planes: e.g. /zed/zed_node/plane_marker
  • Depth: The size of the message queue. Use at least a value of 2 to not lose messages.
  • History policy: Set the QoS history policy. Keep Last is suggested for performance and compatibility.
  • Reliability Policy: Set the QoS reliability policy. Best Effort is suggested for performance and compatibility.
  • Durability Policy: Set the QoS durability policy. Volatile is suggested for compatibility.
  • Namespaces: The list of available information:
    • plane_hit_points: Select to display a sphere where the click has been received.
    • plane_meshes: Select to display all the meshes of the detected planes.

Detected Plane message #

The custom zed_interfaces/PlaneStamped message is defined in the zed-ros2-interfaces repository.