Add NDT AMCL 3D node.

[serraramiro1]

Proposed changes

Adds a new node for 3D NDT AMCL.
Closes #422
Closes #311
Closes #313

Type of change

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

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

[serraramiro1]

@hidmic ready for a first pass.
Also, this seems to not work on Jazzy, I'll TAL in the meantime.

I'll try to bring the coverage for the node a little bit higher.

[serraramiro1]

Running jazzy's container is giving me :

developer@23e2d3be126e:/ws$ colcon build --symlink-install --packages-up-to=beluga_amcl
Starting >>> beluga
--- 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.


---
Failed   <<< beluga [1.02s, exited with code 1]

Summary: 0 packages finished [1.19s]
  1 package failed: beluga
  1 package had stderr output: beluga
  2 packages not processed

Possibly related to #421 @hidmic do you think you can try to repro? I checked out main and getting the same

[hidmic]

First pass, great work.

[hidmic]

Second pass!

[hidmic]

How can I give this a shot locally @serraramiro1?

[serraramiro1]

How can I give this a shot locally @serraramiro1?

Try with the dataset and map I shared locally, and use:

Replace arguments to match your local env

ros2 launch beluga_example ndt_replay_3d.launch.xml localization_ndt_map:=/ws/src/beluga/1m_map.hdf5 bag_path:=/ws/src/beluga/rosbag2_2024_08_01-12_53_07

[hidmic]

Proof of life working