Fix Wrong Map Pointer (#3311)

[borongyuan]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3311
Primary OS tested on	Ubuntu
Robotic platform tested on	Gazebo Simulation of TurtleBot3, Our Own Robot)

---

Description of contribution in a few bullet points

Not assigning fixed map pointer to particle filter, using latest when resample.

Description of documentation updates required from your changes

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mergify]

@borongyuan, please properly fill in PR template in the future. @SteveMacenski, use this instead.

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[SteveMacenski]

@mikeferguson can you let me know if this looks good to you? You have far more experience in the AMCL codebase than I.

[mikeferguson]

So, comparing this against ROS1, this looks like it will work - but there may be other issues lurking:

• In ROS1 - the function handleMapMessage() resets the particle filter instance entirely after loading the map
• In ROS2 - the function handleMapMessage() just updates the map, and does nothing else....

[SteveMacenski]

Ugh. The person that migrated AMCL to ROS 2 had the best intentions of cleaning things up, but ended up creating more issues than solving problems. I'll merge in this fix for @borongyuan but this may be something that needs to be revisited in more detail if any more issues come up.

It may be worth a re-port

[hidmic]

@SteveMacenski would you consider backporting this patch to Humble?

[SteveMacenski]

It changes function signatures -- which is an ABI breaking change. Generally I wouldn't backport something but if this resolves a crash I'm more open to it. Can you confirm that you can reproduce a crash that this fully resolves (and understand why it resolves it)?

[hidmic]

I can confirm, under the exact same conditions described in #3311, BUT I don't have an MRE to share at hand. I can try and craft one.

That said, the problematic execution path is more or less evident once you know where to look. The underlying particle filter is initialized on configure and upon dynamic re-parametrization, but the map gets clobbered and recreated every time a new occupancy grid gets published. It doesn't take much for the underlying particle filter to wind up with a non-owning reference to nothing, which doesn't become a problem unless adaptive resampling is enabled (and the particle filter tries to use that reference).

[SteveMacenski]

OK. Backport in progress.

AMCL is a codebase I haven't looked into for a few years. I read through it a few time as an exercise early in my career and intentionally avoided it afterwards for reasons I'm sure you and your team are all too familiar with ;-)