Fix servo trajectory point timestamping

[jliukkonen]

This PR removes trajectory point timestamping which causes warnings about trajectory points being dropped due to having old timestamps:

ros.joint_trajectory_controller: Dropping all 1 trajectory point(s), as they occur before the current time.
Last point is 0.000064s in the past.

By removing explicit timestamping, the stamps will default to 0, meaning the trajectory execution defaults to start now. For more details, see section 2.3: http://wiki.ros.org/joint_trajectory_controller

Small time from start is added to the trajectory point generated in suddenHalt to prevent warnings about trajectory point being 0.0000 seconds in the past (not sure if this should be considered as a bug or not, but it is clearly a corner case).

Finally, there is a tiny optimization added to the way twist command is passed to publisher from calculateTwistCommand function. It is not strictly related but felt too small for its own PR.

[codecov]

Codecov Report

Merging #2375 into master will decrease coverage by 0.07%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##           master    #2375      +/-   ##
==========================================
- Coverage   56.27%   56.21%   -0.06%     
==========================================
  Files         287      287              
  Lines       24450    24447       -3     
==========================================
- Hits        13757    13740      -17     
- Misses      10693    10707      +14     

Impacted Files	Coverage Δ
moveit_ros/moveit_servo/src/pose_tracking.cpp	47.02% <0.00%> (+0.31%)	⬆️
moveit_ros/moveit_servo/src/servo_calcs.cpp	58.72% <0.00%> (-0.41%)	⬇️
moveit_core/robot_model/src/joint_model_group.cpp	58.03% <0.00%> (-2.46%)	⬇️
...e/src/parameterization/model_based_state_space.cpp	70.40% <0.00%> (-2.39%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	67.36% <0.00%> (-0.47%)	⬇️

---

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 e6ca319...9a855af. Read the comment docs.

[AndyZe]

I think the same comment as well as explicitly setting the stamp to 0 would be good here:

// When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately"
// See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement
initial_joint_trajectory->header.stamp = ros::Time(0);

[AndyZe]

Will wait to +1 until we get some related issues sorted out on a PickNik project

[AndyZe]

This seems a little dangerous. Wouldn't we want to set it to 0, to ensure the joint_trajectory_controller begins stopping as soon as it receives the message? (per the same wiki section you mentioned, http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement)

[AndyZe]

Oh, never mind. That's a different message field (header.stamp). I think this is fine.

[AndyZe]

Please add a comment about why you did this.

[jliukkonen]

If time_from_start is zero and the trajectory start time is also zero, then it's all zeros and that leads to warnings about points being 0 seconds in the past (and like I said in the PR description, it's a bit weird). This is a way to work around that problem. Well, actually it makes perfect sense. I added a comment about why that 1ns is added as time_from_start.

[henningkayser]

This is actually a weird issue but your fix is a reasonable workaround.