Use adjustable waypoint batch sizes for Ruckig

[AndyZe]

Addresses #1439.

This feeds waypoints into Ruckig in batches of min(100 waypoints, 10% of waypoints).

• If you feed in too many waypoints at once, the output trajectory will have a longer duration than it needs to.
• If you feed in too few waypoints at once, and the trajectory is very long (like 100,000 waypoints), the runtime will be very slow.

Hopefully this is a happy medium. @swiz23 are you able to test this?

This can be tested easily by adding this line to moveit_resources/panda/ompl_planning.yaml, then run any of the motion planning MoveIt2 tutorials:

request_adapters: >-
default_planner_request_adapters/AddRuckigTrajectorySmoothing
default_planner_request_adapters/AddTimeOptimalParameterization
...

[codecov]

Codecov Report

Base: 50.28% // Head: 50.31% // Increases project coverage by +0.04% 🎉

Coverage data is based on head (b43248a) compared to base (9f7d6df).
Patch coverage: 95.13% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #1719      +/-   ##
==========================================
+ Coverage   50.28%   50.31%   +0.04%     
==========================================
  Files         374      374              
  Lines       31286    31325      +39     
==========================================
+ Hits        15728    15759      +31     
- Misses      15558    15566       +8     

Impacted Files	Coverage Δ
...rajectory_processing/src/ruckig_traj_smoothing.cpp	91.58% <94.60%> (+0.62%)	⬆️
...ics_plugin_loader/src/kinematics_plugin_loader.cpp	83.34% <100.00%> (+0.58%)	⬆️
...nning_scene_monitor/src/planning_scene_monitor.cpp	45.26% <0.00%> (-0.43%)	⬇️
moveit_core/robot_model/src/joint_model_group.cpp	65.36% <0.00%> (-0.24%)	⬇️
...e/collision_detection_fcl/src/collision_common.cpp	73.76% <0.00%> (-0.22%)	⬇️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[sjahr]

Looks good, just a couple of minor comments

[henningkayser]

Looks like this could use RobotTrajectory::append(). Same for the loop further up.

[AndyZe]

Good idea but I don't think that will work because of the dt argument. We might not have a constant dt.

  RobotTrajectory& append(const RobotTrajectory& source, double dt, size_t start_index = 0,
                          size_t end_index = std::numeric_limits<std::size_t>::max());

[henningkayser]

it's only the dt between the two trajectories. All other steps remain the same

[AndyZe]

👍 I replaced the second loop. The first loop --> [move_group-4] [WARN] [1671207429.602316971] [RobotTrajectory]: First and only waypoint has a duration of 0.

[AndyZe]

Sigh, I think there may be a bug in append()

[AndyZe]

Bug report here: #1813

[AndyZe]

Can we merge this in the meantime?