Finishing touches of Rosify Difodo

6 minute read

THe rosify package of difodo is almost done. Currently is working but some final touches are required like using ros logs, getting the configuration parameters from a .launch file etc.

Publishing at the desired rate

Since I have created a parameter for the desired execution of the algorithm, the odometry topic that is being publish should publish at that specified rate. For this porpuse we have ros::Rate which allows us to set a frequency and then by using rate.sleep(), ros will wait the leftover time in a cycle based on the frequency specified.

For any strange reason applying ros::Rate sleep() method, increases the execution time of the Difodo algorithm itslef from 8ms to 12ms. Probably ros is doing something in the background that takes CPU processing time, increasing the CPU load and leaving Difodo algorithm less CPU power.

Still, is ve4ry useful to publish at a desired speed.

Creating a fully functional rospackage

In order to have a rospackage that can be used with ros commands as roscd, rospack, rosrun, roslaunch… We have to make some steps.

Creating a launch file

A launch file is a configuration file. Since we have toi configure some parameters for our algorithm, we will specify them in a configuration file, in ros we will use a .launch file which is a extension on xml.

In order to create a launch file all you have to do is create a file within your ros package with a .launch extension. Usually we will have several launch files for different configurations so we create a /launch directory within the package directory.

In this case my package is called rosify_difodo this is specified in the package.xml and in your CMakeList.txt to build your project in the section project(your_package_name). Finally is also good to name the same as your package the directory containing the package (for obvious reasons) but is not necessary.

Your ros project or package should look like this:

rosify_difodo/
  CMakeList.txt
  package.xml
  src/
  include/
  launch/
    my_launch_file.launch

This is all that is required to be a ros package, this package should be in a ROS_PACKAGE_PATH environmental variable.

` env | grep -i catkin `

Will show you the environmental variables that are related to your catkin environment, but you can also install the packages on the main ros path.

To see the value of you ROS_PACKAGE_PATH environmental variable:

$ echo $ROS_PACKAGE_PATH
/home/omar/Programs/catkin_ws/src:/opt/ros/melodic/share

I could put either in the catkin path /home/omar/Programs/catkin_ws/src or in the ros path /opt/ros/melodic/share the package.

I copied it on my catkin_ws and the on /home/omar/Programs/catkin_ws I run **catkin_make to build the package node (or binary). The binary is specified in the add_executable(my_executable_name my_files_to_build) from the CMakelist.txt**.

Finally in you launch file to start the desired node just put:

<launch>
    <!--pkg is the name of the package as in the package.xml is defined, type is the node name that is the name of the executable created, finally name is just a given name-->
    <node pkg="rosify_difodo" type="ros_difodo" name="myros_difodo_name" />
</launch>

With this you can then call your ros node with a launch file by typing in a terminal:

$ roslaunch my_package_name my_launch_file.launch

Some useful links:

Adding configuration parameters to your launch file

Parameters in ros are shared using the Parameter Server. The Parameter Server is a place where variables can be set and retrieve from any ros node or launch file. We will add configuration parameters in our launch file and retrieved them in the ros node using their API as shown here. For a general view of Parameter Server there is info here.

In summary a parameter can be added in the .launch file by doing this:

<launch>
  <param name="somestring1" value="bar" />
  <!-- force to string instead of integer -->
  <param name="somestring2" value="10" type="str" />

  <param name="someinteger1" value="1" type="int" />
  <param name="someinteger2" value="2" />

  <param name="somefloat1" value="3.14159" type="double" />
  <param name="somefloat2" value="3.0" />

  <!-- you can set parameters in child namespaces -->
  <param name="wg/childparam" value="a child namespace parameter" />

  <!-- upload the contents of a file to the server -->
  <param name="configfile" textfile="$(find roslaunch)/example.xml" />
  <!-- upload the contents of a file as base64 binary to the server -->
  <param name="binaryfile" binfile="$(find roslaunch)/example.xml" />

</launch>

And they can be retrieved later with:

std::string global_name, relative_name, default_param;
if (ros::param::get("/global_name", global_name))
{
  ...
}

if (ros::param::get("relative_name", relative_name))
{
...
}

// Default value version
ros::param::param<std::string>("default_param", default_param, "default_value");

I recommend to read the previous links that provide useful information and different ways to get and create parameters.

Another interesting way is to create several launch files that can be included within a main launch and also YMAL can be used to have configuration parameters that can be included later within a launch file.

How to install

First get the code from github:

git clone https://github.com/RoboticsLabURJC/2019-tfm-omar-garrido.git

Move or copy the package to a catking workspace or your ros workspace In my case:

cp -r rosify_difodo/ ~/Programs/catkin_ws/src/

If you dont know where your catkin workspace is or you havent created one:

To see if the package has been correctly install and is being recognized:

rospack list | grep -i difodo

This command should return the rosify_difodo package and its location.

Build the application, generate the binary node

# Go to your catkin_ws root.
cd  ~/Programs/catkin_ws
# Build
catkin_make

catkin_make will build the packages within your catkin workspace. In order to just generate the desired package:

# Once this in done, from now on only this package will be build even with catkin_make
catkin_make --only-pkg-with-deps rosify_difodo
# Be sure to do this to be able to build again all the packages
catkin_make -DCATKIN_WHITELIST_PACKAGES=""

To see other options to build packages and manage catkin see this answer that also uses:

sudo apt-get install python-catkin-tools

All the code above in just one code block for copy paste:

git clone https://github.com/RoboticsLabURJC/2019-tfm-omar-garrido.git
cp -r rosify_difodo/ ~/Programs/catkin_ws/src/
cd  ~/Programs/catkin_ws
catkin_make --only-pkg-with-deps rosify_difodo
catkin_make -DCATKIN_WHITELIST_PACKAGES=""

How to run and debug

After building is time to run the node.

Running

First we have to make the desire camera publish depth frames in the specified topic in the ros_difodo.launch In my case I use realsense D435 (See previous posts).Then we run the package with the ros_difodo.launch.

# Run realsense camera with:
roslaunch realsense2_camera rs_rgbd.launch
# Run difodo with:
roslaunch rosify_difodo **ros_difodo.launch**

Ive created another set of launch files that runs the realsense node and the difodo algorithm

roslaunch rosify_difodo ros_difodo_and_realsense.launch

In order to change any configuration parameter of Difodo, the parameters are in the ros_difodo.launch since ros_difodo_and_realsense.launch loads the node from that launch file.

Also the realsense node has been integrated so if you want to change any parameter just change the rs_rgbd.launch from rosify_difodo package. Since rs_rgbd.launch includes others launch and xmls sometimes changes are overwritten by the new ones. (CHECK how to properly change depth FPS)

Debugging with CLion

CLion is my IDE of choice to develop for ROS in C++. In order to work with ros packages CLion (and other IDEs) can be configured to have a better integration with ROS check attach the debugger to a process

In CLion go to Run->Attach to Process and type ros_difodo to attach to it.

In case there is an error with ptrace do:

echo 0 | sudo tee /proc/sys/kernel/yama/ptrace_scope

This will solve it temporarily in order to have a permanent solution see: https://www.jetbrains.com/help/clion/attaching-to-local-process.html

Logs in ROS

You can see the logs or output from the node in several places, the first place is on your terminal. But there are also othero options.

See the odometry topic in terminal

rostopic echo /difodo/odometry

The ROS logs location is:

~/.ros/log/latest

Where latest represents the lastest node launched.

rqt_console

Open in a terminal:

rqt_console

If no logs are being displayed just press the red circle to start. When runnning the ros_difodo node you will see the logs there.

For a visual representation using rviz see the previous entry in the blog

Results

You can see two examples videos of the logs generated by difodo working at two different frequencies:

You May Also Enjoy