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 Add mvsim slam demo. mvsim#2
• PR Adding landmarks to mrpt_bridge. mrpt-ros-pkg/mrpt_navigation#26
• PR Add a callback function to stream MRPT logs to ROS. mrpt-ros-pkg/mrpt_navigation#27
• PR Add a 3D Motion model. mrpt#307
• PR Add mrpt_rbpf_slam, mrpt_icp_slam_2d, mrpt_ekf_slam_3d, mrpt_ekf_slam_2d packages. mrpt-ros-pkg/mrpt_slam#7

[jlblancoc]

Student: @Logrus
Mentor: @jlblancoc
Backup mentor: @buzzer

[Logrus]

Hi @jlblancoc and @buzzer, thank you again for the opportunity to take part in this project!
During the next week, I am planning to be looking more into the existing code of the mrpt_slam package and rbpf-slam application.

I was also wondering if this issue MRPT/mrpt#193 is related to the project. I could try to provide the descriptions while also learning the code structure.

If there are some links or materials that you think may assist understanding of the relevant parts of the library, I would appreciate having them. :)

[jlblancoc]

Welcome on board!

Actually, yes, I intended to solve that issue by the time GSoC started but... I'm swamped with other projects and just couldn't found spare time for it. As an Master student in Robotics, I'm sure that you could indeed help with the documentation, it would be great for the rest of users and also a perfect initial task to get into the nuts of MRPT RBPF code.

Any doubt you find in the way, just come back here and we'll discuss it, ok?

As you'll see, there is a complex network of C++ classes involved. As someone having to face it for the first time, if you need to draw a graph with the relationship between the different classes, it will be also interesting to show it in the docs ;-)

Some hints to start (I recommend you going thru the sources of all these guys):

• mrpt::bayes::CParticleFilter : This is the "entry point" the user ends up calling to iterate a PF algorithm.
• mrpt::bayes::CParticleFilterDataImpl : The container of "particles". It's implemented using the CRTP pattern in derived classes, one for each type of estimation problem.
• mrpt::maps::CMultiMetricMap : A generic container for metric maps (of any kind). The rationale is that the world is the overlap of all these maps simultaneously (i.e. each map represents a different aspect of the world).
• mrpt::maps::CMultiMetricMapPDF : A probability density function (PDF), implemented as a set of particles, each storing a path hypotheses and the corresponding metric map that is generated by "projecting" ("inserting" in MRPT parlance) all the observations from the hypothesized path.
• mrpt::slam::CMetricMapBuilderRBPF : A wrapper of CParticleFilter + CMultiMetricMapPDF = RBPF slam engine

I think that all these comments above may not even described like that in the Dox docs, so feel free of fork+PR with any annotations you see useful to improve their docs.

Cheers!

[Logrus]

Thank you for the pointers!
I indeed have drawn some schemas with these classes while preparing my proposal, I will revise them with the new comments. I will write back\make a PR when I have something.

[buzzer]

Hi Vladislav,

Congratulations for getting accepted!
I am looking forward to this project and in case of any doubts or concerns feel free to ask.

Best,
Sebastian

[Logrus]

@buzzer, Thanks!

@buzzer, @jlblancoc
I have been playing with library and created simulated datasets. I am still looking into the code and also playing with existing MRPT ROS packages and gmapping.

I have a question about fakeActs in CMetricMapBuilderRBPF.
As far as I understood, the localization only carried out after either angular or linear threshold is surpassed. Is it only for performance or does it have some other implications?

Additionally, while examining code, I think I have found where to add a missing interface for pfAuxiliaryPFStandard. I am not sure that this is the correct way of doing it though, but it seems to be working. Here the small commit in my fork: Logrus/mrpt@2ab542a

Regarding documentation, I see that classes are well described already, so at the moment I don't see anything that I can add. The CRTP idiom is new for me, so I haven't fully understood this part yet.

[jlblancoc]

Hi!

As far as I understood, the localization only carried out after either angular or linear threshold is surpassed. Is it only for performance or does it have some other implications?

Mainly for efficiency (typically, one may have odometry at >10Hz) but it also has an important implication: every time the RBPF is updated, the weights of particles got updated via the sensor likelihood function (or more complex formulas depending on PF_algorithm) so we are losing "particle diversity" and accelerating the need to do a resample. Resampling is great only after having a real loop closure, otherwise it normally implies a loss of information (hypotheses).

I am not sure that this is the correct way of doing it though, but it seems to be working. Here the small commit in my fork: Logrus/mrpt@2ab542a

Good! That's probably because we never used that combination in any paper, but it's a new legal and valid RBPF algorithm 👍
Please, give it a try with some datasets and, if no exception is launched (just in case there is any internal virtual method missing or something alike), let me know or directly do a pull-request. This is a small change and is worth integrating into the main repo.

The CRTP idiom is new for me, so I haven't fully understood this part yet.

Think of it as an efficient (and flexible) way of doing virtual classes.
Or "static polymorphism", as coined in this excellent book that I strongly recommend:

Alexandrescu, Andrei (2001). Modern C++ Design: Generic Programming and Design Patterns Applied. Addison-Wesley. ISBN 0-201-70431-5.

[Logrus]

Resampling is great only after having a real loop closure, otherwise it normally implies a loss of information (hypotheses).

Thanks for clarification! I thought that this can result in better stability of the filter.

Please, give it a try with some datasets and, if no exception is launched (just in case there is any internal virtual method missing or something alike), let me know or directly do a pull-request. This is a small change and is worth integrating into the main repo.

Sure, I will report back whether I have problems or it is working fine with different datasets.

Or "static polymorphism", as coined in this excellent book that I strongly recommend:

Alexandrescu, Andrei (2001). Modern C++ Design: Generic Programming and Design Patterns Applied. Addison-Wesley. ISBN 0-201-70431-5.

It looks like I can access it at the university library. I will read it, thanks!

[Logrus]

Hi,

Please, give it a try with some datasets and, if no exception is launched (just in case there is any internal virtual method missing or something alike), let me know or directly do a pull-request.

I made several tests with my simulated datasets and with default configs. No exceptions were raised.
Specifically, from default configs I tested:

•  example_3_gridmaps.ini
  

•  gridmapping_intel.ini
  

•  gridmapping_optimal_sampling.ini
  

•  gridmapping_RBPF_grid_ICPbased_malaga.ini
  

•  gridmapping_RBPF_ICPbased_intel.ini
  

•  gridmapping_RBPF_ICPbased_malaga.ini
  

•  RO-SLAM_simulatedData_MC.ini
  

•  RO-SLAM_simulatedData_SOG.ini
  

I didn't test gas_mapping_2lasers_2enoses.ini because I didn't find the dataset 2006-12DEC-18-JavierOffice_2laser_2enoses.rawlog

I created a PR MRPT/mrpt#252.

[jlblancoc]

Hi @Logrus !

I think that one of the first things to think about, is deciding how many different ROS packages to build (or maybe just one?), and its interface in terms of input ROS msgs and such...

After all the experience you have gained these weeks, what are your thoughts about this?

It's strictly necessary to think and discuss a bit about this before getting hands on coding, as you can imagine :-)

[Logrus]

Hi @jlblancoc,

Currently I am exploring how ROS pf_localization is made by replicating parts of the code myself. I want to make my own wrapper for mrpt pf_localization by the end of the week, in order to get hands on experience on how to wrap mrpt for the ROS packages. I will probably be ready to discuss in details next steps after that. But from what I have learned so far, I think to start from building around CMetricMapBuilderRBPF class like rbpf-slam.cpp app does.

[jlblancoc]

Great! Let us know when you want to discuss some point or if you have several alternatives and want to learn our opinion.

[Logrus]

@jlblancoc Thanks for support!

I am looking into mrpt_ro_slam package made by Raffi and compare it to mrpt_localization that I've been exploring so far. There are a lot in common including stubs for handling laser data. So it seems logical to extend it to full functionality. Besides, it is already called RBPFSlamNode in code.
At the beginning however, it will be easier for me to start coding my own node and look into what is already there, but in the long run I think it make sense to update mrpt_ro_slam to mrpt_rbpf_slam. What is your opinion?
Then, this will also be in line with existing MRPT app and what was discussed on mrpt mailing list (the node should be able to fuse different sensor information).

Also, I have noticed that in MRPT after running rbpf-slam configs for the first time, in all consecutive runs the interactive window doesn't show up unless I remove LOG_* folder. Is it the desired behavior?

[jlblancoc]

I think it'll be perfect to start your own version from scratch, which is always easier than getting into someone's else code ;-)

Eventually, when the functionality of mrpt_ro_slam is superseded by yours, we might probably just delete it and update the docs.

On the LOG_* directory and the window... it doesn't make sense to me at all! If you mean that the "3D view" does not show up, you must know that there is a really old and pervasive bug (that only affects the Linux) with mrpt::gui windows such that, in rare occasions (apparently at random, so it must be related to a race condition), the windows will just not open.

To rule out this possibility, watch for this error message in cerr.

[Logrus]

Hi @jlblancoc!

Last week I went through the code in mrpt_localization package and mrpt_ro_slam. I started to code the package mrpt_rbpf_slam. So far I've done the visualization of the grid map slam from rawlog file in rviz.
Logrus/mrpt_slam@ea54fe6
It can be launched by the following the command:

roslaunch mrpt_rbpf_slam slam.launch 

This week I am also going to do visualization for ro slam in order to understand how to visualize the ro map in rviz.

When it comes to the structure of the package, currently I am thinking about the next one:

There will be just two files:
mrpt_slam.h (and mrpt_slam.cpp) - with functions:

bool read_iniFIle() - read ini file
void slam_run(actions, observations, &map, &pose) - one iteration of the slam which return map and pose
bool read_rawlog()  - read data from rawlog

mrpt_slam_wrapper.h (and mrpt_slam_wrapper.cpp) with functions:

bool get_param() -read parameters from the launch file
void init() - initialize topics and slam with parameters
void laserCallback - callback for the laser scans
void odomCallback - callback for the odometry
void mapUpdate - update map
void poseUpdate - update pose of the robot
void rawlog_replay - make slam from rawlog file 

My plan for this week:

Complete the visualization for ro slam for rawlog replay.
Make gridmap slam from rosbag (like online slam for ros).

I also have some questions:

1. It is not clear for me what is the purpose of the map server and map client in ROS. So far, I just output the gridmap to the topic and it works pretty good. But both the gmapping and mrpt_localization implementations use map server and client along with publishing to the topic.
2. I am thinking about synchronization of the laser scans and odometry data from callback functions. Because, for example, in some timesteps we can receive the laser scan without odometry. The logical solution would be just to look in the header.stamp in each message and collect only messages from laser scan and odometry with the same timestamp. Then use them for one iteration of the slam. But, if I am asking to output me the odometry and scans with exact same time stamp, then from 2000 data array only approx 200 has the same timestamp both in odom and laser scan. So that is clearly wrong way of doing it. Could you give any hints on how to sync odom and laser scans?
3. Concerning the structure of the code. Will it be okay to leave it like I've done it right now, or should I later change it to make it similar to mrpt_localization?

So that's it so far.
I am looking forward to your feedback!

[jlblancoc]

Good @Logrus !

In principle, go ahead with that structure and we'll see if it has to be changed later.
My main doubt there comes with "publish map". In MRPT we can have many different kinds of maps (beacons for RO, gridmap, pointcloud,...) so everything that in gmapping defaults to a "gridmap", should be translated here as a set of separate map topics, one for each map type. The C++ code could be general, taking a CMultiMetricMap as input and processing its stored metric maps one by one, for example.

On your doubts:

It is not clear for me what is the purpose of the map server and map client in ROS. So far, I just
output the gridmap to the topic and it works pretty good. But both the gmapping and
mrpt_localization implementations use map server and client along with publishing to the topic.

Neither it is to me! If it's simpler, just go on with a single SLAM package which, itself, is in charge of publishing the map to a topic and/or offering it via a service. Simpler = better.

I am thinking about synchronization of the laser scans and odometry data from callback functions. Because, for example, in some timesteps we can receive the laser scan without odometry. The logical solution would be just to look in the header.stamp in each message and collect only messages from laser scan and odometry with the same timestamp. Then use them for one iteration of the slam. But, if I am asking to output me the odometry and scans with exact same time stamp, then from 2000 data array only approx 200 has the same timestamp both in odom and laser scan. So that is clearly wrong way of doing it. Could you give any hints on how to sync odom and laser scans?

Easy solution: keep the latest odometry pose ("global pose in the odometry TF", in ROS parlance) in a variable and, whenever a new observation arrives (and the mapper is idle so it can accept it), build the pair observation + odometry increment, using a copy of the latest "global odometry pose", then update that copy with the new odometry pose. If the timestamp interval is small enough, the correction will be within the uncertainty model of the odometry, so everything should be OK.

Also: Take a look at this class. Just as a cool idea to be added after the basics work: that class takes localization/SLAM pose estimations and odometry, each at its own rate (odometry is typically much faster) and "compose them" and even, if needed, extrapolate the odometry, so one can ask the class "what is the robot pose now?" and you'll have a good estimation, not the one from the last time localization/SLAM did run. It will work great if odometry provides velocity, otherwise, it will default to the last odometry position but anyway will be more accurate than the last SLAM output, specially when SLAM is slow to run...

Concerning the structure of the code. Will it be okay to leave it like I've done it right now, or should I later change it to make it similar to mrpt_localization?

It's fine!

[jlblancoc]

Hi @Logrus,

Just wanted to hear about your progress with the new package. I didn't found it online on your GitHub fork, are you routinely committing and pushing??

Let us know if you need further advice on something from my last message.

Cheers,

[Logrus]

Hi Jose,

Sorry for the late reply.
Right now, I am trying to improve the speed of the online slam. it is a bit slow.
I made it in the way that each time when callback function receives the laser scan msgs or beacon msgs it will wait for odometry (here, I am using the same odometry update mechanism as in mrpt_localization) and then, after receiving the pair of observation and odometry, it executes one iteration of slam and publishes the map.

The problem is that this execution of one slam update takes some time (approx 0.5 sec), and during this time the callback function doesn't receive any messages.
Therefore in Rviz online slam works with jumps in the robot's position. I thought that it would be possible to collect all missed observation and odometry pairs in a vector and then, when slam function completes the job, send this vector for the next updates. However in this case this won't be an online slam, because the robot will always be ahead of the calculated position from slam.
So, I made a special function for playing the rawlog files which first gets all the action and observation pairs and then processes each and visualizes, because for slam from rawlog we don't need online slam.

To run the online grid map slam (played from rawlog file by using mrpt_rawlog node):

roslaunch mrpt_rbpf_slam rbpf_slam.launch
roslaunch mrpt_rbpf_slam rbpf_slam2.launch  

To run the online beacon slam:

roslaunch mrpt_rbpf_slam  rbpf_slam_ro.launch

To run replay slam from rawlog files:

roslaunch mrpt_rbpf_slam rbpf_slam_rawlog.launch
roslaunch mrpt_rbpf_slam ro_slam_rawlog.launch

In the last two launch files there is a parameter "rawlog_play_delay" which affects the speed of visualization of the slam.

I also tried online slam by using turtlebot simulator and compared it with gmapping:
Simulator: http://wiki.ros.org/turtlebot_simulator
Installation:

sudo apt-get install ros-indigo-turtlebot-simulator
sudo apt-get install ros-indigo-turtlebot-apps ros-indigo-turtlebot-rviz-launchers

The instruction for launching gmapping:
http://wiki.ros.org/turtlebot_gazebo/Tutorials/indigo/Make%20a%20map%20and%20navigate%20with%20it

For convenience, I will write all commands here:
Running gazebo:
roslaunch turtlebot_gazebo turtlebot_world.launch
or
roslaunch turtlebot_gazebo turtlebot_world.launch world_file:=worlds/willowgarage.world

Running robot key controls:
roslaunch turtlebot_teleop keyboard_teleop.launch
Running Rviz:
roslaunch turtlebot_rviz_launchers view_navigation.launch

Running SLAM:
mrpt-slam:
roslaunch mrpt_rbpf_slam rbpf_slam_turtlebot.launch
or gmapping:
roslaunch turtlebot_gazebo gmapping_demo.launch

That's it so far. I've updated my repo on github.
Logrus/mrpt_slam@8209a59

My plans right now are:
1.Try to improve the speed of the code.
2.Clean the code and also add doxygen comments.
3.Add the dynamically reconfigure server for motion model.
4.Add map server.

The goal is to prepare package for the pull request and the first submission to the end of the next week.

I also have a couple of questions:
How to retrieve the map_metadata? So far, I created the topic map_metadata but I didn't publish anything there.
Why do we need initial pose topic? I added the topic initialpose (like it is in the mrpt_localization node) but it is empty and robot localize itself without it. It took as the initial pose the first pose from the slam.

That's it so far! Thanks!

Best regards,
Vladislav

[jlblancoc]

Hi! I'm on a trip on the smartphone so I'll answer all later but just a
quick: take a look at my ros pkg 'mvsim' which allows making sims of lidar
& odom datasets from gridmaps. Just in case it's useful to help debugging!

---

Escrito desde Zapatófono, disculpe la brevedad.

[jlblancoc]

Right now, I am trying to improve the speed of the online slam. it is a bit slow.
I made it in the way that each time when callback function receives the laser scan msgs or beacon msgs it will wait for odometry (here, I am using the same odometry update mechanism as in mrpt_localization) and then, after receiving the pair of observation and odometry, it executes one iteration of slam and publishes the map.

But I don't understand why it always takes so long: there are some
parameters in RBPF-SLAM (the C++ class itself) for the "minimun
distance to run localization/SLAM", so in many timesteps (if odometry
is at high-rate as it's usual) the rbpf-slam class should return
almost immediately...

The problem is that this execution of one slam update takes some time (approx 0.5 sec), and during this time the callback function doesn't receive any messages.

Mmm... that's really serious! I think that then we should add a second
processing thread where SLAM runs. It shoudl take its input
(action+observation pairs) from a queue (e.g. std::deque or class
mrpt::utils::CThreadSafeQueue) where the main ROS thread writes to.

If we have to choose between getting a delayed solution to SLAM or an
incorrect solution because we are skipping sensor readings that may be
important... I think that the solution is clear, isn't it?? ;-)

Let me know if you need help or assistance with anything of this new thread.

Therefore in Rviz online slam works with jumps in the robot's position. I thought that it would be possible to collect all missed observation and odometry pairs in a vector and then, when slam function completes the job, send this vector for the next updates. However in this case this won't be an online slam, because the robot will always be ahead of the calculated position from slam.
So, I made a special function for playing the rawlog files which first gets all the action and observation pairs and then processes each and visualizes, because for slam from rawlog we don't need online slam.

I think that is a useful idea! Don't waste time trying to run "online"
if we are processing a dataset, right.
But this mode of working should be selected (vs. "online", reading
everything from ROS topics) via some parameter or roslaunch parameter,
right? How are you doing it right now?

For convenience, I will write all commands here:

That's it so far. I've updated my repo on github.
Logrus/mrpt_slam@8209a59

Great! I'll have to test them later since I don't have access right
now to a laptop with ROS.

My plans right now are:
1.Try to improve the speed of the code.
2.Clean the code and also add doxygen comments.
3.Add the dynamically reconfigure server for motion model.
4.Add map server.

The goal is to prepare package for the pull request and the first submission to the end of the next week.

Good!

I also have a couple of questions:
How to retrieve the map_metadata? So far, I created the topic map_metadata but I didn't publish anything there.

What's your starting point for this? Do you already know to access to
the grid map C++ class?
Some hints:

• See mapPDF within CMetricMapBuilderRBPF
• It's a mrpt::maps::CMultiMetricMapPDF object. Call its method
  CMultiMetricMap * getCurrentMostLikelyMetricMap ()
• Get the first gridmap in the CMultiMetricMap (check that there is at
  least one gridmap, as it will be not always the case!).
• Use the methods of the gridmap class to query the resolution, etc.

Why do we need initial pose topic? I added the topic initialpose (like it is in the mrpt_localization node)

In localization it makes sense, but in SLAM (in a first version, where
we will always assume that we are starting the map from scratch) it
does not. So, just drop that topic / parameter. The robot will always
start at (0,0,0) and it's OK like that.

Best,
JL

[Logrus]

Hi, Jose!

Thank you very much for so many good hints!

1. Regarding the speed of SLAM:

I will check my code maybe I am doing something inefficient (for example using variables isntead of pointers) in order to improve the speed of SLAM and I will also figure out how to add the second processing tread for SLAM (I think it is necessary anyway because it will increase the speed).

2. Regarding reading from rawlog:

Right now it is necessary to write the name of the rawlog file in the launch file:

<param name="rawlog_filename" value="$(find mrpt_rbpf_slam)/tutorial/grid_slam_demo_2.rawlog"/>

Then the slam node checks: if this file exists it will read from rawlog, otherwise it will run online slam.

3). Regarding the map_metadata topic:

Thank you for the hint I will go through gridmap c++ class and I will retrieve metadata after topic/map will receive the first map.

Best regards,
Vladislav

[jlblancoc]

On your point no. 2: it is perfect like that! Just dont forget documenting
it ;-)

I think you have permissions to edit the ros wiki, let us know if you need
any help with that.

El 13/06/2016 15:51, "Vladislav Tananaev" [email protected]
escribió:

Hi, Jose!

Thank you very much for so many good hints!

1. Regarding the speed of SLAM:

I will check my code maybe I am doing something inefficient (for example
using variables isntead of pointers) in order to improve the speed of SLAM
and I will also figure out how to add the second processing tread for SLAM
(I think it is necessary anyway because it will increase the speed).

2. Regarding reading from rawlog:

Right now it is necessary to write the name of the rawlog file in the
launch file:

Then the slam node checks: if this file exists it will read from rawlog,
otherwise it will run online slam.

3). Regarding the map_metadata topic:

Thank you for the hint I will go through gridmap c++ class and I will
retrieve metadata after topic/map will receive the first map.

Best regards,
Vladislav

—
You are receiving this because you were mentioned.
Reply to this email directly, view it on GitHub, or mute the thread.

[Logrus]

Hi Jose,

This week I cleaned the code, added map metadata and doxygen comments.
Logrus/mrpt_slam@9bdaf2b

I also mostly tried to increase the speed of the SLAM. I found out that the first three SLAM algorithms (0 - pfStandardProposal, 1 - pfAuxiliaryPFStandard, 2 - pfOptimalProposal) works online without any delay. So, you were right, when you said that the SLAM should output results almost immediately.
I made all my previous experiments with the most advanced slam algorithm (3 - pfAuxiliaryPFOptimal) which outputs the most accurate map but it also works slower than others.

So I made some videos in order to show you how everything works so far.
I am using a laptop and also I work on a virtual machine, because my host system windows and it doesn't support ROS.
The quality of the SLAM strongly depends on performance of the computer, but even on my virtualbox it works nicely! :)
Here the examples of online gridmap slam:
0 - pfStandardProposal:
https://www.youtube.com/watch?v=mUyTjJYCKBE
1- pfAuxiliaryPFStandard:
https://www.youtube.com/watch?v=PQXQV_iXHfI
2 - pfOptimalProposal:
https://www.youtube.com/watch?v=pJ5aWcSgq28
3 - pfAuxiliaryPFOptimal:
I didn't record the video because it works slow (with jumps between robot poses) and with video capture it works even worse.

Here the example of online gridmap slam from rawlog:
https://www.youtube.com/watch?v=7k35-VMAnsQ

Here the example of ro online slam:
https://www.youtube.com/watch?v=cnCWb8SMYpI
and the same ro slam from rawlog:
https://www.youtube.com/watch?v=DWql0i0Utok

Could you also try to run the SLAM on your computer in order to check how fast it works?

For convenience, here the commands:

To run online gridmap slam :

roslaunch mrpt_rbpf_slam rbpf_slam.launch

(You can change the type of the SLAM algorithm in ../mrpt_rbpf_slam/tutorial/grid_slam_demo.ini)

From rawlog:

roslaunch mrpt_rbpf_slam rbpf_slam_rawlog.launch

For ro slam:

roslaunch mrpt_rbpf_slam ro_slam.launch

from rawlog:

roslaunch mrpt_rbpf_slam ro_slam_rawlog.launch

And I didn't delete the turtlebot simulator launch file, because it is nice to try the slam on gazebo simulator with turtlebot:
roslaunch mrpt_rbpf_slam rbpf_slam_turtlebot.launch

So that is it so far. I am looking forward for your feedback regarding the speed of the slam (or anything else).

If everything is fine I will make a pull request and also update ros wiki about using SLAM with rawlog files.

I have a question concerning the rawlog file:
The name of the rawlog file can be specified in the ini file, but right now the slam node ignores the line with rawlog file name in ini file and checks only rawlog file specified in the launch file.
Is it okay?

Thanks!

[jlblancoc]

Great @Logrus ! Thanks for all the videos. I've been on an unexpected trip during the last week so I couldn't take an in-depth look at your code yet, but will do it ASAP and let you know about any pending issues to be solved before the midterm evaluation...

Cheers

[Logrus]

Hi @jlblancoc ,

I have completed the midterm evaluation form.
Please tell me what corrections or updates should be done additionally from my side.

Regards

[jlblancoc]

I just filled in your evaluation... so far, very good work! 👍

The rules said that you should create the PR but I tested everything in my computer and it builds ad runs cleanly, so we can work on the PR now after the midterm evaluation is done.

In general, all your code and example files are simply perfect as far as I saw. Just some small comments on the next steps:

• Please, check the other directories in the repo mrpt_slam for packages that now should be safe to remove. Ask me if you are unsure about anyone of them.
• The visualization of RO beacons is OK for debugging, but it should be great to show some real ellipsoids and/or number the beacons with their IDs. Time ago I did some searches and didn't found a way to draw an ellipsoid from a 3x3 cov matrix in RViz. Please, check it out and, if no better alternative is found, I could assist you in creating new functions (in mrpt_bridge) for creating RViz primitives from MRPT objects, etc.
• I tested everything in ROS kinetic and: good part = all methods build and work; bad part = the turtlebot simulator packages you mentioned seem to be dropped (?) in this release, or maybe their name changed. Please, take a look at this so you can write up-to-date instructions for the ROS wiki.
• After all this, create the PR and let's see if we have to tweak Travis to fix building, etc.

Keep up the hard work!

[Logrus]

Hi Jose!

Thank you very much for the kind feedback and good evaluation of my work.
I will do my best in order to complete my work sucessully.

Please, check the other directories in the repo mrpt_slam for packages that now should be safe to remove. Ask me if you are unsure about anyone of them.

Of course I will.

The visualization of RO beacons is OK for debugging, but it should be great to show some real ellipsoids and/or number the beacons with their IDs. Time ago I did some searches and didn't found a way to draw an ellipsoid from a 3x3 cov matrix in RViz. Please, check it out and, if no better alternative is found, I could assist you in creating new functions (in mrpt_bridge) for creating RViz primitives from MRPT objects, etc.

I will figure out the way how to make it this week.

I tested everything in ROS kinetic and: good part = all methods build and work; bad part = the turtlebot simulator packages you mentioned seem to be dropped (?) in this release, or maybe their name changed. Please, take a look at this so you can write up-to-date instructions for the ROS wiki.

The last supported version of the turtlebot simulator is Indigo (I am using ROS Indigo because this is so far one of the most stable version ). I will try to find alternative simulator for kinetic but I am not sure that something like that exists because I wasn't able to find turtlebot simulator even for Jade version.

After all this, create the PR and let's see if we have to tweak Travis to fix building, etc.

I will prepare everything to the end of this week and report to you here.

Thank you!

Best wishes,
Vladislav

[jlblancoc]

Take a look at mvsim package as an alternative simulator. Any feedback(or pull-request!) for it will be also welcome :-)

[Logrus]

Sure, I will do this. Thank you very much for the hint!

[Logrus]

Hi Jose,

I checked out the mvsim. And add the launch files into mvsim tutorial folder for slam. Here the video how it works:

https://www.youtube.com/watch?v=vEjsDGCdkjw

Concerning visualization for ro slam I haven't done it yet, because it is a bit harder than I thought.
So if you already know how to solve this with new mrpt_bridge node for visualization I will appreciate any help.

I also checked folders of the mrpt_slam. I deleted ro_slam but I am not sure about mrpt_icp_slam_2d package.
It looks like there is nothing inside except one binary file. Is it possible to delete it?

Concerning the ekf_slam_2d and ekf_slam_3d I think it would be nice from my side to make wrappers also for this two packages.
At the time when I made a GSOC proposal I didn't expect that the work will go so fast.
But because of a lot of helpful nodes like mrpt_bridge, also ro_slam I made almost everything that I wrote in my proposal. :)

During my work with mrpt library I saw so many stuff implemented inside. It is years of work. That is amazing!
So I think it is definitely necessary to make this great work available for the whole robotics community which mostly use ROS.
It would be nice to create one big ros package something like mrpt_stack where you can find mrpt_localization, mrpt_slam, and mvsim for ROS indigo, jade and Kinetic.
(Same like navigation stack). And it is necessary to add the tutorials starting from the installation and then examples of run slam localization in the simulator (mvsim) or from the rawlog or rosbag files.
I can also add the example of how to use move_base with mrpt stack.
In this case people will have almost complete robotics software, which can make SLAM, localization and motion planning and it is own simulator.

So if it will be ok, I can spend the rest of the GSOC on this tasks. Of course I will do everything that you ask me for rbpf slam.
So, what do you think?

[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 (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?

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! 👍

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 ;-)
MRPT/mrpt@7f50d63

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 Logrus/mrpt_navigation@3c4a247 and this Logrus/mrpt_slam@1d63d81. 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: MRPT/mrpt@512833e
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! 👍

[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