trajectory speed is not constant, after sending cancel goal

[708yamaguchi]

when we send TrajectoryGoal with 1 positions, send cancel during execution, wait few more seconds, and then send the Trajectory again, the robot moves very quickly.

point2 = JointTrajectoryPoint()
point2.positions = [0,0,0,-1.57,1.57,0,0]
point2.time_from_start = rospy.Duration(10)
goal2.trajectory.points.append(point2)

rospy.logwarn("send goal2")
action.send_goal(goal2)
# sleeep 2                                                                                                                       
rospy.sleep(rospy.Duration(3))
rospy.logwarn("send cancel")
action.cancel_goal()
#                                                                                                                                
rospy.sleep(rospy.Duration(10))
rospy.logwarn("sleep 10sec")

# send goal 2                                                                                                                    
rospy.logwarn("send goal2 again")
action.send_goal_and_wait(goal2)

script to reproduce this behavior is https://gist.github.com/708yamaguchi/324b7ef246175619cc3fe7fe7e4bac76

and joint position profile during this experiment is

Our first guess is something related to https://github.com/fetchrobotics/robot_controllers/blob/indigo-devel/robot_controllers/src/follow_joint_trajectory.cpp#L400

Cc: @knorth55, @k-okada