Plane Detection in ROS2

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 ROS2 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.

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 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.

Configure Rviz to display the results

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

Key parameters:

• Topic: The topic that contains the information relative to the detected planes: e.g. /zed2i/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 performances and compatibility.
• Reliability Policy: Set the QoS reliability policy. Best Effort is suggested for performances 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 zed_interfaces/PlaneStamped message is defined as:

# Standard Header
std_msgs/Header header

# Mesh of the place
shape_msgs/Mesh mesh

# Representation of a plane, using the plane equation ax + by + cz + d = 0
shape_msgs/Plane coefficients

# Normal vector
geometry_msgs/Point32 normal

# Center point 
geometry_msgs/Point32 center

# Plane pose relative to the global reference frame
geometry_msgs/Transform pose

# Width and height of the bounding rectangle around the plane contours
float32[2] extents

# The polygon bounds of the plane
geometry_msgs/Polygon bounds