Debug print function for RobotTrajectory by DLu · Pull Request #715 · moveit/moveit2

DLu · 2021-09-30T17:00:03Z

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

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

codecov · 2021-09-30T18:07:28Z

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.

moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h

moveit_core/trajectory_processing/src/trajectory_tools.cpp

DLu · 2021-09-30T20:37:56Z

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

moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h