initJointTrajectory in ros_controllers ignoring first JointTrajectoryPoint in provided message
I am trying to initialize a (quintic spline) Trajectory using the functionality within ros_controllers/joint_trajectory_controller but am getting some undesired behavior when I sample the Trajectory constructed via joint_trajectory_control::initJointTrajectory.
In particular, when sampling times between the first (at time t=0.0) and second points, the spline returns always the second point's values (expected behavior for all times before the Trajectory's start time)
Additionally, when calling the initialization I get the following message:
Dropping first 1 trajectory point(s) out of ### as they occur before the current time...
It seems that the initialization function is throwing away my first point (with time_from_start = 0.0) despite my trajectory starting from ros::Time(0.0) as specified in the passed message.
Digging through the code, the comments make me think that this should work but it is not. Is there perhaps a strict less-than compare instead of a less-than-or-equal or a missing check/case for msg.header.time == 0.0?
I seem to remember from somewhere that ros_control (in contrast to MoveIt!) assumes that the proved trajectory does not contain the current position and I assume that this is related to my issue BUT I feel that there is inherently something wrong with this approach. In particular, what about the case (mine currently) where the provided trajectory_msg for initialization is significantly sparse in time such that a significant number of sampled times are between the current pose and the first spline segment?
