Added laser data from map in nav2_loopback_sim

[JatinPatil2003]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4616
Primary OS tested on	Humble
Robotic platform tested on
Does this PR contain AI generated software?	(No; Yes and it is marked inline in the code)

---

Description of contribution in a few bullet points

• Publishes realistic laser data according to the map

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.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

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

see 2 files with indirect coverage changes

[SteveMacenski]

Thanks for taking this on! Largely looks great but a few bugs / nits that are good to have resolved that I think makes it even better :-)

[JatinPatil2003]

Converting PR to draft for implementation of LineIterator

[SteveMacenski]

Thanks! Looks so much better already! :-)

[JatinPatil2003]

Thanks! Looks so much better already! :-)

@SteveMacenski Should I change PR status then?

[SteveMacenski]

I assume you tested this and see a reasonable laser scan that Nav2 can use? Just checking before I do my final pass through that 'this has shown to work' :-)

[JatinPatil2003]

nav2_loopback.mp4

This is working of nav2_loopback_sim, laser scan with nav2

[SteveMacenski]

I generally approve except for the findMapRange stuff which should use the line iterator

This is working of nav2_loopback_sim, laser scan with nav2

Generally looks good! I think something is possibly going on (?) since the laser scan appears to be flickering between 2 solutions, even when the robot is still. These should perfectly line up since there's no source of error. If there's no error/noise and the robot's TF doesn't move, this should be completely deterministic. I'm wondering if that's from something in the ray tracing that the line iterator should resolve or the call to TF. You use TF lookups to get the map->laser frame, but we do have map->odom and odom->base_link transforms in the simulator exactly. I suppose if you looked up at startup base_link->laser frame and cached that, then it should be 100% aligned (if that's the error cause)

[JatinPatil2003]

I generally approve except for the findMapRange stuff which should use the line iterator

This is working of nav2_loopback_sim, laser scan with nav2

Generally looks good! I think something is possibly going on (?) since the laser scan appears to be flickering between 2 solutions, even when the robot is still. These should perfectly line up since there's no source of error. If there's no error/noise and the robot's TF doesn't move, this should be completely deterministic. I'm wondering if that's from something in the ray tracing that the line iterator should resolve or the call to TF. You use TF lookups to get the map->laser frame, but we do have map->odom and odom->base_link transforms in the simulator exactly. I suppose if you looked up at startup base_link->laser frame and cached that, then it should be 100% aligned (if that's the error cause)

Yea it's not due to ray tracing, while testing with nav2 Itested with nav2_bringup so amcl is also publishing another tf

I think a parameter for publishing map to odom will be the solution for this

[SteveMacenski]

Did you try the line iterator method? Perhaps you didn't push this to origin so I could see

[JatinPatil2003]

I think the current ray tracking algorithm is working well as compared to LineIterator
This case is only when robot is near obstacle but works well when not too close

This image is using LineIterator

This Image is using current ray tracking algorithm

[SteveMacenski]

This image is using LineIterator

I like this in concept. I'm a little unclear why they seem to "stack up" to so much thickness of the wall -- shouldn't it have hit one of the previous cells before being basically on the other side of the wall's thickness? We do this in the collision footprint checker in cell coordinates with 0.5 cell steps -- maybe that's worth a try instead of using world coordinates? line_iterator = LineIterator(worldToMap(float(x0)), worldToMapfloat(y0)), worldToMap(x1), worldToMap(y1), 0.5). Maybe that makes it more reasonable?

This Image is using current ray tracking algorithm

I also like this in concept, but the arcing is a bit unnatural and I think would cause some odd behaviors with some localization algorithms. I think like the previous LineIterator comment, perhaps there's a way to adapt this to be more natural and create a more clean "line" behavior. For example (though maybe not the best solution):

• Find a ray that hits object
• If the next ray(s) hits the same cell, store it
• When the next cell is hit, take the stored rays and interpolate linearly between them

[JatinPatil2003]

This image is using LineIterator

This has been fixed. Now it is working perfectly

loopback.mp4

[SteveMacenski]

Thanks @JatinPatil2003 ! This is really nice to have + the option now for using other localizers with the 'perfect' data!