Added Footprint Collision Checker (Simple Commander API)

[afifswaidan]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3151
Primary OS tested on	Ubuntu 22.04.01
Robotic platform tested on	None

---

Description of contribution in a few bullet points

• Added Footprint Collision Checker (Simple Commander API)
• Added one missing function in costmap_2d.py, it was needed in the footprint collision checker

---

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]

@afifswaidan, 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

[afifswaidan]

@SteveMacenski This is the initial commit, I still need to add documentation, docstrings and test cases. But I wanted to take your opinion if there are any specific test cases that I should implement?
Thanks!

[SteveMacenski]

LGTM for a first run, just a little cleanup and should be able to merge in soon!

[afifswaidan]

I added the doc strings, migrated to Polygon and Point32 messages, and renamed the validity function.
Once you check them let me know if I need to modify anything. After that i will implement the test cases and commit them.
Thanks! :)

[SteveMacenski]

Looked it all over and resolved the comments solved + added more recommendations!

[SteveMacenski]

any update? I would like this in before Christmas to button up that ticket for 2022 😄

[afifswaidan]

any update? I would like this in before Christmas to button up that ticket for 2022 😄

I was sick for the past week so I couldn't work or contribute. I already implemented some test cases, I'll continue the rest and commit on the weekend as usual. Is that okay? 😊
I believe we can finish it before Christmas for sure.

[SteveMacenski]

Awesome, totally understand. Feel better! I appreciate the help :-)

[SteveMacenski]

Awesome!

i think the last thing was the verified worldtomap None return for 1 call!

[afifswaidan]

Awesome!

i think the last thing was the verified worldtomap None return for 1 call!

This was already implemented a few commits back. Now the function returns None in case of invalid inputs, else, it returns the [X,Y] coordinates from world coordinates to map coordinates.

EDIT: Also I added this explanation in the worldToMapValidated function's docstring in the costmap_2d.py file.
Let me know what you think when you check it out! :)

[SteveMacenski]

We talked about changing that to worldToMapValidated -> [None, None] so that you only called the function 1 time and you check x, y for being none.

[afifswaidan]

We talked about changing that to worldToMapValidated -> [None, None] so that you only called the function 1 time and you check x, y for being none.

Ah yes correct, I modified the function but still kept calling it more than one time.
My bad. It is an easy fix, I will fix it and commit ASAP. I'll try my best to do it tomorrow or at least before the weekend.
Thanks for pointing it out again.

[SteveMacenski]

Last thing : the unit tests should have some that have non-zero costs in them to make sure things are working properly. If you make a 100x100 map with a 10x10 block of cost in the middle (or something) and then run these same tests to make sure we're picking things up correctly.

Then this is good to merge! Thanks!

[SteveMacenski]

nav2_simple_commander.test.test_footprint_collision_checker.TestFootprintCollisionChecker failed

test/test_footprint_collision_checker.py:67: in test_worldToMapValidated
    self.assertEqual(fcc_.worldToMapValidated(0, 5), None)
E   AssertionError: (None, None) != None

Also a linting error

AssertionError: Found 1 code style errors / warnings:
  ./nav2_simple_commander/footprint_collision_checker.py:70:1: W293 blank line contains whitespace
assert 1 == 0
test/test_flake8.py:23: in test_flake8
    assert rc == 0, \
E   AssertionError: Found 1 code style errors / warnings:
E     ./nav2_simple_commander/footprint_collision_checker.py:70:1: W293 blank line contains whitespace
E   assert 1 == 0

[SteveMacenski]

@afifswaidan I forgot - can you add docs https://navigation.ros.org/commander_api/index.html about this feature?

[SteveMacenski]

https://discourse.ros.org/t/nav2-costmap-collision-checking-python-api-added-to-nav2-simple-commander/28766

[afifswaidan]

@afifswaidan I forgot - can you add docs https://navigation.ros.org/commander_api/index.html about this feature?

Sure thing!
I have 2 question, I should add the documentation for this collision checker and for the line iterator that I did previously right?

My 2nd question is, its my first time of adding/modifying a docs website, how do you usually do it for ros2 nav2 docs? Sorry if its a basic question but I'd like to know how and do it!

Thanks :)

[afifswaidan]

https://discourse.ros.org/t/nav2-costmap-collision-checking-python-api-added-to-nav2-simple-commander/28766

Thank you so much for the credits!
I must say that contributing under your supervision/assistance was a pleasant and beneficial experience for me.

Please ping me whenever there are some pending issues that needs contributors, I've been working with ROS Navigation since 2017 and I hope I can be a good help for the development of ROS.

[SteveMacenski]

https://github.com/ros-planning/navigation.ros.org/blob/master/commander_api/index.rst Basically just modify this file under the Costmap API section to mention the collision checker and provide a link to it

[SteveMacenski]

Something that would be very helpful if you were interested is #3205. The idea is that if you create a while true loop, you may notice that the controller can run much faster than if you throttle it down to a set rate. The project is to try to find a way so that we can leverage more of that speed. It might be timer we use isn't working as we'd like, context switching of threads, I mis-diagnosed the issue when I was developing something else, etc.

[afifswaidan]

https://github.com/ros-planning/navigation.ros.org/blob/master/commander_api/index.rst Basically just modify this file under the Costmap API section to mention the collision checker and provide a link to it

Okay sure I'll do that either tomorrow or on Saturday.
Thanks for the help!

[afifswaidan]

Something that would be very helpful if you were interested is #3205. The idea is that if you create a while true loop, you may notice that the controller can run much faster than if you throttle it down to a set rate. The project is to try to find a way so that we can leverage more of that speed. It might be timer we use isn't working as we'd like, context switching of threads, I mis-diagnosed the issue when I was developing something else, etc.

Okay sure, I can look into it.
But can you give me some details about the timeline?
When is it expected to be finished?
Because next week I might not be fully available.

Thanks for telling me about it as well.

[SteveMacenski]

I don't expect much to get done from tomorrow to the end of the year from anyone 😆

When you have time and interest :-) If this doesn't interest you, there are plenty of things in the issue tracker and would very much appreciate contributions where you would have interest!

[afifswaidan]

I don't expect much to get done from tomorrow to the end of the year from anyone 😆

When you have time and interest :-) If this doesn't interest you, there are plenty of things in the issue tracker and would very much appreciate contributions where you would have interest!

Yeah exactly its the holidays and end of year season 😆

Okay I'll check it out and let you know. I'll check the other issues as well.

[afifswaidan]

Hello Steve,
I wish you a Merry Christmas!

I will be looking into all the stuff you told me about later in the first week of January 2023.
I noticed something that I wanted to ask you about.
In our last commit, a wrong email address was included in the details of the commit, can this be fixed somehow?
My email address is afifswaidan.as@gmail.com
My github username is afifswaidan
Some other account is linked to the commit I don't actually know why.
Thanks in advance!

[SteveMacenski]

Unfortunately, that’s to do with your git configurations and not something I can control. Perhaps its possible in Git to manually manipulate that, but it would be wrong (and possibly problematic legally) for me to do that as a representative of a company.

What we can do instead though is revert the commit so its out of the git history and you can resubmit a new PR under your preferred address. Its mostly problematic for me as a 3rd party to reassign the commit. For you, its fine if its more properly representative.

[afifswaidan]

Unfortunately, that’s to do with your git configurations and not something I can control. Perhaps its possible in Git to manually manipulate that, but it would be wrong (and possibly problematic legally) for me to do that as a representative of a company.

What we can do instead though is revert the commit so its out of the git history and you can resubmit a new PR under your preferred address. Its mostly problematic for me as a 3rd party to reassign the commit. For you, its fine if its more properly representative.

Thanks for for the quick reply.
Of course I don't want us to do anything that is potentially problematic.
So if I understood correctly, you revert back the repo, and I submit a new PR with corrected git configs?

[afifswaidan]

https://github.com/ros-planning/navigation.ros.org/blob/master/commander_api/index.rst Basically just modify this file under the Costmap API section to mention the collision checker and provide a link to it

I modified the file based on previous APIs (costmap and commander)
Let me know if anything else should be added.