Free functions for calculating properties of trajectories

[tylerjw]

Co-authored-by: Sebastian Jahr [email protected] Signed-off-by: Tyler Weaver [email protected]

[codecov[bot]]

Codecov Report

Merging #1503 (c47dee7) into main (888fc53) will increase coverage by 0.09%. The diff coverage is 100.00%.

@@            Coverage Diff             @@
##             main    #1503      +/-   ##
==========================================
+ Coverage   50.98%   51.06%   +0.09%     
==========================================
  Files         380      380              
  Lines       31768    31802      +34     
==========================================
+ Hits        16193    16238      +45     
+ Misses      15575    15564      -11     

Impacted Files	Coverage Δ
...include/moveit/robot_trajectory/robot_trajectory.h	98.72% <ø> (ø)
...eit_core/robot_trajectory/src/robot_trajectory.cpp	67.06% <100.00%> (+3.13%)	:arrow_up:
...bot_state/include/moveit/robot_state/robot_state.h	90.14% <0.00%> (+0.19%)	:arrow_up:
moveit_core/robot_state/src/robot_state.cpp	47.91% <0.00%> (+0.55%)	:arrow_up:
moveit_core/robot_model/src/joint_model_group.cpp	64.87% <0.00%> (+0.99%)	:arrow_up:

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

[tylerjw]

@sjahr I lifted this work from your other PR. Would you mind cleaning up this PR and responding to Henning's comments? If you don't I'm happy to also respond to these when I'm back on monday.

--

@henningkayser about free functions and methods on RobotTrajectory, I think I could greatly simplify this code and make it more composable by changing it from a pair of deques to a single deque of two composed objects something like this:

template<typename T>
struct WithDuration {
  rclcpp::Duration duration;
  T value;
};

using RobotTrajectory = std::deque<WithDuration<RobotState>>;

Then I could implement all these method functions as free functions. Many of the implementations here (including functions in this PR) could be made simpler by making this conversion because you would have a first-class stl container with deque instead of this container with its own iterator implementation that is missing basic features needed to use stl algorithms.

This conversion could be done gradually with deprecation attributes but would likely not affect anyone as this function is primarily an internal interface of MoveIt. My other PR against this file begins that work.

There are many advantages to using free functions that operate on containers instead of classes.

As to the exact design of the base data type, I don't understand why a custom data type exists here that is not just a deque of the waypoint type from in the RobotTrajectory msg. It must be something about how the various ways this object is being constructed and something about how reasoning about RobotState objects is better than the basic message type that contains only joint angles.

I also don't understand why duration from last waypoint is used and not duration from start as in the RobotTrajectory msg. I need to read the code that uses this to try to better understand why this decision was made.