Hi everyone,
in my application I would like to switch between manual navigation in areas outside the mapped environment, and autonomous navigation where the map is available. Hence I need to re-run global localization (and then scan localization) every time I enter the mapped area. There is no need to do it automatically, it can be triggered via web sight. I’ve tried using the behavior_tree components and the (deprecated) GroupSelectorBehavior.
I’m able to start (with the GroupSelectorBehavior) the localization behavior (global + scan localization) whenever I want, but only the first time. Then if I stop those nodes (with the GroupSelectorBehavior) and I try to restart the localization behavior I don’t understand what happens: the global localization node starts but it never succeeds/fails, and of course the scan localization doesn’t start.
It’s quite complicated to explain but I hope I was clear enough.
TL;DR: Is there a way to run/re-run the global GridSearchLocalizer when needed? (e.g. I manually drive the robot outside of the map and I need to relocalize it when I get back inside the map, or the scan localization gets too bad and I want to reset the robot pose with the global localization).
I believe there should be a way to do it with behavior_tree, but as you suggest a manual approach, I have an alternative solution that might work and be simpler:
Instead of restarting the GlobalLocalization, you can just force the scan_localization to ressed around the area you want:
In sight go to the settings of the Map View (right click on the map)
Go to the marker section in the bottom and add the robot_init_global_localizer, don’t forget to click Add marker before clicking update.
You should now see a marker on your map that you can move around, move it approximately where the robot is currently located. (I am assuming you know where the robot is).
Then in the config in sight (right panel), look for reseed_particles, check the box and click Submit
This will force the localizer to reseed itself neer the position of the marker. This might be simpler, though it assumes that you know the location of the robot and have some estimate of its orientation.
Let me know if it works for you, if not I will try to come with a setup to with behavior tree that will restart the global localizer.
Hi Ben,
thanks for your reply.
Actually reseeding the particle filter is what I’ve been doing until now so I wanted to improve a bit my application. Basically my robot has both gps outdoor (no map) navigation and Lidar indoor navigation (map). What I’m trying to do is to switch between the 2 navigation modes. The first one only uses my gps driver Codelet and a simple path planner, while the indoor needs the whole navigation stack. So what I would like to do is to start/stop a set of nodes when I switch between the two navigation modes. So when I’m in gps mode the nodes from the nav. stack are not started at all, and I start them when I change nav mode from the web sight interface. So far so good, I can stop the gps nodes and successfully start the lidar navigation (global localization included). Then I switch back to the gps mode (all the nodes of the nav stack are stopped; e.g. grid localizer, particle filter, global planner, map). So far so good. At this point, if I switch again to lidar navigation, map and global planner nodes start well while the global localizer starts but never succeds. I see it is ticking in the statistics but I never see the log “global localization finished in x seconds”.
Last but not least as a future improvement I would also like to automatically re-run global localization if needed (e.g. just like the LocalizeBehavior of the previous versions that was bugged LocalizeBehavior error in version 2019.3)
After looking more closely into the code our engineer working on Localization found it the issue. The GridSearchLocalizer is not reset properly in the start function and therefore can’t be run twice at the moment.
While he had already provided a fix, it’s unlikely to be made public until the next release.
There are some other improvements coming on the Localization for next release that should make it easier to restart the Localization.
Has this been fixed? I am also struggling with restarting the global localization using version 2020.2.
I am teleporting the robot in Isaac Sim using dynamic control and it seems like the localization does not update as in the Isaac Sight Map View the robot is always in the same position and does not teleport when the robot teleports in Isaac Sim.
I have tried to reseed the particle in the ParticleFilterLocalization but it still does not work.