ROS 2 - ZED Mono Node

The easiest way to start a ZED ROS 2 node for a monocular camera is by using the command line:

ros2 launch zed_wrapper zed_camera.launch.py camera_model:=<camera model>

where <camera model> must be replaced with one in the list zedxonegs, and zedxone4k

Published Topics #

The ZED node publishes data to the following topics:

  • Image

    • `~/rgb/raw/camera_info
    • ~/rgb/raw/image
    • ~/rgb/rect/camera_info
    • ~/rgb/rect/image
  • Sensors data

    • ~/imu/data
    • ~/imu/data_raw
    • ~/temperature
    • ~/left_cam_imu_transform

Image Transport #

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

All the image topics are published 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 relative compressed image streams.

QoS profiles #

All the topics are configured by default to use the following ROS 2 QoS profile:

  • Reliability: RELIABLE
  • History (Depth): KEEP_LAST (10)
  • Durability: VOLATILE
  • Lifespan: Infinite
  • Deadline: Infinite
  • Liveliness: AUTOMATIC
  • Liveliness lease duration: Infinite

📌 Note: Read the official ROS 2 documentation about the QoS and the relatived settings for details.

The QoS settings can be modified by changing the relative parameters in the YAML files, as described in this official ROS 2 design document.

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

Compatibility of QoS durability profiles:

PublisherSubscriberConnectionResult
VolatileVolatileYesVolatile
VolatileTransient localNo-
Transient localVolatileYesVolatile
Transient localTransient localYesTransient local

Compatibility of QoS reliability profiles:

PublisherSubscriberConnectionResult
Best effortBest effortYesBest effort
Best effortReliableNo-
ReliableBest effortYesBest effort
ReliableReliableYesReliable

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: for a detailed list of all the compatibility settings, please refer to the official ROS 2 documentation.

Configuration parameters #

Specify your launch parameters in the common_mono.yaml, zedxonegs.yaml, and zedxone4k.yaml files available in the folder zed_wrapper/config.

General parameters #

Namespace: general

common_mono.yaml

ParameterDescriptionValue
camera_timeout_secCamera timeout (sec) if communication failsint
serial_numberSelect a ZED camera by its Serial Numberint
pub_resolutionThe resolution used for output. ‘NATIVE’ to use the same general.grab_resolution - CUSTOM to apply the general.pub_downscale_factor downscale factory to reduce bandwidth in transmissionstring,‘NATIVE’‘SVGA’, ‘VGA’, ‘LOW’
pub_downscale_factorRescale factor used to rescale image before publishing when ‘pub_resolution’ is ‘CUSTOM’double
gpu_idSelect a GPU device for depth computationint
optional_opencv_calibration_fileOptional path where the ZED SDK can find a file containing the calibration information of the camera computed by OpenCV.string

* Dynamic parameter

zedxonegs.yaml, and zedxone4k.yaml

ParameterDescriptionValue
camera_modelType of Stereolabs camerastring, zed, zedm, zed2, zed2i, zedx, zedxm, virtual
camera_nameUser name for the camera, can be different from camera modelstring
grab_resolutionThe native camera grab resolution.string, ‘HD1200’, ‘QHDPLUS’, ‘HD1080’, ‘SVGA’, ‘AUTO’
grab_frame_rateZED SDK internal grabbing rateint, ‘15’,‘30’,‘60’,‘90’,‘100’,‘120’

Streaming server parameters #

Namespace: stream_server

common_mono.yaml

ParameterDescriptionValue
stream_enabledenable the streaming server when the camera is opentrue, false
codecifferent encoding types for image streaming: ‘H264’, ‘H265’string
portPort used for streaming. Port must be an even number. Any odd number will be rejected.int
bitrateStreaming bitrate (in Kbits/s) used for streaming. See docint, [1000 - 60000]
gop_sizeThe GOP size determines the maximum distance between IDR/I-frames. Very high GOP size will result in slightly more efficient compression, especially on static scenes. But latency will increase.int, [max 256]
adaptative_bitrateBitrate will be adjusted depending the number of packet dropped during streaming. If activated, the bitrate can vary between [bitrate/4, bitrate].true, false
chunk_sizeStream buffers are divided into X number of chunks where each chunk is chunk_size bytes long. You can lower chunk_size value if network generates a lot of packet lost: this will generates more chunk for a single image, but each chunk sent will be lighter to avoid inside-chunk corruption. Increasing this value can decrease latency.int, [1024 - 65000]
target_framerateFramerate for the streaming output. This framerate must be below or equal to the camera framerate. Allowed framerates are 15, 30, 60 or 100 if possible. Any other values will be discarded and camera FPS will be taken.int

Video parameters #

Namespace: video

common_mono.yaml

ParameterDescriptionValue
saturation*Image saturationint, [0,8]
sharpness*Image sharpnessint, [0,8]
gamma*Image gammaint, [0,8]
auto_whitebalance*Enable the auto white balancetrue, false
whitebalance_temperature*White balance temperatureint [28,65]
exposure_time*Defines the real exposure time in microseconds. Recommended to control manual exposure (instead of video.exposure setting)int, [28,30000]
auto_exposure_time_range_min*Defines the minimum range of exposure auto control in micro secondsint
auto_exposure_time_range_max*Defines the maximum range of exposure auto control in micro secondsint
exposure_compensation*Defines the Exposure-target compensation made after auto exposure. Reduces the overall illumination target by factor of F-stops.int, [0 - 100]
analog_gain*Defines the real analog gain (sensor) in mDB. Recommended to control manual sensor gain (instead of video.gain setting)int, [1000-16000].
auto_analog_gain_range_min*Defines the minimum range of sensor gain in automatic controlint
auto_analog_gain_range_max*Defines the maximum range of sensor gain in automatic controlint
digital_gain*Defines the real digital gain (ISP) as a factor. Recommended to control manual ISP gain (instead of video.gain setting)int, [1-256]
auto_digital_gain_range_min*Defines the minimum range of digital ISP gain in automatic controlint
auto_digital_gain_range_max*Defines the maximum range of digital ISP gain in automatic controlint
denoising*Defines the level of denoising applied on both left and right imagesint, [0-100]

* Dynamic parameter

zedxone4k.yaml

ParameterDescriptionValue
enable_hdrWhen set to true, the camera will be set in HDR mode if the camera model and resolution allows ittrue, false

* Dynamic parameter

Debug parameters #

Namespace: debug

common_mono.yaml

ParameterDescriptionValue
sdk_verboseSet the verbose level of the ZED SDKint, 0 -> disable
debug_commonEnable general debug log outputstrue, false
debug_video_depthEnable video/depth debug log outputstrue, false
debug_camera_controlsEnable camera controls debug log outputstrue, false
debug_sensorsEnable sensors debug log outputstrue, false
debug_streamingEnable streaming debug log outputstrue, false
debug_advancedEnable advanced debug log outputstrue, false

Dynamic parameters #

The ZED node lets you reconfigure many parameters dynamically during the execution of the node. All the dynamic parameters are indicated with a * in the list above and with the tag [DYNAMIC] in the comments of the YAML files.

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

ros2 param set /zed/zed_node video.exposure_time 5000

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 type of error:

ros2 param set /zed/zed_node video.exposure_time 0
Setting parameter failed: Parameter {video.exposure_time} doesn't comply with integer range.

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

You can also use the Configuration -> Dynamic Reconfigure plugin of the rqt tool to dynamically set parameters by using a graphic interface.

Transform frames #

The ZED ROS 2 wrapper broadcasts multiple coordinate frames that each provides information about the camera’s position and orientation. If needed, the reference frames can be changed in the launch file.

  • <camera_name>_camera_link is the current position and orientation of the ZED base center. It corresponds to the bottom central fixing hole.
  • <camera_name>_camera_center is the current position and orientation of ZED middle baseline, determined by visual odometry and the tracking module.
  • <camera_name>_camera_frame is the position and orientation of the ZED’s CMOS sensor.
  • <camera_name>_camera_optical_frame is the position and orientation of the ZED’s camera optical frame.
  • <camera_name>_imu_link is the origin of the inertial data frame (not available with ZED).

Frames

Services #

The ZED node provides the following services:

  • ~/enable_streaming: enable/disable a local streaming server.

Services can be called using the rqt graphical tool by enabling the plugin Plugins -> Services -> Service caller.

It is possible to call a service also by using the CLI command ros2 service call.

For example to start Object Detection for the ZED2 camera:

ros2 service call /zed/zed_node/enable_streaming std_srvs/srv/SetBool "{data: True}"

and the service server will reply:

requester: making request: std_srvs.srv.SetBool_Request(data=True)

response:
std_srvs.srv.SetBool_Response(success=True, message='Streaming Server started')

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