ROS packages for versatile RBPF-based SLAM (By: Vladislav Tananaev)

[jlblancoc]

Initial description Mobile Robot Programming Toolkit (MRPT) implements a versatile Rao-Blackwellized Particle Filter (RBPF) for practical robotics applications. At present, there are no Rao-Blackwellized Particle Filter ROS packages with such broad range of features. The existing slam_gmapping ROS package, which is based on gmapping from OpenSLAM, uses only grid-based maps from laser data. The implementation from the MRPT library can work simultaneously with multiple instances of grid-based maps, point maps, beacon maps and combinations of them. Moreover, MRPT provides several types of Particle Filters such as PF with Standard and Optimal [Grisetti et al., 2007] Proposals, as well as an Approximate Optimal Sampling [Blanco et al., 2008a]. Thus, existing slam_gmapping package features only a subset of this functionality and robotics community will benefit greatly from being able to use implementation offered by MRPT library. Therefore, the goal of this project is to provide MRPT-powered ROS packages for Rao-Blackwellized Particle Filter.

Progress: All comments below reflect the interactions between the GSoC student and the mentor(s).

Final results:

• PR https://github.com/ual-arm-ros-pkg/mvsim/pull/2
• PR https://github.com/mrpt-ros-pkg/mrpt_navigation/pull/26
• PR https://github.com/mrpt-ros-pkg/mrpt_navigation/pull/27
• PR https://github.com/MRPT/mrpt/pull/307
• PR https://github.com/mrpt-ros-pkg/mrpt_slam/pull/7

[jlblancoc]

Perfect! Accepted, I'll try them later. Could you please take a look at this error? http://build.ros.org/job/Idev__mrpt_slam__ubuntu_trusty_amd64/1/

It looks like a missing ifdef for older mrpt versions...

[jlblancoc]

Please, see also:

/mrpt_ekf_slam_2d_wrapper.h:24:44: fatal error: visualization_msgs/MarkerArray.h: No such file or directory compilation terminated.

From http://build.ros.org/job/Kdev__mrpt_slam__ubuntu_xenial_amd64/1/

Maintaining open source is this complicated!! ;-)

[Logrus]

Hi Jose!

I added ifdefs to check the version of MRPT and also visualization_msgs dependency and created a pull request. I still get an error with CActionRobotMovement3D though, since it is different in previous MRPT version and therefore mrpt_ekf_slam_3d doesn't compile.

Best wishes, Vladislav

[jlblancoc]

Hi! I investigated the error regarding CActionRobotMovement3D and the only way out I see is... to just ignore odometry in 3D EKF-SLAM if built against older versions of MRPT!

I also fixed other minor things, take a look at the latest commits in: https://github.com/mrpt-ros-pkg/mrpt_slam/commits/master

Cheers!

[jlblancoc]

I fixed many errors, but there is still one more: https://travis-ci.org/mrpt-ros-pkg/mrpt_slam/builds/153132252

Enough for today, let's continue tomorrow! Let me know if you make any progress fixing the build in that old trusty distribution!

[Logrus]

Hi Jose,

Thank you very much for you help!

I made an if statement in CMake that prevents mrpt_ekf_slam_3d from building (https://github.com/Logrus/mrpt_slam/commit/8edbb21bb8cff0b63abba06ee5d3ce8c07f03116) if the user has MRPT version lower than 1.5.0. I think it is generally better to exclude the node that will not work correctly from built. What do you think?

Apparently, after fixing this error there's another in mrpt_navigation/mrpt_rawlog this time. At the moment, Travis is building against MRPT version 1.0.2 installed via sudo apt-get install libmrpt-dev. But what do you think about updating Travis script to install MRPT from ppa which is the recommended default way of installing MRPT?

I also built ROS nodes against the version from ppa. At the current state, it still complains on the 3D motion model and mapBuilder->setVerbosityLevel(mrpt::utils::LVL_DEBUG);, but it should build after ppa is updated with the latest changes.

Thanks!

Best, Vladislav

[jlblancoc]

I made an if statement in CMake that prevents mrpt_ekf_slam_3d from building (Logrus/mrpt_slam@8edbb21) if the user has MRPT version lower than 1.5.0. I think it is generally better to exclude the node that will not work correctly from built. What do you think?

In general, it's a better idea, ok! But when all nodes build cleanly in the matrix of Ubuntu versions / ROS versions, I'll try to do the first binary release (i.e. what allows people to do sudo apt-get install ros-XXX) and then we'll see if bloom-release complains. To the "future works" list ;-)

I also built ROS nodes against the version from ppa. At the current state, it still complains on the 3D motion model and mapBuilder->setVerbosityLevel(mrpt::utils::LVL_DEBUG);, but it should build after ppa is updated with the latest changes.

Right! I'm right now sending the latest master version to the PPA, let's see if all go ok and packages become available within hours...

But what do you think about updating Travis script to install MRPT from ppa which is the recommended default way of installing MRPT?

That would be wonderful... but there is a huge queue of PPAs waiting to be accepted in travis (they don't allow PPAs by default), and PCL and MRPT are in one of their tickets... waiting till ~0.5-1 year ago, so I guess they don't want to allow more PPAs into their systems :-(

[Logrus]

Right! I'm right now sending the latest master version to the PPA, let's see if all go ok and packages become available within hours...

Great! I will update and check if everything builds, then I'll create a pull request.

That would be wonderful... but there is a huge queue of PPAs waiting to be accepted in travis (they don't allow PPAs by default), and PCL and MRPT are in one of their tickets... waiting till ~0.5-1 year ago, so I guess they don't want to allow moew PPAs into their systems :-(

That is unfortunate... Otherwise we can of course download and build MRPT master branch, but this will add additional 30 min build time. But as far as I can see Travis limits jobs to 50 minutes, so this still could work.

[Logrus]

I have just found this in travis docs: https://docs.travis-ci.com/user/installing-dependencies/#Installing-Packages-from-a-custom-APT-repository

It seems that it should be possible to install from ppa, we can try. :)

[Logrus]

Hi Jose!

I added installation from ppa and it seems to be working (https://travis-ci.org/Logrus/mrpt_slam/builds/153572469), but the ppa isn't updated yet.

I also updated ROS wiki (example): http://wiki.ros.org/mrpt_rbpf_slam When I tried to update Nodes section for icp slam it disappeared when I saved my changes, but I can still see markup when I click edit.

== Nodes ==
{{{#!clearsilver CS/NodeAPI
 name = mrpt_icp_slam_2d
...

I didn't understand how this part is working. Is it generated only from time to time?

Thanks!

[jlblancoc]

I added installation from ppa and it seems to be working (https://travis-ci.org/Logrus/mrpt_slam/builds/153572469),

Great! I don't know why then I discarded this choice months ago, but if it works, let's go that way!

but the ppa isn't updated yet.

Yes, I know... it seems there is some problem installing the .so files for the new library (it was previously a header-only lib) mrpt-graphslam by @bergercookie, e.g. see this log and look for the error:

    dpkg-shlibdeps: error: couldn't find library libmrpt-graphslam.so.1.5 needed by debian/mrpt-apps/usr/bin/graphslam-engine (ELF format: 'elf64-x86-64'; RPATH: '')

I'll try to figure out a solution ASAP...

I also updated ROS wiki (example): http://wiki.ros.org/mrpt_rbpf_slam

Fantastic! I think it's perfect, as you know, much better than many other ROS packages out there ;-)

When I tried to update Nodes section for icp slam it disappeared when I saved my changes, but I can still see markup when I click edit.

I can see text by you, perhaps you had a problem with the cache?? http://wiki.ros.org/mrpt_icp_slam_2d

[Logrus]

Hi Jose!

I created a pull request with the latest changes. As soon as ppa is updated we hopefully have a green build. Anyways, I will keep an eye on it and correct if errors appear.

I can see text by you, perhaps you had a problem with the cache?? http://wiki.ros.org/mrpt_icp_slam_2d

Sorry! I think I figured out what was wrong and everything works fine now. So no worries. ;) But what I meant is that "Nodes" section for mrpt_icp_slam_2d describing topics, parameters, etc. wasn't rendered, but markup was there. Turned out I made a typo.

Is there anything else I should do before filling the final GSoC evaluation?

Thanks!

Best wishes, Vladislav

[jlblancoc]

For some reason, only part of the distro list were updated yesterday, but the good news is that I fixed the problem and now they build Debian packages cleanly again. I've just re-uploaded the missing distros (xenial, vivid, trusty), so expect them to be available in the PPA within hours.

Cheers

[Logrus]

It builds now! But in mrpt_bridge one test is failing for some reason; https://travis-ci.org/Logrus/mrpt_slam/builds/153580500

Best, Vladislav

[jlblancoc]

Yes! I just fixed the unit test in mrpt_bridge.

You can proceed with ending GSoC, for me all you did is more than perfect! Congrats! :+1:

But before releasing binary ROS packages with bloom-release, I would like to do one more final change to @bergercookie 's COutputLogger to allow {icp,ekf,rbpf}-slam outputs to get redirected to ROS's logging system. I'll write back here when the new interface is done.

[jlblancoc]

It was quick. Please, take a look at this commit, hopefully it's selfexplanatory ;-) https://github.com/MRPT/mrpt/commit/7f50d63bf2f3659227186d381131781b2d2b51bd

I would love to see every call from {icp,ekf,rbpf}-slam nodes, redirected to ROS logging system (keeping the same "severity"/verbosity level, etc.), which will make the nodes easier to debug with standard ROS tools, don't you agree??

No rush at all, though...

[Logrus]

I would love to see every call from {icp,ekf,rbpf}-slam nodes, redirected to ROS logging system (keeping the same "severity"/verbosity level, etc.), which will make the nodes easier to debug with standard ROS tools, don't you agree??

Totally agree, although I am not sure what would be the best way of doing it. I made some commits, this https://github.com/Logrus/mrpt_navigation/commit/3c4a2474153e1a56a8639ee728073030a2dbb25e and this https://github.com/Logrus/mrpt_slam/commit/1d63d8195e41924cfaeebbf3807a14f7118ae69a. It works, but MRPT messages are shown along with ROS logger. If I set logging_enable_console_output=false then callbacks are not called. So here how it looks right now:

Additionally, I have finished with ROS wiki. I added description of topics, parameters and tf transforms to all packages.

And finally, I finished GSoC evaluation.

If you think that logging is fine I'll make a pull request.

Thanks!

Best wishes, Vladislav

[jlblancoc]

Brilliant, you are fast!!

If I set logging_enable_console_output=false then callbacks are not called.

I have fixed it: https://github.com/MRPT/mrpt/commit/512833e29eb07521a8506d963e99d3839d99bb19 Will try to get that change to PPA tonight, though.

So, yes, do the PR, your code shouldn't change. Just, make sure of protecting the code involving COutputLogger so it doesn't get built against mrpt < 1.5.0 (rosLoggerLvlToMRPTLoggerLvl() seems to be visible...).

All in all... excellent! :+1:

[Logrus]

Hi Jose!

I created pull requests, now only ROS logs are displayed. So it seems to be working fine. I also noticed that some of the outputs have additional newline symbol, so that after them an empty line is printed, but this is coming from \n and endl in MRPT.

Best wishes, Vladislav

[jlblancoc]

Oh! Could you please add a few code lines to strip such final chars, please? Also, please take a look at https://travis-ci.org/mrpt-ros-pkg/mrpt_slam/builds/154228887

[Logrus]

Oh! Could you please add a few code lines to strip such final chars, please?

I created a PR for this. It's a bit ugly, I am not sure if it is possible to make it nicer :)

Also, please take a look at https://travis-ci.org/mrpt-ros-pkg/mrpt_slam/builds/154228887

My guess is that the version in PPA doesn't have logRegisterCallback function. I checked again that it builds against MRPT master branch. However, when I use a version from PPA it complains about all invocations of logRegisterCallback in any package.

Is the current version in PPA is the latest? If so, I am not sure why this error happens...

[jlblancoc]

Is the current version in PPA is the latest? If so, I am not sure why this error happens...

Sure, that is the problem :-)

I mistook that Travis error with another one which was complaining because of Eigen being missing, but can't locate it now. Actually, I think it was not in Travis, but in Jenkins... So much Continuous Integration drives one crazy!! I'll try to get that one fixed, no worries.

[jlblancoc]

Officially, this GSoC project can be considered as closed. Congrats @Logrus for your excellent contribution to the project, I hope you learned a lot and had fun coding... hopefully we'll keep collaborating in the future ;-)

[Logrus]

Hi Jose!

Thanks for the really superb mentoring and opportunity to participate in GSoC with MRPT! Working with you and your library was a constant joy and really an outstanding experience! I learned a lot.

I will of course keep maintaining the ROS packages, fix errors and keep them up-to-date with MRPT updates. I am also looking forward for possible collaborations in the future!

Thank you!

Best wishes, Vladislav