The ROS2 ZED Node

The easiest way to start a ZED ROS2 node is to use the command line:

$ ros2 launch stereolabs_zed zed.py.launch

Published Topics

The ZED node publishes data to the following topics:

  • Left camera

    • zed/zed_node/rgb/image_rect_color: Color rectified image (left RGB image by default)
    • zed/zed_node/rgb/image_rect_color/camera_info: Color camera calibration data
    • zed/zed_node/rgb/image_raw_color: Color unrectified image (left RGB image by default)
    • zed/zed_node/rgb/image_raw_color/camera_info: Color camera calibration data
    • zed/zed_node/left/image_rect_color: Left camera color rectified image
    • zed/zed_node/left/image_rect_color/camera_info: Left camera calibration data
    • zed/zed_node/left/image_raw_color: Left camera color unrectified image
    • zed/zed_node/left/image_raw_color/camera_info: Left camera calibration data
  • Right camera

    • zed/zed_node/right/image_rect_color: Color rectified right image
    • zed/zed_node/right/image_rect_color/camera_info: Right camera calibration data
    • zed/zed_node/right/image_raw_color: Color unrectified right image
    • zed/zed_node/right/image_raw_color/camera_info: Right camera calibration data
  • Depth and point cloud

    • zed/zed_node/depth/depth_registered: Depth map image registered on left image (Normal mode - 32-bit float in meters)
    • zed/zed_node/depth/depth_registered/camera_info: Depth camera calibration data (Normal mode)
    • zed/zed_node/depth/depth_raw_registered: Depth map image registered on left image (OpenNI mode - 16-bit unsigned in millimeters)
    • zed/zed_node/depth/depth_raw_registered/camera_info: Depth camera calibration data (OpenNI mode)
    • zed/zed_node/point_cloud/cloud_registered: Registered color point cloud
    • zed/zed_node/confidence/confidence_image: Confidence image
    • zed/zed_node/confidence/confidence_image/camera_info: Depth camera calibration data
    • zed/zed_node/confidence/confidence_map: Confidence image (floating point values)
    • zed/zed_node/disparity/disparity_image: Disparity image
  • Inertial data

    • zed/zed_node/imu/data: Accelerometer, gyroscope, and orientation data in Earth frame
    • zed/zed_node/imu/data_raw: Accelerometer and gyroscope data in Earth frame
  • Lyfecycle transitions

    • zed/zed_node/transition_event: notification of lifecycle state changing (see the lifecycle tutorial)
  • Positional tracking

    • /zed/zed_node/pose: Absolute 3D position and orientation relative to the Map frame (Sensor Fusion algorithm + SLAM)
    • zed/zed_node/pose_with_covariance: Camera pose referred to Map frame with covariance
    • /zed/zed_node/odom: Absolute 3D position and orientation relative to the Odometry frame (pure visual odometry for ZED, visual-inertial odometry for ZED-M)
    • /zed/zed_node/map2odom: Current transform from the odometry frame to the map frame. Useful to publish the TF transform map -> odom
    • /zed/zed_node/path_pose: Sequence of camera poses in Map frame
    • /zed/zed_node/path_odom: Sequence of camera odometry poses in Map frame

The transition_event topic has default system settings.

Image Transport

The ROS wrapper supports the stack image_transport introduced with ROS2 Crystal Clemmys.

The rgb, left, right and depth topics are republished using the image_transport::CameraPublisher object, that correctly associates the sensor_msgs::msg::CameraInfo message to the relative sensor_msg::msg::Image message and creates the compressed image streams.

The prefix it_ is added to the root of each image stream to indicate that the images are published using image_transport (e.g. /zed/zed_node/it_rgb/image_rect_color/compressed).

QoS profiles

All the topics are configured to use the default ROS2 QoS profile:

  • History: KEEP_LAST
  • Depth: 10
  • Reliability: RELIABLE
  • Durability: VOLATILE

The QoS settings can be modified changing the relative parameters in the YAML files, as described below.

When creating a subscriber, be sure to use a compatible QOS profile according to the following tables:

Compatibility of QoS durability profiles:

Publisher Subscriber Connection Result
Volatile Volatile Yes Volatile
Volatile Transient local No -
Transient local Volatile Yes Volatile
Transient local Transient local Yes Transient local

Compatibility of QoS reliability profiles:

Publisher Subscriber Connection Result
Best effort Best effort Yes Best effort
Best effort Reliable No -
Reliable Best effort Yes Best effort
Reliable Reliable Yes Reliable

In order for a connection to be made, all of the policies that affect compatibility must be compatible. For instance, if a publisher-subscriber pair has compatible reliability QoS profiles, but incompatible durability QoS profiles, the connection will not be made.

Note: The names of the topics are not fixed, you can change them modifying the relative parameter, as explained below.

Configuration parameters

Specify your launch parameters in the common.yaml, zed.yaml and zedm.yaml files available in the folder zed_wrapper/config.

General parameters

Namespace: general

common.yaml

Parameter Description Value
svo_file Specify SVO filename string, commented by default to use a physical device
camera_timeout_sec Camera timeout (sec) if communication fails int, default ‘5’
camera_max_reconnect Reconnection temptatives after timeout, before shutting down int, default ‘5’
camera_reactivate Reactivate the camera after a “deactivate” + “clean up” + “configure” sequence due to a camera connection issue. Set it to false if there is an external node manager (see the lifecycle tutorial) bool, default false
camera_flip Flip the camera data if it is mounted upsidedown true, false
zed_id Select a ZED camera by its ID. IDs are assigned by Ubuntu. Useful when multiple cameras are connected. ID is ignored if an SVO path is specified int, default ‘0’
serial_number Select a ZED camera by its Serial Number int, default ‘0’
resolution Set the ZED camera resolution ‘0’: HD2K, ‘1’: HD1080, ‘2’: HD720, ‘3’: VGA
verbose Enable/disable the verbosity of the SDK true, false
mat_resize_factor* Image resize factor float [0.01,1.0]
frame_rate Set ZED camera video framerate int
gpu_id Select a GPU device for depth computation int, default ‘-1’ (best device found)
base_frame Frame_id of the frame that indicates the center of the camera string, default=‘zed_camera_center’
left_camera_frame Frame_id of the left camera string, default=‘zed_left_camera_frame’
left_camera_optical_frame Frame_id of the optics of the left camera string, default=‘zed_left_camera_optical_frame’
right_camera_frame Frame_id of the right camera string, default=‘zed_right_camera_frame’
right_camera_optical_frame Frame_id of the optics of the right camera string, default=‘zed_right_camera_optical_frame’

* Dynamic parameter

zed.yaml and zedm.yaml

Parameter Description Value
camera_model Type of Stereolabs camera 0: ZED, 1: ZED-M

Video parameters

Namespace: video

common.yaml

Parameter Description Value
auto_exposure* Enable the auto exposure true, false
exposure* Exposure value if auto-exposure is false int [0,100]
gain* Gain value if auto-exposure is false int [0,100]
rgb_topic_root** Root of the topic for the RGB image message string, default=rgb
left_topic_root** Root of the topic for the Left image message string, default=left
right_topic_root** Root of the topic for the Right image message string, default=right
qos_history How to queue messages ‘0’: KEEP_LAST - ‘1’: KEEP_ALL
qos_depth Size of the queue if KEEP_LAST int, default 10
qos_reliability Enable resending of not delivered message, if RELIABLE ‘0’: BEST_EFFORT - ‘1’: RELIABLE
qos_durability Enable storing of unread message if TRANSIENT_LOCAL ‘0’: TRANSIENT_LOCAL - ‘1’: VOLATILE

* Dynamic parameter

** The topics will be automatically created using the following rules:

  • image raw: /namespace/node_name/topic_root/image_raw_color
  • image raw camera info: /namespace/node_name/topic_root/image_raw_color/camera_info
  • image rectified: ** /namespace/node_name/topic_root/image_rect_color
  • image rectified camera info: /namespace/node_name/topic_root/image_rect_color/camera_info

Depth parameters

Namespace: depth

common.yaml

Parameter Description Value
quality Select depth map quality ‘0’: NONE, ‘1’: PERFORMANCE, ‘2’: MEDIUM, ‘3’: QUALITY, ‘4’: ULTRA
sensing_mode Select depth sensing mode ‘0’: STANDARD, ‘1’: FILL
confidence* Confidence threshold (lower values = more filtering) int [0,100]
depth_stabilization Enable depth stabilization. Stabilizing the depth requires an additional computation load as it enables tracking 0: disabled, 1: enabled
openni_depth_mode Enable/disable the depth OpenNI format (16 bit, millimeters) int, [0,1], default: 0
depth_topic_root** Root of the topic for the depth image message string, default=depth
point_cloud_topic Topic for the pointcloud message string, default=point_cloud/cloud_registered
disparity_topic Topic for the disparity message string, default=disparity/disparity_image
confidence_topic_root Root of the topic for the confidence message string, default=confidence
confidence_img_topic Topic for the confidence image message (Visualization) string, default=confidence_image
confidence_map_topic Topic for the confidence map message (Elaboration) string, default=confidence_map
qos_history How to queue messages ‘0’: KEEP_LAST - ‘1’: KEEP_ALL
qos_depth Size of the queue if KEEP_LAST int, default 10
qos_reliability Enable resending of not delivered message, if RELIABLE ‘0’: BEST_EFFORT - ‘1’: RELIABLE
qos_durability Enable storing of unread message if TRANSIENT_LOCAL ‘0’: TRANSIENT_LOCAL - ‘1’: VOLATILE

Note: To disable depth calculation set quality to NONE. All the other parameters relative to depth will be ignored and only the video streams and inertial data (if using a ZED-M) will be available

* Dynamic parameter

** The topics will be automatically created using the following rules:

  • depth (no OpenNI mode): /namespace/node_name/topic_root/depth_registered
  • depth camera info (no OpenNI mode): /namespace/node_name/topic_root/depth_registered/camera_info
  • depth (OpenNI mode): /namespace/node_name/topic_root/depth_raw_registered
  • depth camera info (OpenNI mode): /namespace/node_name/topic_root/depth_raw_registered/camera_info
  • confidence image: /namespace/node_name/topic_root/confidence_img_topic
  • confidence image camera info: /namespace/node_name/topic_root/confidence_img_topic/camera_info
  • confidence map: /namespace/node_name/topic_root/confidence_map_topic
Parameter Description Value
min_depth Minimum depth value (from the camera) that will be computed double, min: 0.3 (ZED) / 0.1 (ZED M), max 3.0
max_depth* Maximum range allowed for depth. Values beyond this limit will be reported as TOO_FAR float, ]0.0,20.0]

* Dynamic parameter

### Positional tracking parameters Namespace: traking

common.yaml

Parameter Description Value
publish_tf Enable publishing of transform odom -> base_link and all the other transforms true, false
publish_map_tf Enable publishing of transform map -> odom. Only valid if publish_tf is true true, false
world_frame Static world frame name string, default=map
pose_frame Frame_id of the pose message string, default=map
odometry_frame Frame_id of the odom message string, default=odom
odometry_db The full path of the space memory DB string, FUTURE USE
pose_smoothing Enable smooth pose correction for small drift correction true, false - Enable only if making an AR application
spatial_memory Enable Loop Closing algorithm true, false
floor_alignment Indicates if the floor must be used as origin for height measures true, false
two_d_mode Enable if robot moves on a planar surface. Forces Z to fixed_z_value, roll and pitch to zero true, false
fixed_z_value Fixed value assigned to Z coordinate if two_d_mode is true double
initial_tracking_pose Initial reference pose. Useful if the base_link does not start from the origin vector, default=’[0.0,0.0,0.0, 0.0,0.0,0.0]’ -> [X, Y, Z, R, P, Y]
pose_topic Topic for the camera pose message string, default=‘pose’
odometry_topic Topic for the camera odometry message string, default=‘odom’
init_odom_with_first_valid_pose Indicates if the odometry must be initialized with the first valid pose received by the tracking algorithm true, false
path_pub_rate Frequency (Hz) of publishing of the path messages double, default=2.0
path_max_count Maximum number of poses kept in the pose arrays (-1 for infinite) int, default=-1
publish_pose_covariance Enable publishing of the pose/odom covariance matrices true, false
qos_history How to queue messages ‘0’: KEEP_LAST - ‘1’: KEEP_ALL
qos_depth Size of the queue if KEEP_LAST int, default 10
qos_reliability Enable resending of not delivered message, if RELIABLE ‘0’: BEST_EFFORT - ‘1’: RELIABLE
qos_durability Enable storing of unread message if TRANSIENT_LOCAL ‘0’: TRANSIENT_LOCAL - ‘1’: VOLATILE

IMU parameters (ZED-M only)

Namespace: imu

zedm.yaml

Parameter Description Value
imu_frame Frame_id of the IMU sensor string, default=zed_imu_link
imu_topic Topic for the IMU data (with orientation) string, default=imu/data
imu_raw_topic Topic for the IMU raw data (without orientation) string, default=imu/data_raw
imu_pub_rate Frequency of IMU data (Hz) float, default=500.0
imu_sync_frame Specify if the messages between two consecutive frames will keep the timestamp of the last frame (true) or the real time (false) true, false
qos_history How to queue messages ‘0’: KEEP_LAST - ‘1’: KEEP_ALL
qos_depth Size of the queue if KEEP_LAST int, default 10
qos_reliability Enable resending of not delivered message, if RELIABLE ‘0’: BEST_EFFORT - ‘1’: RELIABLE
qos_durability Enable storing of unread message if TRANSIENT_LOCAL ‘0’: TRANSIENT_LOCAL - ‘1’: VOLATILE

Dynamic parameters

The ZED node lets you reconfigure these parameters dynamically during the execution of the node:

  • general.mat_resize_factor: Sets the scale factor of the output images and depth map. Note that the camera will acquire data at the dimension set by the resolution parameter; images are resized before being sent to the user
  • video.auto_exposure: Enables/disables automatic gain and exposure
  • video.exposure: Sets camera exposure only if auto_exposure is false
  • video.gain: Set camera gain only if auto_exposure is false
  • depth.confidence: Sets a threshold that filters the values of the depth or the point cloud. With a confidence threshold set to 100, all depth values will be written in the depth and the point cloud. This is set to 80 by default, which removes the least accurate values.
  • depth.max_depth: Sets the maximum depth range

You can set the parameters using the CLI command ros2 param set, e.g.:

$ ros2 param set /zed/zed_node depth.confidence 80

if the parameter is set successfully, you will get a confirmation message:

Set parameter successful

In the case you tried to set a parameter that’s not dynamically reconfigurable, or you specified an invalid value, you will get this error:

$ ros2 param set /zed/zed_node depth.confidence 150
Set parameter failed

and the ZED node will report a warning message explaining the error type:

1538556595.265117561: [zed.zed_node] [WARN]	The param 'depth.confidence' requires an INTEGER value in the range ]0,100]

Transform frame

If executed using the zed.launch.py or the zed_unmanaged.launch.py (which also launches the robot_state_publisher node), the ZED ROS wrapper broadcasts multiple coordinate frames that each provide information about the camera’s position and orientation. If needed, the reference frame names can be changed in the configuration files as described above.

  • map is the fixed frame
  • odom is the odometry frame
  • base_link is the position of the base of the ZED camera. Useful if using a tripod or similar
  • zed_camera_center is the reference point of the ZED camera, in the middle of the two camera centers
  • zed_right_camera is the position and orientation of the ZED’s right camera
  • zed_right_camera_optical is the position and orientation of the ZED’s right camera optical frame
  • zed_left_camera is the position and orientation of the ZED’s left camera
  • zed_left_camera_optical is the position and orientation of the ZED’s left camera optical frame
  • imu_link is the origin of the inertial data frame (ZED-M only)

The TF tree generated by the zed_wrapper reflects the REP105 standard.

  pose_frame (`map`)
    └─odometry_frame (`odom`)
      └─base_frame (`base_link`)
          └─camera_frame (`zed_camera_center`)
          |  └─left_camera_frame (`zed_left_camera_frame`)
          |  |      └─left_camera_optical_frame (`zed_left_camera_optical_frame`)
          |  └─right_camera_frame (`zed_right_camera_frame`)
          |        └─right_camera_optical_frame (`zed_right_camera_optical_frame`)
          └─imu_frame (`imu_link`) (*only ZED-M*)

ZED-M

The ZED-M provides the same information as the ZED, plus inertial data from its IMU sensor.

Services

The ZED node provides the following services:

  • set_pose: Restarts the Tracking algorithm setting the initial pose of the camera to the value passed as vector parameter -> [X, Y, Z, R, P, Y]
  • reset_tracking: Restarts the Tracking algorithm setting the initial pose to the value available in the param server or to the latest pose set with the service set_pose
  • reset_odometry: Resets the odometry values eliminating the drift due to the Visual Odometry algorithm. The new odometry value is set to the latest camera pose received from the tracking algorithm
  • start_svo_recording: Start recording an SVO file. If no filename is provided the default zed.svo is used. If no path is provided with the filename the default recording folder is ~/.ros/
  • stop_svo_recording: Stop an active SVO recording.

Note: Currently the H26x SVO compression uses the resolution and the real FPS to calculate the bitrate. This feature can lead to some issues of quality when FPS is low. On Nvidia Jetson TX1 and Jetson TX2, the FPS is quite low at the beginning of the grab loop, therefore it could be better to wait for some grab calls before calling start_svo_recording so that the camera FPS is stabilized at the requested value (15fps or more).

Assigning a GPU to a camera

To improve performance, you can specify the gpu_id of the graphic card that will be used for the depth computation in the common.yaml configuration file. The default value (-1) will select the GPU with the highest number of CUDA cores. When using multiple ZEDs, you can assign each camera to a GPU to increase performance.

Limitations

Performance

This wrapper lets you quickly prototype applications and interface the ZED with other sensors and packages available in ROS. However, the ROS layer introduces significant latency and a performance hit. If performance is a major concern for your application, please consider using the ZED SDK library.

Multiple ZEDs

In the folder examples/multi_camera an example project is available to demonstrate how to launch two different nodes to acquire data from a ZED and a ZED-M connected to the same PC.

To execute the two nodes you can use the command: $ ros2 launch stereolabs_example_multi_camera zed_multi_camera.launch.py

and visualize the data using RVIZ2: $ rviz2 -d <ros2_workspace_fullpath>/src/zed-ros2-wrapper/examples/multi_camera/rviz/rviz_multi_camera.rviz

The ZED camera uses the maximum bandwidth provided by USB 3.0 to output video. When using multiple ZEDs, you may need to reduce camera framerate and resolution to avoid corrupted (noisy green or purple) frames. You can also use multiple GPUs to load-balance computations and improve performance.