Migrate Thruster Mapper

[DaniParr]

Describe the feature

Migrate https://github.com/uf-mil/mil/tree/master/NaviGator/gnc/navigator_thrust_mapper to ROS2.

Describe the solution you'd like

Ensure the solution is written in C++.

[mklingamsetty]

After looking through thruster.py, I believe I will have to look into using the Numpy equivalent for C++ which is the Eigen Library. Classes can be easily translated over from Python to C++, and so will functions, I believe the most difficult part would be to see and utilize equivalent C++ libraries and Python's libraries are more in number.

I'm looking into how to use ROS2, how to use C++ with ROS2, and now I will investigate how to use advanced mathematical libraries for C++, such as Eigen, and the ROS2 API for C++.

[mklingamsetty]

I'm currently unfamiliar with some of the math libraries and conventions in C++ that are needed for implementing the thruster mapping logic (e.g., vector/matrix operations, linear algebra), so I 'm reading into that. I'm also reading through the Python equivalent from the original MIL to better understand the logic and how to translate it.

[mklingamsetty]

Pushed on my branch, commenting here for re-iteration:
Branch: Migrate-Thrust-Mapper-Murali

Add preliminary ROS2 C++ thruster mapper

I added a new ROS2 C++ package, navigator_thrust_mapper, and implemented a preliminary node that converts high-level wrench commands into per‑thruster outputs. I also added a simple placeholder mapping (a basic 4‑thruster formula) and updated the build files so the package builds cleanly. This is an initial scaffold for testing and iteration — it’s functional but still preliminary. Next steps are to port the full Python mapping logic, replace the placeholder with a URDF‑based allocator, and add safety and hardware integration.

[mklingamsetty]

Pushed on my branch, commenting here for an issue update:
Branch: Migrate-Thrust-Mapper-Murali

Converted thrust_mapper.py with Eigen-based allocation

• Implement complete ThrusterMap class in C++ with all Python logic
• Add VRX force-to-command conversion functions (scalar and vectorized)
• Implement least-squares thrust allocation using Eigen SVD pseudoinverse
• Create linear force-to-command generator for custom scaling ratios
• Add comprehensive unit tests (thruster_map_test) verifying all conversions
• Build succeeds with no errors or warnings
• All 4 core test cases pass:
  • VRX force conversion across full range (-150 to 300 N)
  • Vectorized force processing
  • Linear force-to-command generation
  • Wrench-to-thrusts allocation with correct pseudoinverse computation
• ROS2 package properly configured with ament_cmake and Eigen3 dependency
• Executables: thruster_mapper_node (ROS2 node) and thruster_map_test (standalone)

[mklingamsetty]

Pushed on my Branch

Complete C++ port with kill alarm integration
Port thrust_mapper.py to C++ with full feature parity. Implements VRX inverse dynamics, SVD-based thrust allocation, kill alarm handling, and comprehensive unit tests. All tests passing, clean build.