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)
• [x] 🚀 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.

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

Additional comments

[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]

@DPR00 we should update the PR title.

[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]

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.

[DPR00]

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