Add motion_primitives_from_trajectory_controller with trajectory approximation using PTP/LIN primitives

[mathias31415]

This PR complements motion_primitives_forward_controller PR #1636 .
Instead of sending hardcoded motion primitives to the motion_primitives_forward_controller, this controller (motion_primitives_from_trajectory_controller) approximates a JointTrajectory using either PTP or LIN motion primitives.

This allows, for example, collision-free paths planned with MoveIt to be executed as motion primitives. The approximation is performed using the Ramer-Douglas-Peucker (RDP) algorithm:

• For PTP primitives in joint space
• For LIN primitives in Cartesian space

This PR is marked as a draft for the following reasons:

• It depends on the merge of the motion_primitives_forward_controller PR #1636
• Shared functionality between the two controllers should be refactored, e.g., into a motion_primitives_base_controller
• The package name might be reconsidered for clarity, e.g., renamed to motion_primitive_controllers

Even though this is still a draft, I would appreciate feedback on the current implementation.

[codecov]

Codecov Report

❌ Patch coverage is 20.09852% with 811 lines in your changes missing coverage. Please review.
✅ Project coverage is 81.25%. Comparing base (b21a7e1) to head (665079d).

Files with missing lines	Patch %	Lines
...c/motion_primitives_from_trajectory_controller.cpp	0.00%	368 Missing ⚠️
...ward_controller/src/approx_primitives_with_rdp.cpp	0.00%	158 Missing ⚠️
...oller/src/motion_primitives_forward_controller.cpp	38.46%	133 Missing and 19 partials ⚠️
motion_primitives_forward_controller/src/rdp.cpp	0.00%	92 Missing ⚠️
...on_primitives_forward_controller/src/fk_client.cpp	0.00%	20 Missing ⚠️
...r/motion_primitives_from_trajectory_controller.hpp	0.00%	13 Missing ⚠️
...test/test_motion_primitives_forward_controller.hpp	89.06%	3 Missing and 4 partials ⚠️
...test/test_motion_primitives_forward_controller.cpp	97.82%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1837      +/-   ##
==========================================
- Coverage   85.99%   81.25%   -4.74%     
==========================================
  Files         129      139      +10     
  Lines       13109    14124    +1015     
  Branches     1142     1284     +142     
==========================================
+ Hits        11273    11477     +204     
- Misses       1469     2256     +787     
- Partials      367      391      +24     

Flag	Coverage Δ
unittests	81.25% <20.09%> (-4.74%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...ontroller/motion_primitives_forward_controller.hpp	100.00% <100.00%> (ø)
...test_load_motion_primitives_forward_controller.cpp	100.00% <100.00%> (ø)
...test/test_motion_primitives_forward_controller.cpp	97.82% <97.82%> (ø)
...test/test_motion_primitives_forward_controller.hpp	89.06% <89.06%> (ø)
...r/motion_primitives_from_trajectory_controller.hpp	0.00% <0.00%> (ø)
...on_primitives_forward_controller/src/fk_client.cpp	0.00% <0.00%> (ø)
motion_primitives_forward_controller/src/rdp.cpp	0.00% <0.00%> (ø)
...oller/src/motion_primitives_forward_controller.cpp	38.46% <38.46%> (ø)
...ward_controller/src/approx_primitives_with_rdp.cpp	0.00% <0.00%> (ø)
...c/motion_primitives_from_trajectory_controller.cpp	0.00% <0.00%> (ø)

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[mathias31415]

I plan to split this PR into two separate PRs to make it easier to review:

1. A PR to extract the shared functionality of the motion_primitves_forward- and motion_primitives_from_trajectory- controllers into a base controller (not implemented yet).
2. A PR to add the motion_primitives_from_trajectory_controller.

Nevertheless, the motion_primitves_forward_controller PR should be merged first.

[mathias31415]

This PR has been replaced by PR #1857 and #1858 .