if time-segment is not aligned, need to fix the data, adjust time and segment-time to exact position

Author: k-okada

irteus/irtutil.l

@@ -294,11 +294,18 @@
      (setq position (send self :interpolation))
      (incf time dt)
      (setq segment-time (- time (if (= segment 0) 0 (nth (1- segment) time-list))))
-     (when (> time (nth segment time-list))
-       (setq segment-time (- time (nth segment time-list)))
-       (incf segment))
+     (when (eps> time (nth segment time-list) (* 0.1 dt))
+       ;; if time-segment is not aligned, need to fix the data (see https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/457)
+       (while (and (< segment segment-num) (eps> time (nth segment time-list) (* 0.1 dt)))
+         (setq segment-time (- time (nth segment time-list)))
+         (incf segment)))
      (when (>= segment segment-num)
-       (setq position (car (last position-list)))
+       ;; adjust time and segment-time to exact position
+       (setq segment (1- segment-num))
+       (setq time (car (last time-list)))
+       (setq segment-time (- time (if (= segment 0) 0 (nth (1- segment) time-list))))
+       ;; re-calculate :interpolation
+       (setq position (send self :interpolation))
        (send self :reset))
      position))
   )