Debug print function for RobotTrajectory

[DLu]

Signed-off-by: David V. Lu davidvlu@gmail.com

Description

The ability to print a trajectory is useful and should not be limited to the one set of tests.

Checklist

• Required by CI: Code is auto formatted using clang-format

[codecov]

Codecov Report

Merging #715 (871ff48) into main (69b59b9) will increase coverage by 0.05%.
The diff coverage is 70.00%.

@@            Coverage Diff             @@
##             main     #715      +/-   ##
==========================================
+ Coverage   54.18%   54.22%   +0.05%     
==========================================
  Files         192      192              
  Lines       20187    20227      +40     
==========================================
+ Hits        10936    10966      +30     
- Misses       9251     9261      +10     

Impacted Files	Coverage Δ
...include/moveit/robot_trajectory/robot_trajectory.h	85.72% <ø> (ø)
...eit_core/robot_trajectory/src/robot_trajectory.cpp	23.13% <70.00%> (+7.39%)	⬆️
...bot_state/include/moveit/robot_state/robot_state.h	89.95% <0.00%> (+1.06%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 69b59b9...871ff48. Read the comment docs.

[v4hn]

Great idea! I added a lot of feedback inline.

[DLu]

Old Output:

trajectory length is 2.96513 seconds.
  Trajectory Points
  waypoint  0 time   0.00 pos   0.00 vel   0.00 acc   0.00 
  waypoint  1 time   0.07 pos   0.00 vel   0.03 acc   1.00 jrk  15.03
  waypoint  2 time   0.67 pos   0.20 vel   0.63 acc   1.00 jrk  -0.00
  waypoint  3 time   0.93 pos   0.40 vel   0.89 acc   1.00 jrk  -0.00
  waypoint  4 time   1.13 pos   0.60 vel   1.03 acc   0.34 jrk  -3.19
  waypoint  5 time   1.32 pos   0.80 vel   1.16 acc   1.00 jrk   3.53
  waypoint  6 time   1.48 pos   1.00 vel   1.24 acc  -0.00 jrk  -6.03
  waypoint  7 time   1.65 pos   1.20 vel   1.16 acc  -1.00 jrk  -6.03
  waypoint  8 time   1.83 pos   1.40 vel   1.03 acc  -0.34 jrk   3.53
  waypoint  9 time   2.04 pos   1.60 vel   0.89 acc  -1.00 jrk  -3.19
  waypoint 10 time   2.30 pos   1.80 vel   0.63 acc  -1.00 jrk  -0.00
  waypoint 11 time   2.90 pos   2.00 vel   0.03 acc  -1.00 jrk  -0.00
  waypoint 12 time   2.97 pos   2.00 vel   0.00 acc  -0.00 jrk  15.03

New Output:

Trajectory has 13 points over 2.97 seconds
  waypoint   0 time  0.00 pos 0.00 vel 0.00 acc 0.00 
  waypoint   1 time  0.07 pos 0.00 vel 0.03 acc 1.00 
  waypoint   2 time  0.67 pos 0.20 vel 0.63 acc 1.00 
  waypoint   3 time  0.93 pos 0.40 vel 0.89 acc 1.00 
  waypoint   4 time  1.13 pos 0.60 vel 1.03 acc 0.34 
  waypoint   5 time  1.32 pos 0.80 vel 1.16 acc 1.00 
  waypoint   6 time  1.48 pos 1.00 vel 1.24 acc -0.00 
  waypoint   7 time  1.65 pos 1.20 vel 1.16 acc -1.00 
  waypoint   8 time  1.83 pos 1.40 vel 1.03 acc -0.34 
  waypoint   9 time  2.04 pos 1.60 vel 0.89 acc -1.00 
  waypoint  10 time  2.30 pos 1.80 vel 0.63 acc -1.00 
  waypoint  11 time  2.90 pos 2.00 vel 0.03 acc -1.00 
  waypoint  12 time  2.97 pos 2.00 vel 0.00 acc -0.00 

[henningkayser]

Great idea! I definitely like the << overload

[tylerjw]

A couple minor nits, otherwise approve. Thank you for this.

[v4hn]

We should really finally implement moveit/moveit#1162 ...

[DLu]

Got it started #720

[v4hn]

great!

[v4hn]

I guess you should rebase to get this merged without admin access.
The repo is setup to require PR branches target the current main branch commit (not my favorite setting, but I didn't decide it).