Creating a ROS wrapper around DIFODO

13 minute read

In this entry Ill talk about the process to create a ROS wrapper around DIFODO algorithm so it can be used with any RGBD or TOF camera that implements ROS and sends ROS messages. DIFODO currently has their own depth image structure (define as a floating matrix in C++) and they only work with a set of cameras on their platform, realsense not having support. Making a ROS package for DIFODO will allow to use realsense cameras and much more due to the standard of ROS topics and messages.

Setting up

There are a few elements which I have to get familiar with: catkin, CMake and a C++ IDE.

Since I havent work before with CMake, I would like and IDE which can integrate CMake build system in an easy way, CLion (non-free unless you are a student) seems to integrate good with CMake since is the default build system used by the IDE.

There are more options which can be used also like Eclipse, Visual Studio Code…

Here is a full list to ROS compatible IDEs

CLion IDE

CLion support for ROS has two ways to deal with ROS.

Either you use on of the not offical plugins or you just configure a few things. I used the latter option but Ill try the other one in the future also.

To download CLion I recommend to download it from the offical webpage and download the .tar or .zip. Unzip it and then on the /bin directory within the extracter folder the clion.sh can be found.

Then, follow the instrucctions from the previous hyperlink to configure clion so it can see the ROS environment variables and have compatibility with the catkin macros within the CMakefile. (The only thing to do here is to call clion within a shell with the catkin environment variables).

Also more useful instructions can be found in this link from jetbrains

CMake

Is a build system which is undependant from compiler ads operating systems. Its prepare to use the native builder in the target OS. CMake works with the CMakefiles.txt which are the configuration files that have the instrucctions to generate the build files. Its a great tool if you want to be able to build in different OS.

Catkin

Is the build system of ROS which is written over CMake, adding functionalities to the CMake build system to make ROS building easier.

A great resource to get started with CMake and catkin can be found here. Also catkin introduces the package.xml which is some sort of configuration file for ROS packages.

But catkin itself is not only this, it also has a set of tools (install along the ROS installation) which lets you manage workspaces and more.

Creating a ROS package

The best resources to start to get in touch with catkin and ROS packages are their tutorials. Also catkin tutorials can be found here

Ill list the more important steps here but is recommended to follow the full tutorials.

Create a catkin workspace:

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/
catkin_make

This will create a general CMakefile.txt in the src folder. Also there is nothing to build yet. The directories /devel and /build will be created.

To make this workspace available to catkin do

source devel/setup.bash

Add this line to you .bashrc to make it automatically for every shell. (Make sure you use a full path)

Create a catkin package with catkin_create_pkg:

catkin_create_pkg beginner_tutorials std_msgs rospy roscpp

And finally use catkin_make within the catkin_ws dir to build all of the catkin_packages inside /src.

The executables, libraries and include headers can be found on /devel and /install directories.

Once you have your catkin_package created and if you have configured your IDE of choice correctly you should be able to open your package with CLion and start working on it.

CLion will index all the libraries within the CMakefile.txt (including also the stardard libraries) so autocompletition should be available if everything is working.

If the CMakefile.txt couldnt be fully read by CLion (if catkin commands cannot be found) that means that your IDE is not working alongside catkin and some actions may be required(go back to the documentation to configure IDEs with catkin or ROS).

Getting the depth images from realsense onto my C++ application

Finding out realsense useful topics

Realsense has a lot of topics since we have an RGB camera, a depth camera, 2 infrared options, aligned depth and rgb camera…

  1. Realsense publishing ros messages There are several ways to get the realsense to start publishing or sending ros messages throuhg different topics. Here are two of the ones that Ive used for now:
  • Default way, included with ros installation At least that I remember, this way of starting a roscore with a realsense2_camera node was installed alongside ros_realsense2. The infrared topics and the pointcloud are not publish with this rs_camera.launch but we get the desired topics (depth).

bash``` roslaunch realsense2_camera rs_camera.launch**


- More topics available
To use this .launch I had to install it first since it didnt come with the ros\_realsense package. It can we installed with bash or simply downloading the rs\_rgbd.launch file from the internet and saving it somewhere in the ros installation so it can be found by ros.

bash```
sudo apt install ros-melodic-rgbd-launch
roslaunch realsense2\_camera rs\_rgbd.launch

The realsense uses .launch files, which are configuration files that enhaces the xml language. You can create your own launch files, in fact Ill have to do it soon to publish at a certain resolution and at a certain speed (FPS). To know more about launch files and the roslaunch command:

  1. Realsense available topics Here is a list of all the topics available for the realsense:
/camera/aligned_depth_to_color/camera_info
/camera/aligned_depth_to_color/image_raw
/camera/aligned_depth_to_color/image_raw/compressed
/camera/aligned_depth_to_color/image_raw/compressed/parameter_descriptions
/camera/aligned_depth_to_color/image_raw/compressed/parameter_updates
/camera/aligned_depth_to_color/image_raw/compressedDepth
/camera/aligned_depth_to_color/image_raw/compressedDepth/parameter_descriptions
/camera/aligned_depth_to_color/image_raw/compressedDepth/parameter_updates
/camera/aligned_depth_to_color/image_raw/theora
/camera/aligned_depth_to_color/image_raw/theora/parameter_descriptions
/camera/aligned_depth_to_color/image_raw/theora/parameter_updates
/camera/aligned_depth_to_infra1/camera_info
/camera/aligned_depth_to_infra1/image_raw
/camera/aligned_depth_to_infra1/image_raw/compressed
/camera/aligned_depth_to_infra1/image_raw/compressed/parameter_descriptions
/camera/aligned_depth_to_infra1/image_raw/compressed/parameter_updates
/camera/aligned_depth_to_infra1/image_raw/compressedDepth
/camera/aligned_depth_to_infra1/image_raw/compressedDepth/parameter_descriptions
/camera/aligned_depth_to_infra1/image_raw/compressedDepth/parameter_updates
/camera/aligned_depth_to_infra1/image_raw/theora
/camera/aligned_depth_to_infra1/image_raw/theora/parameter_descriptions
/camera/aligned_depth_to_infra1/image_raw/theora/parameter_updates
/camera/color/camera_info
/camera/color/image_raw
/camera/color/image_raw/compressed
/camera/color/image_raw/compressed/parameter_descriptions
/camera/color/image_raw/compressed/parameter_updates
/camera/color/image_raw/compressedDepth
/camera/color/image_raw/compressedDepth/parameter_descriptions
/camera/color/image_raw/compressedDepth/parameter_updates
/camera/color/image_raw/theora
/camera/color/image_raw/theora/parameter_descriptions
/camera/color/image_raw/theora/parameter_updates
/camera/color/image_rect_color
/camera/color/image_rect_color/compressed
/camera/color/image_rect_color/compressed/parameter_descriptions
/camera/color/image_rect_color/compressed/parameter_updates
/camera/color/image_rect_color/compressedDepth
/camera/color/image_rect_color/compressedDepth/parameter_descriptions
/camera/color/image_rect_color/compressedDepth/parameter_updates
/camera/color/image_rect_color/theora
/camera/color/image_rect_color/theora/parameter_descriptions
/camera/color/image_rect_color/theora/parameter_updates
/camera/color_rectify_color/parameter_descriptions
/camera/color_rectify_color/parameter_updates
/camera/depth/camera_info
/camera/depth/image_rect_raw
/camera/depth/image_rect_raw/compressed
/camera/depth/image_rect_raw/compressed/parameter_descriptions
/camera/depth/image_rect_raw/compressed/parameter_updates
/camera/depth/image_rect_raw/compressedDepth
/camera/depth/image_rect_raw/compressedDepth/parameter_descriptions
/camera/depth/image_rect_raw/compressedDepth/parameter_updates
/camera/depth/image_rect_raw/theora
/camera/depth/image_rect_raw/theora/parameter_descriptions
/camera/depth/image_rect_raw/theora/parameter_updates
/camera/depth_registered/points
/camera/extrinsics/depth_to_color
/camera/extrinsics/depth_to_infra1
/camera/extrinsics/depth_to_infra2
/camera/infra1/camera_info
/camera/infra1/image_rect_raw
/camera/infra1/image_rect_raw/compressed
/camera/infra1/image_rect_raw/compressed/parameter_descriptions
/camera/infra1/image_rect_raw/compressed/parameter_updates
/camera/infra1/image_rect_raw/compressedDepth
/camera/infra1/image_rect_raw/compressedDepth/parameter_descriptions
/camera/infra1/image_rect_raw/compressedDepth/parameter_updates
/camera/infra1/image_rect_raw/theora
/camera/infra1/image_rect_raw/theora/parameter_descriptions
/camera/infra1/image_rect_raw/theora/parameter_updates
/camera/infra2/camera_info
/camera/infra2/image_rect_raw
/camera/infra2/image_rect_raw/compressed
/camera/infra2/image_rect_raw/compressed/parameter_descriptions
/camera/infra2/image_rect_raw/compressed/parameter_updates
/camera/infra2/image_rect_raw/compressedDepth
/camera/infra2/image_rect_raw/compressedDepth/parameter_descriptions
/camera/infra2/image_rect_raw/compressedDepth/parameter_updates
/camera/infra2/image_rect_raw/theora
/camera/infra2/image_rect_raw/theora/parameter_descriptions
/camera/infra2/image_rect_raw/theora/parameter_updates
/camera/realsense2_camera_manager/bond
/camera/rgb_camera/parameter_descriptions
/camera/rgb_camera/parameter_updates
/camera/stereo_module/parameter_descriptions
/camera/stereo_module/parameter_updates

From these topics the ones that are interesting for this project are:

TOPIC TYPE /camera/color/image_raw sensor_msgs/Image /camera/depth/image_rect_raw sensor_msgs/Image

There are more topics image related like compressed, compressedDepth but for now we will focus on the raw images without any compression.

Subscribing to the desired topics

  1. ROS suscribers and publishers http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

  2. Specialized suscribers and publishers for Image topics: image_transport package The original image_transport package allows to work with raw images, but there are low bandwith packages that works with compress images (JPEG/PNG) that can be found on the previous link to the original package.

API documentation

Converting from ROS std_msgs to CV alike matrices

http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages

depth example: https://github.com/IntelRealSense/realsense-ros/issues/807

MRPT: Integrating DIFODO with ROS

The next step is to create a derived class from the DIFODO class from mrpt. To do this Ill check again their tutorials about developing a C++ application that uses the mrpt library.

1. Writing apps with the MRPT library

Following the link is the mini-tutorial to create apps based on MRPT. The first step is to include their libraries as a dependency on our program in the CMakefile.txt of our app. In order to know which modules or libraries we need for the desired application (based on DIFODO) there is information here.

Doxygen documentation of DIFODO will help us know which packages we need to include, in this case mrpt::vision.

The full doxygen documentation of the vision module.

And finally an example of what we have to add to our CMakefile.txt. Only the vision module will be required in this project and also the poses module for the 3D pose output.

Once we have our CMakelist.txt ready, we can start to include mrpt CDifodo and start building the Rosified CDifodo module.

2. DIFODO for ROS.

I integrated the suscriber in ROS so when new images comes throught ROS realsense depth topic a callback is call within the new DIFODO class, that does the following:

  • Converts from a ros msg to an openCV matrix.
  • Resizes the image if needed (if downsampling is specified)
  • Converts from openCv Mat to the matrix class used in mrpt.
  • Calls loadFrame(): Actually is not being called at the moment, the callback is the new loadFrame().
  • Call to odometryCalcutation(): This computes the new pose.

3. Creating a .launch file for realsense

Since DIFODO works at real time (30FPS) with images of low resolution such as QVGA (320x240) and at 90FPS at QQVGA (160x120), we have to make the realsense depth topic send the images at one of these resolutions.

The way to do this is created or modify an existing .launch file. Specifying the desired resolution and FPS or frequency since the launch files used until now rs_rgbd.launch and rs_rgbd.launch send images of 1280x1024 and at 30FPS.

Since the .roslaunch files for realsense by default already publish depth images at 640x480 and 30FPS we wont have to change them for now.

By executing command rs-enumerate-devices with the realsense camera connected one can see the different streams and configurations available to send.

For the depth topic we have:

Some higher resolutions...

Depth	  640x480	@ 90Hz	   Z16
Depth	  640x480	@ 60Hz	   Z16
Depth	  640x480	@ 30Hz	   Z16
Depth	  640x480	@ 15Hz	   Z16
Depth	  640x480	@ 6Hz	   Z16
Depth	  640x360	@ 90Hz	   Z16
Depth	  640x360	@ 60Hz	   Z16
Depth	  640x360	@ 30Hz	   Z16
Depth	  640x360	@ 15Hz	   Z16
Depth	  640x360	@ 6Hz	   Z16
Depth	  480x270	@ 90Hz	   Z16
Depth	  480x270	@ 60Hz	   Z16
Depth	  480x270	@ 30Hz	   Z16
Depth	  480x270	@ 15Hz	   Z16
Depth	  480x270	@ 6Hz	   Z16
Depth	  424x240	@ 90Hz	   Z16
Depth	  424x240	@ 60Hz	   Z16
Depth	  424x240	@ 30Hz	   Z16
Depth	  424x240	@ 15Hz	   Z16
Depth	  424x240	@ 6Hz	   Z16

The lowest 4/3 resolution is 640x480 while we also have resolutions of 16/9 like 480x270 and 424x240. The DIFODO algorithm should work on any resolution given but the DifOdometry-Datasets app for testing used in the past entrie of this blog used 4/3 resolutions. So for now Ill like to keep this factor.

The problem here is that the lowest 4/3 resolution is 640x480. So in order to go into realtime Ill have to downsample the depth images that arrive from ROS, before using them on the DIFODO algorithm. This resize will also take time but since we dont have lower resolutions is the only way to go with the realsense camera.

4. Publishing Pose to ROS topic (tf2)

Since this is an odometry method we end up as result with a Pose estimation, relative to the origin in the world coordinates where the camera started.

In this case DIFODO returns a translation (x,y,z) and a rotation in euler angles (Roll, Pitch, Yaw).

The standard message to publish Pose in ros is geometry_msgs/Pose which is compound by another two ros messages geometry_msgs/Point and geometry_msgs/Quaternion. The point is a vector with (x,y,z) the quaternion is a 4 vector size (x,y,z,w).

Here is the first problem, we have to change from a RPY representation to a Quaternion representation. In order to do this I came across this library for ROS call tf2 which stands for transformation library 2. It helps to make transformation from ros msgs to different representations. In this case is a small problem, so we only need the sublibrary tf2_geometry_msgs to convert from a RPY representation to a Quaternion, here are some examples: example1, example2.

5. Publishing odometry messages on ROS

The odometry message is composed of a pose message and a twist (or velocity) message. Here is a nice tutorial of publishing odometry information in ROS On ROS we have the concept of frames which is the same as a coordinate reference system. The goal here is to be able to express the position and movement of robots based on different coordinates systems (frames) or handling several robots. We can also think of different parts of the body of a robot referenced to another parts of the robot itself, like the position of the hand in reference to the should and the should itself is referenced to the “base_link” (the default name for the main reference to the body of a robot), and the position of the robot (or base_link) is referenced itself to the main frame or “odom” usually.

There are some variables that define speed in Difodo named “kai_abs” and “kai_loc”, also I could compute the speed based on the differences between poses “cam_pose” and “cam_old_pose” and dividing by the “execution_time” of the algorithm. Since speed is not entirely necessary for the odometry message to work Ill left this open, and come back in the future. The same goes for “covariance” for the Pose and Twist, these are not being fill with any values at the moment.

6. Visualization

Using: rosrun tf view_frames You can get a PDF that allows you to see the connections betweens frames for all your rosnodes.

Now with the ros message being publish we can see it with rviz for example, which is ros visualizer by default.

To see the odometry message in rviz, we will have to select the desired frame (odom in this case) and then add and odometry topic with the ADD button.

First, go to the Displays->Global Options->Fixed Frame and select “odom” frame. Also select the frame rate which should match with the odometry messages frequency, which will be the “working_fps” set in the algorithm by the user.

Secondly, select the Add button. There are two ways to add or suscribe to topics. The easiest is use the tab “By topic” and there you will see all the available topics. Search for the odometry topic and add it. Another way to do it is used the tab “By display type” and select “odometry”.

Once the odometry is add, go to Odometry->Topic and select the desired odometry topic, in this case the default topic is /difodo/odometry.

To be able to see the algorithm update properly set the properties Position Tolerance and Angle Tolerance to 0.01 instead of 0.1, that way the pose will be update on the display with changes of 0.01m instead of 0.1m. The changes will be smoother. Also is good to set Keep propery to 1 if you only want to see the current position and not a trajectory.

Also I recommend to suscribe and add the topics depth images and color images, so one can appreciate the movement within the images also.

7. Results

You can check the final results after all these steps in this video: https://youtu.be/ji4qcrJzF7U.

As can be seen theres still room for improvements as the algorithm quickly accumulates error. A preprocessing on the depth image should be done in order to work only with good depth values.

TODO or to Improve

  • The conversion from ros msg to a mrpt matrix has an intermediate step that converts to opencv Mat, it will be more efficient to directly convert from ros msg to mrpt Mat.
  • Create a configuration file for the ROSDifodo algorithm. We could use a .launch file since it will be used as a ros package but also an xml, json, yaml, ini…since it gives more freedom when integrating this software with other non ros softwares.
  • Preprocessing of the depth image
  • Adjust some values of the DIFODO algorithm to the camera being used (in this case Realsense D435). Like FOV, limit values for depth (max_depth and min_depth)…These values usually depend on the resolution being used for the camera and the camera model itself so create them as configuration values and also try to look for general values that can apply to lot of cameras with good results.
  • Create a queue to store the depth images as they come from ros and in a separate thread the odometryCalculation consumes those images from the queue. That way the arquitecture is not event driven but producer-consumer oriented. Probably more efficient and we can be sure when the speed is not enough to consume depth images and set warnings. The current way will drop depth images if they cannot being process at the required speed, or if the ros message queue is large enough, it will start to have a time delay.

You May Also Enjoy