Extending likelihood field to model unexplored spaces

[DPR00]

Proposed changes

The following changes are proposed in order to enhance the Likelihood Field Model based on the suggestions described on #55:

• First, a new function unknown_obstacle_data() is created to ideally replace the obstacle_data() function in beluga/sensor/data/OccupancyGrid.hpp. This new function will return a srd::tuple<bool, bool> containing the values which are occupied and unknown.
• Then, this new function will be passed to calculate the distance map using a new function nearest_obstacle_unknown_distance_map. This new function will ideally replace the nearest_obstacle_distance_map function.
• The value of the distance map for unknown values is assigned with a pre-computed value inside the nearest_obstacle_unknown_distance_map function. Based on this computed value, the likelihood will be $\frac{1}{z_{max}}$.

Some comments:

• It passed the compilation steps and the beluga example was tested. It would be nice to make some maps testing the draft. How could I perform that?

Type of change

• 🐛 Bugfix (change which fixes an issue)
• 🚀 Feature (change which adds functionality)
• 📚 Documentation (change which fixes or extends documentation)

💥 Breaking change! Explain why a non-backwards compatible change is necessary or remove this line entirely if not applicable.

Checklist

Put an x in the boxes that apply. This is simply a reminder of what we will require before merging your code.

• Lint and unit tests (if any) pass locally with my changes
• I have added tests that prove my fix is effective or that my feature works
• I have added necessary documentation (if appropriate)
• All commits have been signed for DCO

Additional comments

[hidmic]

First pass!

It would be nice to make some maps testing the draft. How could I perform that?

I recommend extending the likelihood field model unit tests for this next.

[DPR00]

Hi @hidmic! I'm trying to replicate locally the 'build_and_test.sh' file, since it is failing in the pipeline. I wanted to tested first before updating the draft PR. However, I'm getting the following error (similar to #422, but it actually build when using -colcon build --packages-up-to beluga_example --cmake-args -DBUILD_TESTING=OFF as indicated in Documentation):

-- Configuring incomplete, errors occurred!
---
--- stderr: beluga
CMake Error at test/CMakeLists.txt:30 (find_package):
  By not providing "Findbenchmark.cmake" in CMAKE_MODULE_PATH this project
  has asked CMake to find a package configuration file provided by
  "benchmark", but CMake did not find one.

  Could not find a package configuration file provided by "benchmark" with
  any of the following names:

    benchmarkConfig.cmake
    benchmark-config.cmake

  Add the installation prefix of "benchmark" to CMAKE_PREFIX_PATH or set
  "benchmark_DIR" to a directory containing one of the above files.  If
  "benchmark" provides a separate development package or SDK, be sure it has
  been installed.
---

This is happening in jazzy distro, but it actually works on humble. I also tried running it from the main branch, but it keeps throwing that error. Should I arise an issue?

[hidmic]

codecov isn't happy because we are lacking tests :)

[hidmic]

@DPR00 we should update the PR title.

[hidmic]

This is amazing @DPR00, great job!

We should add tests now though, for the likelihood model and for the action we just added.

[hidmic]

This is no draft anymore, it's solid PR material.

[DPR00]

Awesome @hidmic, thanks! I will be addressing the comments during the following days and adding tests, as well.
I also want to test it in a simulated world. Any recommendations of a world different than turtlebot_world?

[hidmic]

Third pass I think. Really good work @DPR00.

[hidmic]

This PR needs a rebase after #433.

[DPR00]

I did the rebase and reduce all the changes to one commit. The latter was due there were a lot of commits where the changes are no longer valid.

[hidmic]

I left a few more comments and CI is not happy, but this is a great piece of work @DPR00.

I'm happy with it once the missing bits are addressed and CI goes green (looks like clang-tidy is not happy).

[hidmic]

Plain amazing. I'm good with this PR as-is. @nahueespinosa ?

[nahueespinosa]

LGTM! After fixing forwarding references.

[nahueespinosa]

LGTM!

[hidmic]

@DPR00 go go go 🚀

[DPR00]

I am not able to merge it because I need to be an authorized user. Could you merge it @hidmic, pls? 🚀