ZED Nodelets

The zed_wrapper package contains the standalone ZED Wrapper node that can be started as is using the provided launch files, as described in the ZED Wrapper Node documentation.

But the ZED Wrapper has been designed to take the maximum advantage of the nodelet package, in order to run multiple algorithms in the same process with zero copy transport between algorithms.

The core of the ZED Wrapper is indeed the ZEDWrapperNodelet nodelet, available in the zed_nodelets package that provides the interface between the ZED SDK and the ROS environment.

Starting a ZED nodelet

A nodelet manager is required to start the ZEDWrapperNodelet. It can be started manually from command line:

$ rosrun nodelet nodelet manager __name:=zed_nodelet_manager

now the ZEDWrapperNodelet nodelet can be loaded inside the new zed_nodelet_manager nodelet manager:

$ rosrun nodelet nodelet load zed_nodelets/ZEDWrapperNodelet zed_nodelet_manager __name:=zed_nodelet

The nodelet will work exactly like a standalone ZED node, we can use indeed the command rosnode list to verify that it is correctly running together with the zed_nodelet_manager:

$ rosnode list
/rosout
/zed_nodelet
/zed_nodelet_manager

It is also possible to start the ZEDWrapperNodelet nodelet as a standalone node, but losing all the advantages of a nodelet that will be shown next:

$ rosrun nodelet nodelet standalone zed_nodelets/ZEDWrapperNodelet

Note: this is exactly what’s done internally by the ZED Node, using the nodelet C++ APIs.

ZED nodelet parameters

The ZEDWrapperNodelet nodelet started using one of the two methods illustrated above is configured using the default parameters. We provide a launch file that allows to load custom parameters using the YAML files described in the ZED Wrapper Node documentation:

First start a nodelet manager:

$ rosrun nodelet nodelet manager __name:=zed_nodelet_manager

then launch the nodelet:

ZED:

roslaunch zed_wrapper zed_camera_nodelet.launch camera_model:=zed

ZED Mini:

roslaunch zed_wrapper zed_camera_nodelet.launch camera_model:=zedm

ZED2:

roslaunch zed_wrapper zed_camera_nodelet.launch camera_model:=zed2

The power of nodelets

To really perceive the power of the nodelet package it is necessary to load more nodelets inside the same nodelet manager, that is inside the same process. In this way the different nodelets can exploit the advantages of the intra process communication that allows zero copy passing of data between nodelets.

Why is this important? The ZED nodelet publishes images and pointclouds at high frequency. Subscribing to these kind of topics using the standard node serialized communication requires a lot of memory copies that generate latency and requires more computational power. Instead, using zero-copy, no memory copy is required as the memory is simply shared between the nodelets that are contained in the same nodelet manager process. Furthermore intra-process communication allows to reduce the network traffic for nodes running on the same machine.

Examples

To demonstrate how to use the ZED nodelet exploiting the power of nodelets we provide two examples:

  • topics synchronization and mux
  • depth image to 2D laser scan

Topics synchronization

Sometimes it is useful to synchronize and mux a few topics in one single topic using their timestamp, for example to save a rosbag or to guarantee that all the topics published in the same instant are received on the other side of a network.

To perform this operation we provide the nodelet zed_nodelets/RgbdSensorsSyncNodelet that allows to synchronize RGB, Depth, IMU and Magnetometer information in a single custom topic RGBDSensors.

Custom topic: RGBDSensors

The custom topic RGBDSensors is defined in the package zed_interface as:

# Header
Header header

# CameraInfo
sensor_msgs/CameraInfo rgbCameraInfo
sensor_msgs/CameraInfo depthCameraInfo

# Raw
sensor_msgs/Image rgb
sensor_msgs/Image depth

# IMU
sensor_msgs/Imu imu

# Magnetometer
sensor_msgs/MagneticField mag

the topic allows to publish visual/depth information (RGBD) synchronized with inertial and orientation data

Parameters

The nodelet can be configured using the follow parameters, the file sync.yaml with the definition of all the parameters and their default values is provided in the zed_wrapper package:

Parameter Description Value
zed_nodelet_name The name of the ZED nodelet publishing the topics to be synchronized string,default=zed_nodelet
approx_sync Use approximate synchronization for the input topics. If false all the message must have the same timestamp, this is almost impossible if subscribing also to IMU and Magnetometer topics and the parameter sensors_timestamp_sync is false in the ZED nodelet bool, default=true
queue_size Size of message queue for each synchronized topic int, default=600
sub_imu Synchronize IMU messages bool, default=true
sub_mag Synchronize Magnetometer messages bool, default=true

Start the nodelet

You can start the RgbdSensorsSyncNodelet nodelet and the ZEDWrapperNodelet nodelet in the same nodelet manager easily using the zed_sync_nodelet.launch launch file provided in the zed_nodelet_example package:

ZED:

$ roslaunch zed_nodelet_example zed_sync_nodelet.launch camera_model:=zed sub_imu:=false sub_mag:=false

Note: The ZED camera does not provide inertial and magnetometer information, so we must disable the subscription to these topics

ZED Mini:

$ roslaunch zed_nodelet_example zed_sync_nodelet.launch camera_model:=zedm sub_mag:=false

Note: The ZED Mini camera does not provide magnetometer information, so we must disable the subscription to this topic

ZED2:

$ roslaunch zed_nodelet_example zed_sync_nodelet.launch camera_model:=zed2

The launch file explained

A brief description of the main part of the zed_sync_nodelet.launch file.

Setting the name of the nodelet manager using the camera name as prefix:

<!-- Name of the Nodelet Manager -->
arg name="nodelet_manager_name"  default="$(arg camera_name)_nodelet_manager" />

The nodelet manager, the RgbdSensorsSyncNodelet nodelet and the ZEDWrapperNodelet nodelet are included in the same namespace that takes the name from the camera name parameter:

<group ns="$(arg camera_name)">
  [...]
</group>

Starting the nodelet manager:

<!-- Nodelet Manager -->
        <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)"  args="manager" output="screen" />

Loading the ZEDWrapperNodelet nodelet:

<!-- Load ZED wrapper nodelet -->
<include file="$(find zed_wrapper)/launch/include/zed_camera_nodelet.launch">

Loading the RgbdSensorsSyncNodelet nodelet:

<node pkg="nodelet" type="nodelet" name="$(arg node_name)_sync" args="load zed_nodelets/RgbdSensorsSyncNodelet $(arg nodelet_manager_name)" output="screen">
    <rosparam file="$(find zed_wrapper)/params/sync.yaml" command="load" />

    <!-- ZED Nodelet Name -->
    <param name="zed_nodelet_name"                 value="$(arg node_name)" />
</node>

The RgbdSensorsSyncNodelet nodelet loads the parameter directly from the sync.yaml file as described above, but the zed_nodelet_name parameter value is overwritten to take automatically the name set for the ZEDWrapperNodelet nodelet that publishes the topics to be synchronized.

Demux the topics

A RGBDSensors topic message cannot be used by the standard ROS node. You must create your own node that subscribes it and extract all the available data.

To make it easier to access the information stored in the RGBDSensors topic, we provide a demux nodelet: RgbdSensorsDemuxNodelet.

You can start the demux nodelet as a standalone node using the following command:

$ rosrun nodelet nodelet standalone zed_nodelets/RgbdSensorsDexNodelet __name:=demux_nodelet rgbd_sens:=<full_name_of_the_topic_to_be_subscribed>

or you can use it inside a nodelet manager to exploit zero-copy, if you have other nodelets that can be combined together to process the provided data.

Note: The RgbdSensorsDemuxNodelet subscribes to a topic called rgbd_sens. It is important to remap the name of this topic to correctly match the name of the available topic to demux, i.e. /zed/zed_nodelet_sync/rgbd_sens.

Depth image to 2D laser scan

Most of the mapping packages included in ROS require 2D laser scan information to work (see gmapping, cartographer, amcl). The easier way to generate virtual 2D laser scan from 3D information is to use the depthimage_to_laserscan package and the provided DepthImageToLaserScanNodelet nodelet.

Start the nodelet

We provide the zed_laserscan_nodelet.launch launch file in the zed_nodelet_example package to easily start the ZEDWrapperNodelet and the DepthImageToLaserScanNodelet in the same nodelet manager:

ZED:

$ roslaunch zed_nodelet_example zed_laserscan_nodelet.launch camera_model:=zed

ZED Mini:

$ roslaunch zed_nodelet_example zed_laserscan_nodelet.launch camera_model:=zedm

ZED2:

$ roslaunch zed_nodelet_example zed_laserscan_nodelet.launch camera_model:=zed2

The launch file explained

A brief description of the main part of the zed_laserscan_nodelet.launch file.

Setting the name of the nodelet manager using the camera name as prefix:

<!-- Name of the Nodelet Manager -->
<arg name="nodelet_manager_name"  default="$(arg camera_name)_nodelet_manager" />

The nodelet manager, the DepthImageToLaserScanNodelet nodelet and the ZEDWrapperNodelet nodelet are included in the same namespace that takes the name from the camera name parameter:

<group ns="$(arg camera_name)">
  [...]
</group>

Starting the nodelet manager:

<!-- Nodelet Manager -->
<node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)"  args="manager" output="screen" />

Loading the ZEDWrapperNodelet nodelet:

<!-- Load ZED wrapper nodelet -->
<include file="$(find zed_wrapper)/launch/include/zed_camera_nodelet.launch">

Loading the `` nodelet:

<!-- Virtual laser scan as nodelet -->
<!-- "$ sudo apt install ros-kinetic-depthimage-to-laserscan" -->
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan" args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet $(arg nodelet_manager_name)">
      <param name="scan_height" value="10"/>
      <param name="output_frame_id" value="$(arg camera_name)_left_camera_frame"/>
      <param name="range_min" value="0.1"/>
      <remap from="image" to="$(arg zed_nodelet_name)/depth/depth_registered"/>
</node>

The DepthImageToLaserScanNodelet requires the following parameters to be set to correctly work:

  • scan_height: The number of pixel rows to use to generate the laserscan. For each column, the scan will return the minimum value for those pixels centered vertically in the image.
  • output_frame_id: The frame id of the laser scan. For point clouds coming from an “optical” frame with Z forward, this value should be set to the corresponding frame with X forward and Z up.
  • range_min: The minimum ranges to return in meters. Ranges less than this will be output as -Inf.

Finally we must remap the input topic image so DepthImageToLaserScanNodelet can subscribe to the correct sensor_msgs/Image topic providing depth information: depth/depth_registered, with the ZED nodelet name as prefix.

Virtual 2D laser scan