Fix time stamp issues.

[tbolin]

motion_capture_system/mocap_qualisys/src/QualisysDriver.cpp

Line 132 in d5371e8

start_time_local_ = ros::Time::now().toSec();

The current calculation for the time stampa of a received 6DOF pose does not account for any delay in the link between the QTM server and receiver. Neither does it recognize that the delay can vary.
Suggestion 1: Update _start_time_local if a received time would cause the time stamp to lie in the future.
Suggestion 2: Add a token delay (around 1 ms) to the timestamp, to compensate for the delay.
Suggestion 3: Remove the weird smoothing found here:

motion_capture_system/mocap_qualisys/src/QualisysDriver.cpp

Line 138 in d5371e8

frame_interval = 0.6*frame_interval + 0.4*(packet_time - last_packet_time)/1e6;

[tbolin]

Another time smoothing issue in

motion_capture_system/mocap_base/src/KalmanFilter.cpp

Line 113 in 8e4d773

dt = dt*0.9 + msg_interval*0.1;