initialpose localization callback should reset scan buffer

[SteveMacenski]

No description provided.

[facontidavide]

It would be very awesome if you can also add a ros service to reset the scan buffer from the "outside".

[SteveMacenski]

Also a good idea, I'll do that at the same time. Maybe even let them reset it to a given size as well

[SteveMacenski]

Also if you're sufficiently motivated to submit PRs on any of this, let me know and I can work around anything you're intending to do

[facontidavide]

I will be happy to eventually contribute with actual PR.

I sent recently PRs to Cartographer, GMapping, Lego-LOAM, iris_lama_ros, etc...
I am telling you this to say that I am one of those users that is happy to collaborate proactively.

But in this particular case, i have the feeling that you know where to put your hands much better than I do ;)

[SteveMacenski]

GMapping

Wait 5 years for a review

... oh crap I think I'm a maintainer on that now

[SteveMacenski]

@facontidavide sorry it took me so long to revisit this, it took some time to shuffle around other ongoing work to get time to work on the localization work we had discussed. I earmarked next week so I should have my full attention on this ticket and #112. Hopefully we can come to a resolution to at least (if not more) half the convergence time from 26 seconds.

[SteveMacenski]

Action items

• create clear buffer method in karto
• expose to common
• advertise service in localization mode to clear
• clear on initialpose callback
• test both cases to make sure has desired effect
• port to ROS2 dashing and Eloquent

[SteveMacenski]

Work for ROS1 melodic on https://github.com/SteveMacenski/slam_toolbox/tree/initial_pose_reset_melodic

[facontidavide]

nice, I will test it

[SteveMacenski]

@facontidavide let me know how it goes! I don't see a reason it shouldn't work. I dont easily have access to right now to evaluate on, so let me know how it goes.

If you don't have any issues I'll PR it and migrate to ROS2 as well

[SteveMacenski]

I also just included the ported versions to Dashing and Eloquent if either interest you.

[SteveMacenski]

@facontidavide have you had a chance to look at it?

[SteveMacenski]

Pinging @facontidavide. It would be good to know if that branch helps / fixes your issues. It's my last action item before a new release

[facontidavide]

The next week I will collect a new dataset and it would be ideal for me to test this with that.
If you can wait, that would be nice, otherwise, go ahead :)

[SteveMacenski]

What’s a week amongst friends? 😉

[SteveMacenski]

@facontidavide hey, did you get a chance to test it?

[glucas350]

This feature is something I am interested in for a project I am currently working on. If you need someone to test it still, I am willing and able.

I am currently running Ubuntu 20.04 ROS noetic. I can merge this into the noetic branch on my side and test.

[SteveMacenski]

That would be great! Yes, please let us know your results and I can merge that PR into all the branches immediately if it helps.

[glucas350]

I just had a chance to look at this and realize this is not what I thought it was. I misread purpose of this PR.

I was looking for a way to reset the entire mapping occupancy grid during mapping to an initial empty state via a command.

[PGotzmann]

First of all, thank you for taking the time to add this feature. This is actually something that is very helpful for my use case.
Speaking of time, I figured it's really time to finally test this out and help you get this PR ready.

So I took a look at your PR (based on the current foxy-devel branch). I used Gazebo for this first test, but would also like to test this on a real robot in our lab afterwards.

Resetting the buffer basically works as expected and certainly has a positive effect on re-localization.
However, there is still a special case where not all nodes are removed from the graph:
If an initial position is specified at startup or a new pose estimate is published to \initialpose , the processor_type_ is set to PROCESS_NEAR_REGION in
LocalizationSlamToolbox::addScan() (slam_toolbox_localization.cpp#L125) only for the current scan. In this case, however, the scan is only added to the graph, not to the localization buffer, as would have been the case with PROCESS_LOCALIZATION (Mapper.cpp#L2723).
So when Mapper::ClearLocalizationBuffer() (initial_pose_reset_eloquent Mapper.cpp#L2910) is called, this node still remains in the graph.
The result is that the size of the graph increases with each call to \initialpose and the graph becomes polluted with (potentially mislocalized) scans.

I also found that the actual size of the scan buffer is scan_buffer_size + 1, since the check takes place before the new scan is added (Mapper.cpp#L2797). This is certainly not a big deal, but I would still like to point it out to you.

[SteveMacenski]

However, there is still a special case where not all nodes are removed from the graph:

Is that related to this patch or another issue altogether? Seems unrelated - we should fix it - but making sure we keep things organized. I see what you mean by this.

So when Mapper::ClearLocalizationBuffer() (initial_pose_reset_eloquent Mapper.cpp#L2910) is called, this node still remains in the graph.

Wouldn't that still be true regardless of this PR adding the clear buffer? I think what you're pointing out is that the first scan from /initialpose remains in the graph even as the code functions today (or I missed something?)

---

Resetting the buffer basically works as expected and certainly has a positive effect on re-localization.

Should this be merged in now then? I'm happy to update those branches and merge them into ROS2/Foxy/Melodic/etc - get one step fixed at a time unless there's something in those branches that should be resolved.

---

I also found that the actual size of the scan buffer is scan_buffer_size + 1, since the check takes place before the new scan is added (Mapper.cpp#L2797). This is certainly not a big deal, but I would still like to point it out to you.

Yeah I was generally aware of that when I was developing it, just forgot to fix it it looks, we could change to >= and we should be good to go

[PGotzmann]

Wouldn't that still be true regardless of this PR adding the clear buffer? I think what you're pointing out is that the first scan from /initialpose remains in the graph even as the code functions today (or I missed something?)

That is exactly what I meant. Now that you say it I also see that this is, at least from a code point of view, independent from this PR. I still think , however, it is the expected behavior to remove also this initial scan form the graph but this could of course be done in another PR.

---

Should this be merged in now then? I'm happy to update those branches and merge them into ROS2/Foxy/Melodic/etc - get one step fixed at a time unless there's something in those branches that should be resolved.

I can only comment on https://github.com/SteveMacenski/slam_toolbox/tree/initial_pose_reset_eloquent branch (which I rebased on the current foxy-devel branch). However, I could not find any other issues with that PR so I think it should be ready to be merged.
Here is a short log_file with scan_buffer_size=3 and some additional debug log messages, showing that the patch does what it is supposed to do. (You can also see here the increasing graph size with repeated \initialpose calls and the off-by-one error of the buffer size)

---

Yeah I was generally aware of that when I was developing it, just forgot to fix it it looks, we could change to >= and we should be good to go

This should indeed fix this, as long as scan_buffer_size = 0 is not a valid use case for you. In this case the actual buffer size would still be 1.

[SteveMacenski]

That is exactly what I meant. Now that you say it I also see that this is, at least from a code point of view, independent from this PR. I still think , however, it is the expected behavior to remove also this initial scan form the graph but this could of course be done in another PR.

I see you filed another ticket #333 about this - we can take it up there since its another topic.

This should indeed fix this, as long as scan_buffer_size = 0 is not a valid use case for you. In this case the actual buffer size would still be 1.

I agree its invalid - a PR for this would be appreciate across the different distributions to change to >=.

However, I could not find any other issues with that PR so I think it should be ready to be merged.

Awesome, will start that process and close this ticket once all of the branches contain it

• melodic
• noetic
• eloquent
• foxy
• ros2
• dashing

[SteveMacenski]

Thanks @facontidavide for bringing this topic up, sorry it took so long to get in / tested. All the branches have been updated for this new change!

[facontidavide]

and sorry for not doing any follow up ;)

As always, I move to other projects and I can't follow up what I started. Great addition!