Skip to content

Depth Camera Payload - SW Integration

Introduction

Depth Camera Payload uses USB-C cable to connect to the Vertex One and selected ROS commands of the Avular Programming Interface can be used to control the module and read data from it.

Intel RealSense Documentation

This section is based on information from Intel RealSense Development Documentation (External Link).

Avular's Software Implementation

The RealSense is accessible through our own docker container. This docker container is converting the RealSense data into the ROS 2 network. This implementation is based on Intel's ROS 2 wrapper (External Link).

Set-up for use on Vertex One

By default, the docker container is started and if the RealSense is connected it will start to automatically publish data on the ROS 2 network, unless container mode is set to Development.

RealSense Sensor Overview

Sensor overview

Sensor Parameters

Each sensor has a unique set of parameters.

  • Video sensors, such as depth_module or rgb_camera have, at least, the profile parameter.
  • The profile parameter is a string of the following format: <width>X<height>X<fps> (The dividing character can be "X", "x" or "," while spaces are ignored.)
  • For example: depth_module.profile:=640x480x30
  • The depth_module streams 3 different data streams: infra1, infra2 and depth. Their parameters, such as width, height and fps are defined by their common sensor.

Note

If the specified combination of parameters is not available by the device, the default configuration will be used.

Available Parameters, Read/Set Parameter

  • For the entire list of parameters type ros2 param list.
  • For reading a parameter value use ros2 param get <node> <parameter_name>
  • For example: ros2 param get /camera/camera depth_module.emitter_on_off
  • For setting a new value for a parameter use ros2 param set <node> <parameter_name> <value>
  • For example: ros2 param set /camera/camera depth_module.emitter_on_off true

Parameters that CAN be Modified During Runtime

enable_<stream_name>

Choose whether to enable a specified stream or not. Default is true for images and false for orientation streams.

  • <stream_name> can be any of infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose.
  • For example: enable_infra1:=true enable_color:=false

enable_sync

Gathers closest frames of different sensors, infra-red, color and depth, to be sent with the same timetag - This happens automatically when such filters as pointcloud are enabled.

<stream_type>_qos

Sets the QoS by which the topic is published. Reference: ROS 2 QoS profiles formal documentation (External Link).

  • <stream_type> can be any of infra, color, fisheye, depth, gyro, accel, pose.
  • Available values are the following strings: SYSTEM_DEFAULT, DEFAULT, PARAMETER_EVENTS, SERVICES_DEFAULT, PARAMETERS, SENSOR_DATA.
  • For example: depth_qos:=SENSOR_DATA

Note

<stream_type>_info_qos refers to both camera_info topics and metadata topics.

tf_publish_rate

  • Double, rate (in Hz) at which dynamic transforms are published
  • Default value is 0.0 Hz (means no dynamic TF)
  • This parameter also depends on publish_tf parameter
    • If publish_tf:=false, then no TFs will be published, even if tf_publish_rate is >0.0 Hz
    • If publish_tf:=true and tf_publish_rate set to >0.0 Hz, then dynamic TFs will be published at the specified rate

Parameters that CANNOT be Modified During Runtime

serial_no

Will attach to the device with the given serial number (serial_no) number.

  • Default, attach to the first (in an inner list) RealSense device.
  • Note: Serial number should be defined with "_" prefix (This is a workaround until a better method will be found to ROS 2's auto conversion of strings containing only digits into integers.)
  • For Example: Serial number 831612073525 can be set in command line as serial_no:=_831612073525.

usb_port_id

Will attach to the device with the given USB port (usb_port_id).

  • Default, ignore USB port when choosing a device.
  • For example: usb_port_id:=4-1 or usb_port_id:=4-2

device_type

will attach to a device whose name includes the given device_type regular expression pattern.

  • Default, ignore device type.
  • For example:
    • device_type:=d435 will match d435 and d435i.
    • device_type=d435(?!i) will match d435 but not d435i.

reconnect_timeout

When the driver cannot connect to the device try to reconnect after this timeout (in seconds). - For Example: reconnect_timeout:=10

wait_for_device_timeout

  • If the specified device is not found, will wait wait_for_device_timeout seconds before exits.
  • Defualt, wait_for_device_timeout < 0, will wait indefinitely.
  • For example: wait_for_device_timeout:=60

rosbag_filename

Publish topics from rosbag file. There are two ways for loading rosbag file:

  • Command line - ros2 run realsense2_camera realsense2_camera_node -p rosbag_filename:="/full/path/to/rosbag.bag"
  • Launch file - set rosbag_filename parameter with rosbag full path (see realsense2_camera/launch/rs_launch.py as reference)

initial_reset

On occasions the device was not closed properly and due to firmware issues needs to reset.

  • If set to true, the device will reset prior to usage.
  • For example: initial_reset:=true

frame_id

Specify the different frame_id for the different frames.

  • _frame_id

  • _optical_frame_id

  • aligned_depth_to__frame_id

This is especially important when using multiple cameras.

base_frame_id

Defines the frame_id all static transformations refers to.

odom_frame_id

Defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system.

unite_imu_method

D400 cameras have built in IMU components which produce 2 unrelated streams, each with its own frequency:

  • gyro - shows angular velocity
  • accel - shows linear acceleration

By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out.

Setting unite_imu_method creates a new topic, imu, that replaces the default gyro and accel topics.

  • The imu topic is published at the frequency of the gyro.
  • All the fields of the IMU message under the imu topic are filled out.
  • unite_imu_method parameter supported values are [0-2] meaning: [0 -> None, 1 -> Copy, 2 -> Linear_ interpolation] when:
    • linear_interpolation - very gyro message is attached by the an accel message interpolated to the gyro's timestamp.
    • copy - very gyro message is attached by the last accel message.

clip_distance

Removes from the depth image all values above a given value (meters). Disable by giving a negative value (default).

  • For example: clip_distance:=1.5

linear_accel_cov, angular_velocity_cov

Sets the variance given to the IMU readings.

hold_back_imu_for_frames

Images processing takes time. Therefore, there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, IMU messages keep on arriving and a situation is created where an image with earlier timestamp is published after IMU message with later timestamp.

If that is a problem, setting hold_back_imu_for_frames to true will hold the IMU messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin.

publish_tf

Boolean, enable/disable publishing static and dynamic TFs. - Default set to true - So, static TFs will be published by default - If dynamic TFs are needed, user should set the parameter tf_publish_rate to >0.0 Hz - If set to false, both static and dynamic TFs won't be published, even if the parameter tf_publish_rate is set to >0.0 Hz

diagnostics_period

  • Double, positive values set the period between diagnostics updates on the /diagnostics topic.
  • 0 or negative values mean no diagnostics topic is published. Default set to 0.

Note

The /diagnostics topic includes information regarding the device temperatures and actual frequency of the enabled streams.

publish_odom_tf

If True (default) publish TF from odom_frame to pose_frame.

ROS 2 (Robot) vs Optical (Camera) Coordination Systems

Point Of View:

  • Imagine we are standing behind of the camera, and looking forward.
  • Always use this point of view when talking about coordinates, left vs right IRs, position of sensor, etc..

image

  • ROS 2 Coordinate System: (X: Forward, Y:Left, Z: Up), References: REP-0103 (External Link), REP-0105 (External Link)
  • Camera Optical Coordinate System: (X: Right, Y: Down, Z: Forward)
  • All data published in Intel's ROS wrapper topics is optical data taken directly from RealSense camera sensors.

Note

Static and dynamic TF topics publish optical CS and ROS CS to give the user the ability to move from one CS to other CS.

TF from Coordinate A to Coordinate B

TF msg expresses a transform from coordinate frame header.frame_id (source) to the coordinate frame child_frame_id (destination), see reference (External Link).

  • In RealSense cameras, the origin point (0,0,0) is taken from the left IR (infra1) position and named as camera_link frame
  • Depth, left IR and "camera_link" coordinates converge together.
  • Intel's ROS wrapper provide static TFs between each sensor coordinate to the camera base camera_link
  • Also, it provides TFs from each sensor ROS coordinates to its corresponding optical coordinates.

Example of static TFs of RGB sensor and Infra2 (right infra) sensor of D435i module as it shown in rviz2:

example

Extrinsics from Sensor A to Sensor B

Extrinsic from sensor A to sensor B means the position and orientation of sensor A relative to sensor B. Imagine that B is the origin (0,0,0), then the Extrensics(A->B) describes where is sensor A relative to sensor B. In D435i the depth_to_color parameter is derived and set as follows:

  • If we look from behind of the D435i, extrinsic from IR1 depth sensor to RGB sensor, means, where is the IR1 sensor located in relative position to the RGB sensor.
  • If we just look at the X coordinates, in the optical coordiantes (again, from behind) and assume that RGB sensor (COLOR) is (0,0,0), we can say that IR1 sensor (DEPTH) is on the right of RGB sensor by 0.0148 m (1.48 cm).

d435i

administrator@perclnx466 ~/ros2_humble $ ros2 topic echo /camera/extrinsics/depth_to_color
rotation:
- 0.9999583959579468
- 0.008895332925021648
- -0.0020127370953559875
- -0.008895229548215866
- 0.9999604225158691
- 6.045500049367547e-05
- 0.0020131953060626984
- -4.254872692399658e-05
- 0.9999979734420776
translation:
- 0.01485931035131216
- 0.0010161789832636714
- 0.0005317096947692335
---

As you can see, extrinsic msg is made up of two parts:

  • float64[9] rotation (Column - major 3x3 rotation matrix)
  • float64[3] translation (Three-element translation vector, in meters)

Published Topics

The published topics differ according to the device and parameters. After running the above command with RealSense D435i attached, the following list of topics will be available (This is a partial list. For full one type ros2 topic list):

  • /camera/aligned_depth_to_color/camera_info
  • /camera/aligned_depth_to_color/image_raw
  • /camera/color/camera_info
  • /camera/color/image_raw
  • /camera/color/metadata
  • /camera/depth/camera_info
  • /camera/depth/color/points
  • /camera/depth/image_rect_raw
  • /camera/depth/metadata
  • /camera/extrinsics/depth_to_color
  • /camera/imu
  • /diagnostics
  • /parameter_events
  • /rosout
  • /tf_static

This will stream relevant camera sensors and publish on the appropriate ROS topics.

Enabling accel and gyro is achieved either by adding the following parameters to the command line:

ros2 launch realsense2_camera rs_launch.py pointcloud.enable:=true enable_gyro:=true enable_accel:=true

Or in runtime using the following commands: 

ros2 param set /camera/camera enable_accel true
ros2 param set /camera/camera enable_gyro true

Enable Stream

Enabling stream adds matching topics. For instance, enabling the gyro and accel streams adds the following topics:

  • /camera/accel/imu_info
  • /camera/accel/metadata
  • /camera/accel/sample
  • /camera/extrinsics/depth_to_accel
  • /camera/extrinsics/depth_to_gyro
  • /camera/gyro/imu_info
  • /camera/gyro/metadata
  • /camera/gyro/sample

Metadata Topic

The metadata messages store the camera's available metadata in a json format. To learn more, a dedicated script for echoing a metadata topic in runtime is attached. For instance, use the following command to echo the camera/depth/metadata topic:

python3 src/realsense-ros/realsense2_camera/scripts/echo_metadada.py /camera/depth/metadata

Post-Processing Filters

The following post processing filters are available:

  • align_depth: If enabled, will publish the depth image aligned to the color image on the topic /camera/aligned_depth_to_color/image_raw.
  • The pointcloud, if created, will be based on the aligned depth image.
  • colorizer: will color the depth image. On the depth topic an RGB image will be published, instead of the 16bit depth values .
  • pointcloud: will add a pointcloud topic /camera/depth/color/points.
    • The texture of the pointcloud can be modified using the pointcloud.stream_filter parameter.
    • The depth FOV and the texture FOV are not similar. By default, pointcloud is limited to the section of depth containing the texture. You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros, by setting pointcloud.allow_no_texture_points to true.
    • pointcloud is of an unordered format by default. This can be changed by setting pointcloud.ordered_pc to true.
  • hdr_merge: Allows depth image to be created by merging the information from 2 consecutive frames, taken with different exposure and gain values.
  • The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the depth_module.sequence_id parameter and then modifying the depth_module.gain, and depth_module.exposure.
  • To view the effect on the infrared image for each sequence id use the sequence_id_filter.sequence_id parameter.
  • To initialize these parameters in start time use the following parameters:
    • depth_module.exposure.1
    • depth_module.gain.1
    • depth_module.exposure.2
    • depth_module.gain.2

  • For in-depth review of the subject please read the accompanying white paper (External Link).

  • The following filters' detailed descriptions can be found here (External Link).

    • disparity_filter - convert depth to disparity before applying other filters and back.
    • spatial_filter - filter the depth image spatially.
    • temporal_filter - filter the depth image temporally.
    • hole_filling_filter - apply hole-filling filter.
    • decimation_filter - reduces depth scene complexity.

Each of the above filters have it's own parameters, following the naming convention of <filter_name>.<parameter_name> including a <filter_name>.enable parameter to enable/disable it.

Available Services

device_info : retrieve information about the device - serial_number, firmware_version etc.

Type ros2 interface show realsense2_camera_msgs/srv/DeviceInfo for the full list.

Call example: ros2 service call /camera/device_info realsense2_camera_msgs/srv/DeviceInfo

Efficient Intra-Process Communication:

Intel's ROS 2 Wrapper node supports zero-copy communications if loaded in the same process as a subscriber node. This can reduce copy times on image/pointcloud topics, especially with big frame resolutions and high FPS.

You will need to launch a component container and launch RealSense node as a component together with other component nodes. Further details on "Composing multiple nodes in a single process" can be found here (External Link). Further details on efficient intra-process communication can be found here (External Link).

  • Example - Manually load multiple components into the same process

  • Start the component: 

    ros2 run rclcpp_components component_container

  • Add the wrapper: 

    ros2 component load /ComponentManager realsense2_camera realsense2_camera::RealSenseNodeFactory -e use_intra_process_comms:=true

  • Load other component nodes (consumers of the wrapper topics) in the same way.

Limitations

  • Node components are currently not supported on RCLPY
  • Compressed images using image_transport will be disabled as this isn't supported with intra-process communication

Latency Test Tool

For getting a sense of the latency reduction, a frame latency reporter tool is available via a launch file. The launch file loads the wrapper and a frame latency reporter tool component into a single container (so the same process). The tool prints out the frame latency (now - frame.timestamp) per frame.

The tool is not built unless asked for. Turn on BUILD_TOOLS during build to have it available: 

colcon build --cmake-args '-DBUILD_TOOLS=ON'

The launch file accepts a parameter, intra_process_comms, controlling whether zero-copy is turned on or not. Default is on: 

ros2 launch realsense2_camera rs_intra_process_demo_launch.py intra_process_comms:=true