Merge pull request #312 from k-okada/fix_310

add test for #310 (comment)

pr2eus_moveit/euslisp/robot-moveit.l

@@ -524,7 +524,7 @@
                     cds confkey :end-coords ed-lst args))
      ret))
   (:angle-vector-motion-plan ;;
-   (av &rest args &key (ctype controller-type) (start-offset-time) (total-time)
+   (av &rest args &key (ctype controller-type) (start-offset-time 0) (total-time)
        (move-arm :larm) (reset-total-time 5000.0) (use-send-angle-vector) (scale 1.0)
        &allow-other-keys)
    (let (traj ret orig-total-time sent-controller)
@@ -546,11 +546,11 @@
        (setq total-time
              (cond
                ((eq total-time :fast) (* scale (* orig-total-time 1000)))
-               ((or (null total-time) (> orig-total-time (/ total-time 1000))) (* orig-total-time 1000))
+               ((or (null total-time) (> orig-total-time (/ total-time 1000.0))) (* orig-total-time 1000))
                (t total-time)))
        (setq traj (send* self :trajectory-filter traj :start-offset-time start-offset-time :total-time total-time args))
-       (ros::ros-info ";; Scaled Trajectory Total Time ~7,3f [sec]" (/ total-time 1000))
-       (ros::ros-info ";; generated ~A points for ~A sec using [~A] group" (length (send traj :points)) (/ total-time 1000) (send ret :group_name))
+       (ros::ros-info ";; Scaled Trajectory Total Time ~0,3f(~0,3f) [sec]" (send (send (car (last (send traj :points))) :time_from_start) :to-sec) (/ total-time 1000.0))
+       (ros::ros-info ";; generated ~A points for ~A sec using [~A] group" (length (send traj :points)) (/ total-time 1000.0) (send ret :group_name))
        (ros::ros-info ";; will send to ~A" (send traj :joint_names))
        ;;
        (mapcar
@@ -600,7 +600,7 @@
          (send* self :send-trajectory traj args))
        )))
   (:trajectory-filter ;; simple trajectory for zero duration
-   (traj &key (copy) (total-time 5000.0) (minimum-time 0.001) (start-offset-time) (clear-velocities) &rest args &allow-other-keys)
+   (traj &key (copy) (total-time 5000.0) (minimum-time 0.001) (start-offset-time 0) (clear-velocities) &rest args &allow-other-keys)
    (let ((orig-total-time (send (send (car (last (send traj :points))) :time_from_start) :to-sec)))
    (when (and minimum-time (not start-offset-time)
               (> orig-total-time

pr2eus_moveit/test/test-pr2eus-moveit.l

@@ -108,6 +108,21 @@
     (assert (> tm-diff 3) (format nil "collsion avoidance motion is too fast ~A" tm-diff))
     ))
 
+;; do not send faster command than moveit output
+(deftest test-moveit-fastest-trajectory ()
+  (let (tm-0 tm-1 tm-diff)
+    (send *ri* :angle-vector (send *pr2* :reset-pose))
+    (send *ri* :wait-interpolation)
+    (send *pr2* :rarm :angle-vector #f(0 0 0 0 0 0 0))
+    (send *ri* :angle-vector-motion-plan (send *pr2* :angle-vector) :total-time 1000 :move-arm :rarm :use-torso nil)
+    (setq tm-0 (ros::time-now))
+    (send *ri* :wait-interpolation)
+    (setq tm-1 (ros::time-now))
+    (setq tm-diff (send (ros::time- tm-1 tm-0) :to-sec))
+    (ros::ros-info "time for duration ~A" tm-diff)
+    (assert (> tm-diff 3) (format nil "collsion avoidance motion is too fast ~A" tm-diff))
+    ))
+
 (run-all-tests)
 (exit)