search

MRPT / GSoC2016-discussions

See README for instructions
7 stars 2 forks source link

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

Closed jlblancoc closed 8 years ago

jlblancoc commented 8 years ago

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:

jlblancoc commented 8 years ago

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

Logrus commented 8 years ago

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 https://github.com/MRPT/mrpt/issues/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 commented 8 years ago

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):

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 commented 8 years ago

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 commented 8 years ago

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 commented 8 years ago

@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: https://github.com/Logrus/mrpt/commit/2ab542ace93f29545ebaf4dd585362bca8af8c8c

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 commented 8 years ago

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 :+1:
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 commented 8 years ago

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 commented 8 years ago

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:

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 https://github.com/MRPT/mrpt/pull/252.

jlblancoc commented 8 years ago

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 commented 8 years ago

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 commented 8 years ago

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

Logrus commented 8 years ago

@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 commented 8 years ago

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 commented 8 years ago

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. https://github.com/Logrus/mrpt_slam/commit/ea54fe6cb3b42f80051ced33fc360c5fae2e4e5c 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 commented 8 years ago

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 commented 8 years ago

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 commented 8 years ago

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. https://github.com/Logrus/mrpt_slam/commit/8209a59914dd1f254ca866c9b71f4c0ee6c7e5e4

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 commented 8 years ago

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 commented 8 years ago

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:

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 commented 8 years ago

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 commented 8 years ago

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" notifications@github.com 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 commented 8 years ago

Hi Jose,

This week I cleaned the code, added map metadata and doxygen comments. https://github.com/Logrus/mrpt_slam/commit/9bdaf2bc366d7a4276394c9b6f8af5fa53e2b836

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 commented 8 years ago

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 commented 8 years ago

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 commented 8 years ago

I just filled in your evaluation... so far, very good work! :+1:

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:

Keep up the hard work!

Logrus commented 8 years ago

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 commented 8 years ago

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

Logrus commented 8 years ago

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

Logrus commented 8 years ago

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 commented 8 years ago

I think that it sounds fantastic, Vladislav!! Thank you very much for the offer, indeed you did progress so fast that you will probably able to port the EKF-like SLAM nodes without much trouble.

The mvsim demo video is also great, nothing to add here since the video tells it all: it works quite solid!

On the idea of putting all into one "stack", that was my initial idea years ago but it seems that ROS "good practices" in OSRF packages tend to prefer splitting stuff into "functional" / "conceptual" smaller repositories, so I'm not concerned with navigation, SLAM and mvsim living in separate repos. It also helps to make releases more often, as one has only to push one or a few packages, not all of them even if no changes happened in most of them.

Regarding mrpt_icp_slam_2d, we could delete it or port the existing icp-slam application into a new node. The point is that ICP-SLAM is such a simple method (it can't close loops correctly) that may be actually of little utility for large maps, but may be the fastest solution for small maps like a few rooms without loops. That would be its niche. So: explore the existing code and examples for icp-slam (and icp-slam-live) in MRPT source tree, and if you see it easy to adapt as a ROS node, go for it without hesitation ;-)

Let me know (or mention me in your commits, etc.) whenever you have some implementation doubt.

Best, JL

jlblancoc commented 8 years ago

I forget about move_base: I would put all mapping stuff in a higher priority wrt to move_base. IMO, your work will be much more valuable and valued by the ROS community if you expend some time documenting all the new nodes and functions, augmenting existing / writing new tutorials, etc. instead of creating one more package related to motion planning... don't you agree?

Logrus commented 8 years ago

Sure, I agree. So, I will concentrate my efforts on writing wrappers for the EKF 2D/3D slam and icp slam. And then I will concentrate on writing the tutorials and examples for the related packages including mrpt_rbpf_slam.

Best wishes, Vladislav

Logrus commented 8 years ago

Hi Jose,

For the last week I've done the icp slam package here the video how it works online:

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

The command to run online icp slam:

roslaunch mrpt_icp_slam_2d icp_slam.launch

In order to run icp slam from rawlog file:

roslaunch mrpt_icp_slam_2d icp_slam_rawlog.launch

Is it ok that I visualize the position of the robot as the red arrow?

I also started to write the wrapper for EKF 3d slam you can run it from rawlog file so far:

roslaunch mrpt_ekf_slam_3d ekf_slam_3d_rawlog.launch

You can see that visualization is not completed (I just visualize the position of the landmarks as blue spheres with id and position of the robot with red sphere). Right now I want to find the way how to visualize covariance ellipses. My idea is to retrieve from the global covariance matrix the covariance for each landmark and use it to deform the sphere into ellipse (by using orientation and scale of the sphere marker).

I have a couple of questions about ekf slam implementation: The 2D ekf slam as observation should use 2d laser scans and 3D ekf slam should work with point cloud sensors (3D laser scans)? Where can I find the rawlog file for 2d ekf slam? On the mrpt web page there is an example video of 2d ekf slam. Could you, please, maybe upload the rawlog file from this example (or any other), because I need a dataset for testing and debugging 2d ekf slam. Thanks!

Best wishes, Vladislav

jlblancoc commented 8 years ago

Good! All I see in your sources is great so far, congrats!

Just a couple of minor points:

On your doubts: "2D/3D ekf slam" in MRPT are actually instantiations of range-bearing EKF SLAM. So the observations will be not scans, but range-bearing sets. See mrpt::obs::CObservationBearingRange.

Demo datasets can be found in MRPT/share/mrpt/datasets/kf-slam*.rawlog' and you can play generating your owns with thesimul-landmarks` app.

Take a look at all of them and let me know if you found more problems...

Best,

Logrus commented 8 years ago

Hi Jose, Thanks for advices!

I added point map to the icp slam. Here is the video: https://www.youtube.com/watch?v=E2HJJbIwnhc

For EKF 2D and 3D I added new class landmark.h to the mrpt_bridge package and also added the possibility to play rawlog files for EKF slam to mrpt_rawlog node. So, in order to run EKF slam you should update your mrpt_navigation package from my git repository. In all packages I implemented the possibility to run slam from rawlog or online and added the launch files with examples.

Here the example of the 2D EKF slam online: https://www.youtube.com/watch?v=_hy5LZQATVM

Here the example of 3D EKF slam online with wheeled robot (robot moves in 2D landmarks in 3D): https://www.youtube.com/watch?v=w3vraHunssI

I found one problem with implementation 3D EKF slam for flying robot. Here how it works so far: https://www.youtube.com/watch?v=g2JKOTB7mHw

As you can see it works incorrectly, map is always shaking and landmarks change the position. The problem here is that I am using the odometry model for 2D motion in order to update actions for slam. The models that I am using contained in the mrpt/obs/CActionRobotMovement2D.h class. I checked out the class mrpt/obs/CActionRobotMovement3D.h but I didn't find the models there. So, do you have this models somewhere else or should I add them?

Thank you!

Best wishes, Vladislav

jlblancoc commented 8 years ago

As usual, that's all great! You're really efficient :-)

Regarding EKF SLAM (2D or 3D), there is something related to visualization that is not only decorative but revealing of what is going on: the uncertainty of robot pose and/or landmarks.

I think that adding this would be the final top to your excellent contribution to GSoC.

How to render uncertanty in rviz? I don't think there are ready-to-use rviz markers for 2d-3d ellipsoids so far, so we may have to do them. One way is to add new functions in mrpt_bridge, taking 2x2 or 3x3 covariance matrices and returning visualization_msgs::MarkerArray which could be sent out to rviz. You can find details on Ellipsoid generation in mrpt::opengl, let me know if you need help locating it (I have to get off the computer now, sorry).

Best!

jlblancoc commented 8 years ago

Just a quick / formal reminder about the most important tasks regarding GSoC2016:

Happy coding!

Logrus commented 8 years ago

Hi Jose!

Thank you very much!

I added correct visualization for the 2D and 3D EKF slam. Here example of EKF 2D slam: https://www.youtube.com/watch?v=oRDCjiYGHF8

I added into the launch file the parameter ellipse_scale. This parameter allows you to change the scale of covariance ellipses. By default it is equal 1, which means that you will see the ellipses with 3 sigma covariance.

Here the example of EKF 3D slam for a wheeled robot (the robot moving in 2D ) and 3D landmarks: https://www.youtube.com/watch?v=68cSqH042zs

Here the example how it works in 3D from rawlog: https://www.youtube.com/watch?v=3100_ZCdl8s

And here how it works online (the same dataset): https://www.youtube.com/watch?v=OW6lZN9BcEg

As you can see, now it is more obvious that it is necessary to add the odometry motion model in class mrpt/obs/CActionRobotMovement3D.h

Best wishes, Vladislav

jlblancoc commented 8 years ago

Ah, ok! I see. Perfect usage of sphere primitives :+1: I've looking around and didn't found a programmatic way to set the default aspect of those spheres to "wireframes", which, I think, is normally more visually revealing than solid spheres to identify the orientation, etc.

Don't waste too much time in investigating that, just mentioning in case you already read somewhere a way to render RViz primitives as wireframes.

On the CActionRobotMovement3D problem: I think that class is seldom used across MRPT, so you can (and should!) improve it as desired to make it more useful. For example, something that lights a red warning to me is that there are two different fields for the pose change: poseChange and poseChangeQuat (!!). I don't want to run a git blame command because I'm afraid it was myself who committed that horrible design years ago! :-)

So: if you need that class for 3D SLAM, please, redesign it (in a fork + pull request) such that the internal motion is encoded in a private field of type CPose3DPDFGaussian and add the getter and setter methods as you need. You can also replicate a dual version of the "gaussian motion model" of the 2D motion class. Let me know (mention me in the commits?) whenever you need some advice in this process.

Don't be afraid if this breaks code in other apps (probably in RawLogViewer), we'll fix them. Fixing CActionRobotMovement3D is more important!

Best,

Logrus commented 8 years ago

Hi Jose!

Sorry for the late report.

For the last week I get an access to the prof. Burgard lab and made some experiments with real robot, and I am exciting to say that everything works properly! I need to process the video material and then I will upload it :)

Concerning the wireframes for the EKF slam visualisation I tried to find the proper way to do that, and It seems to me that there is no good way to make it. So, as a solution I can add a possibility to run MRPT display where you can see the EKF slam with wireframes. The same problem with visualisation for RO slam can be fixed like that. So far I don't have in mind a better solution.

Concerning the motion model, I found the model for 6DOF which could fit good for MRPT. Here the link to the article: http://ppniv12.irccyn.ec-nantes.fr/paper/3ballardini.pdf If this model will be fine I will use it.

Next week I will be on holidays, so I won't be able to work on the problems. But I will handle all of this tasks from 8 - 15 August, than make a pull request and update the wiki tutorials (my request to edit ROS wiki has recently been approved).

Thank you!

jlblancoc commented 8 years ago

For the last week I get an access to the prof. Burgard lab and made some experiments with real robot, and I am exciting to say that everything works properly! I need to process the video material and then I will upload it :)

That's brilliant! I'm longing to see the results on YouTube, but take your time to edit them properly, as I guess that you plan to insert them into the Wiki documentation, so they might be seen by many people over time ;-)

Concerning the wireframes for the EKF slam visualisation I tried to find the proper way to do that, and It seems to me that there is no good way to make it. So, as a solution I can add a possibility to run MRPT display where you can see the EKF slam with wireframes. The same problem with visualisation for RO slam can be fixed like that. So far I don't have in mind a better solution.

Good enough! It will be also an interesting alternative to have a "quick" visualization of what's going on without the need to set up a RViz scene (although I know you have already prepared .rviz files for each case).

Concerning the motion model, I found the model for 6DOF which could fit good for MRPT. Here the link to the article: http://ppniv12.irccyn.ec-nantes.fr/paper/3ballardini.pdf If this model will be fine I will use it.

I see it more than acceptable. You can start implementing it in a fork of MRPT / master and do an independent pull request for it.

Next week I will be on holidays, so I won't be able to work on the problems. But I will handle all of this tasks from 8 - 15 August, than make a pull request and update the wiki tutorials (my request to edit ROS wiki has recently been approved).

Great! Enjoy your well deserved holidays ;-)

Cheers, JL

Logrus commented 8 years ago

Hi Jose,

I made 4 pull requests for mrpt_slam, mrpt, mvsim, mrpt_navigation.

I added the motion model for 6DOF in mrpt and here the results for 3d EKF slam: https://www.youtube.com/watch?v=NNxBxWWpJAc

As you can see the map is still shaking and I haven't found the recommendations about how to adjust the coefficients for motion model (there are 10 different coefficients which significantly affect how the model works). But at least it looks consistent and the final map looks the same as from a rawlog file. Maybe this motion model is not so good. You can compare that result with rawlog: https://www.youtube.com/watch?v=3100_ZCdl8s

Concerning mrpt gui, I faced a problem when adding CDisplayWindow3D class. Somehow it shows me error "undefined reference to mrpt::gui::CDisplayWindow3D" but I double checked everything. MRPT library is linked properly... Maybe you have a clue what can cause a problem? I commented out the function init3Dwindow() in mrpt_icp_slam_2d_wrapper.cpp. If you uncomment it and compile, you will see this error message.

Concerning the ros wiki, could you please make the page for mrpt_slam for kinetic version with the empty pages for each type of the slam (mrpt_ekf_slam_2d,mrpt_ekf_slam_3d, mrpt_icp_slam_2d, mrpt_rbpf_slam ) and I will fill this pages with instructions and videos.

Thank you!

I also processed the video of the robot test and here the result: https://youtu.be/HfkRK3lwEgg

Best wishes, Vladislav

jlblancoc commented 8 years ago

Hi Vlad,

First of all, congrats again for the great work... and the cool videos! :-)

I'm far from having reviewed all the code, but I'll start writing here the small things to be fixed one by one, instead of waiting to have all of them, ok?

First think I found is a build error in Jenkins for mrpt-bridge: look for "error:" in: http://build.ros.org/job/Idev__mrpt_navigation__ubuntu_trusty_amd64/19/console

Probably just a missing using ... ??

jlblancoc commented 8 years ago

On the wiki pages, just ignore the trailing distro=XXX and create the page for the new pages as needed, starting from: http://wiki.ros.org/mrpt_slam

You'll see how part of the wiki data is updated automatically, from time to time (~ every few days) from the contents in the Git repos, so it might take a few days for, e.g. mrpt_rbpf_slam to appear in the upper menu, but nothing prevents you to create the page, start editing it, etc. Let me know if you have doubts during the process...

jlblancoc commented 8 years ago

On:

Concerning mrpt gui, I faced a problem when adding CDisplayWindow3D class. Somehow it shows me error "undefined reference to mrpt::gui::CDisplayWindow3D"

I'm in the middle of a release upgrade, so won't be able to check it in a few hours at least, but... you could double check that the -lmrpt-gui argument is being passed to the linker by running, from within catkin_ws/build/:

        VERBOSE=1 make

Edit: I think I found the problem: the correct variable for MRPT libs to link against is MRPT_LIBS, not MRPT_LIB (!!).

Please, change it in all CMakeLists.txt files and the problem should be gone.

PS: Bonus point: think about how it might be possible to the error to have undergone unnoticed until now ;-)

Logrus commented 8 years ago

First think I found is a build error in Jenkins for mrpt-bridge: look for "error:" in: http://build.ros.org/job/Idev__mrpt_navigation__ubuntu_trusty_amd64/19/console

Probably just a missing using ... ??

I think the problem in the file rawlog_play_node.cpp

In the beginning of the file you have a part:

#if MRPT_VERSION>=0x130
#    include <mrpt/obs/CSensoryFrame.h>
#    include <mrpt/obs/CRawlog.h>
#    include <mrpt/obs/CObservation2DRangeScan.h>
#    include <mrpt/obs/CObservationBeaconRanges.h>
#    include <mrpt/obs/CObservationBearingRange.h>
    using namespace mrpt::obs;
#else
#    include <mrpt/slam/CSensoryFrame.h>
#    include <mrpt/slam/CRawlog.h>
#    include <mrpt/slam/CObservation2DRangeScan.h>
#    include <mrpt/slam/CObservationBeaconRanges.h>
    using namespace mrpt::slam;
#endif

Here in the else part necessary to add #include <mrpt/slam /CObservationBearingRange.h> ,I think it should look like this :

#if MRPT_VERSION>=0x130
#    include <mrpt/obs/CSensoryFrame.h>
#    include <mrpt/obs/CRawlog.h>
#    include <mrpt/obs/CObservation2DRangeScan.h>
#    include <mrpt/obs/CObservationBeaconRanges.h>
#    include <mrpt/obs/CObservationBearingRange.h>
    using namespace mrpt::obs;
#else
#    include <mrpt/slam/CSensoryFrame.h>
#    include <mrpt/slam/CRawlog.h>
#    include <mrpt/slam/CObservation2DRangeScan.h>
#    include <mrpt/slam/CObservationBeaconRanges.h>
#    include <mrpt/slam/CObservationBearingRange.h> // <- Added
    using namespace mrpt::slam;
#endif
Logrus commented 8 years ago

Concerning MRPT_LIBS I added it and it works! :) so I will add the displays to all mrpt_slam packages and make another pull request. Thank you!

jlblancoc commented 8 years ago

Great, I'm happy the error is not blocking you anymore ;-)

On the ROS wiki pages: I could swear I did the pull requests via direct edits in the website yesterday, but it seems something went wrong and it's not there, so just created this second pull request: https://github.com/ros/rosdistro/pull/12386

After it gets accepted, it should be a matter of hours that the wiki pages got updated for "indigo", "jade" & "kinectic": http://wiki.ros.org/mrpt_slam

Logrus commented 8 years ago

Hi Jose!

I added the mrpt gui to each package and made a pull request. Here the video with results:

EKF_2D slam: https://youtu.be/tGMmi6Y6bL8

EKF_3d slam: https://youtu.be/8myjTFCanP8

icp slam: https://youtu.be/cMbI0Ca55gw

rbpf_slam: https://youtu.be/KLMrDmq3G5E

ro_slam: https://youtu.be/ydKxWQh6_qU

So right now I am going to change wiki pages from here:

http://wiki.ros.org/mrpt_slam

And I will create new page for rbpf slam.

Thanks!