Merge branch 'master' into use-go-pos-unsafe

pr2eus/pr2-interface.l

@@ -429,6 +429,13 @@ Example: (send self :gripper :rarm :position) => 0.00"
             (max-av (fill (instantiate float-vector len-av)  180))
             (min-av (fill (instantiate float-vector len-av) -180))
             diff-av (l 0) dist div)
+       (if (atom tms) (setq tms (make-list (length avs) :initial-element tms)))
+       (cond
+	((< (length tms) (length avs))
+         (nconc tms (make-list (- (length avs) (length tms)) :initial-element (car (last tms)))))
+	((> (length tms) (length avs))
+	 (ros::ros-warn "length of tms should be the same or smaller than avs")
+	 (setq tms (subseq tms 0 (length avs)))))
        (dolist (av avs)
          ;; check if joint move more than 180 degree which has -inf/inf limit
          (setq diff-av (v- av prev-av))
@@ -439,13 +446,16 @@ Example: (send self :gripper :rarm :position) => 0.00"
            (ros::ros-warn "                       : ~A ~A" avs tms)
            (ros::ros-warn "  current angle vector : ~A" prev-av)
            (ros::ros-warn "new trajectory command : dvi = ~A" div)
-           (dotimes (i div)
-             (ros::ros-warn "                       : ~A ~A" (midpoint (/ (+ i 1.0) div) prev-av av) (/ (elt tms l) div)))
-           (dotimes (i (1- div))
-             (setq avs (list-insert (midpoint (/ (+ i 1.0) div) prev-av av) (+ l i) avs)))
-           (setf (elt tms l) (/ (elt tms l) div))
-           (dotimes (i (1- div))
-             (setq tms (list-insert (elt tms l) l tms)))
+           (let ((midtm (if (equal (elt tms l) :fast)
+                            :fast
+                            (/ (elt tms l) div))))
+             (setf (elt tms l) midtm)
+             (dotimes (i (1- div))
+               (let ((midpt (midpoint (/ (+ i 1.0) div) prev-av av)))
+                 (ros::ros-warn "                       : ~A ~A" midpt midtm)
+                 (setq avs (list-insert midpt (+ l i) avs))
+                 (setq tms (list-insert midtm l tms))))
+             (ros::ros-warn "                       : ~A ~A" av midtm))
            (incf l (1- div))
            (ros::ros-warn "new trajectory command :")
            (ros::ros-warn "                       : ~A ~A" avs tms)

pr2eus/robot-interface.l

@@ -10,6 +10,9 @@
 (ros::roseus-add-msgs "nav_msgs")
 (require :speak "package://pr2eus/speak.l")
 
+(defun shortest-angle (d0 d1)
+  (atan2 (sin (- d0 d1)) (cos (- d0 d1))))
+
 ;; add ros-joint-angle method using meter/radian
 (defmethod rotational-joint
   (:ros-joint-angle
@@ -478,10 +481,13 @@
          (dolist (av avs)
            (send robot :angle-vector av)
            (setq end-coords-current (mapcar #'(lambda (limb) (send robot limb :end-coords :copy-worldcoords)) limbs))
-	   (setq diff (v- (v- av (setq av (v+ av-prev (send self :sub-angle-vector av av-prev)))) diff-prev))
+           (setq diff (v- (v- av (setq av (v+ av-prev (send self :sub-angle-vector av av-prev)))) diff-prev))
            (setq target-limbs nil)
-	   (setq tm (elt tms i))
-	   (setq pass-tm (/ tm (float end-coords-interpolation-steps)))
+           (setq tm (elt tms i))
+           (if (equal tm :fast)
+               (setq tm (* 1000 (send self :angle-vector-duration
+                                      av-prev av scale min-time ctype))))
+           (setq pass-tm (/ tm (float end-coords-interpolation-steps)))
            (dotimes (l (length limbs))
              (setq ec-prev (elt end-coords-prev l) ec-current (elt end-coords-current l))
              (when (and ec-prev ec-current
@@ -728,6 +734,10 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
    (if (and warningp
 	    (not (yes-or-no-p (format nil "~C[3~CmAre you sure to move the real robot? (~A) ~C[0m" #x1b 49 (send action :name) #x1b))))
        (return-from :send-ros-controller nil))
+   (dolist (name joint-names)
+     (unless (send robot :joint name)
+       (warning-message 1 "[robot-interface.l] (send-ros-controller) could not find joint-name '~A' (~A)~%" name (send robot :joint name))
+       (return-from :send-ros-controller nil)))
    (let* ((goal (send action :make-goal-instance))
 	  (goal-points nil)
 	  (st (if (numberp starttime)
@@ -1464,7 +1474,7 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
    (cond
     ((or
       (equal (send move-base-action :get-state) ros::*simple-goal-state-active*)
-      (equal (send move-base-trajectory-action :get-state) ros::*simple-goal-state-active*))
+      (and move-base-trajectory-action (equal (send move-base-trajectory-action :get-state) ros::*simple-goal-state-active*)))
      (return-from :go-waitp t))
     (t
      (return-from :go-waitp nil))))

pr2eus/test/pr2-ri-test-arm.l

@@ -229,6 +229,19 @@
     (assert (< tm-diff 2) (format nil "end-coords-interpolation takes too long time ~A" tm-diff))
     ))
 
+(deftest test-angle-vector-fast
+  (send *pr2* :angle-vector (send *ri* :state :reference-vector))
+  (send *pr2* :r_wrist_roll_joint :joint-angle
+        (- (send *pr2* :r_wrist_roll_joint :joint-angle) 200))
+  ;; make sure :check-continuous-joint-move-over-180 is true
+  (assert (send *ri* :check-continuous-joint-move-over-180
+                (v- (send *ri* :state :reference-vector) (send *pr2* :angle-vector)))
+          "Prerequisites did not meet at test-angle-vector-fast")
+  ;; check for integer-expected errors when time is `:fast` and
+  ;; `:check-continuous-joint-move-over-180' is true.
+  (send *ri* :angle-vector (send *pr2* :angle-vector) :fast)
+  (send *ri* :wait-interpolation))
+
 (run-all-tests)
 (exit)
 

pr2eus/test/pr2-ri-test-base.l

@@ -9,9 +9,6 @@
               (substringp "violate max-angle" format))
     (apply #'warning-message-org color format mesg)))
 
-(defun shortest-angle (d0 d1)
-  (atan2 (sin (- d0 d1)) (cos (- d0 d1))))
-
 ;; initialize *pr2*
 
 (setq *pr2* (pr2))

pr2eus/test/pr2-ri-test-simple.l

@@ -55,9 +55,11 @@
     (assert (send *ri* :go-pos 1 0 0) "(send *ri* :go-pos 1 0 0)") ;; go-pos is relative to current position
     (assert (send *ri* :go-pos 0 1 90) "(send *ri* :go-pos 0 1 90)")
     (assert (send *ri* :go-pos-no-wait -1 1 -90) "(send *ri* :go-pos-no-wait -1 1 -90)")
+    (ros::sleep 1) ;; wait for 1 sec to activate goal status...
     (assert (send *ri* :go-waitp) "(send *ri* :go-waitp)")
     (assert (send *ri* :go-wait) "(send *ri* :go-wait)")
     (assert (eps-v= (send (send *ri* :worldcoords ) :worldpos) #f(0 0 0)))
+    (assert (send *ri* :go-pos-unsafe-no-wait -1 1 -90) "(send *ri* :go-pos-unsafe-no-wait -1 1 -90)")
     ))
 
 (deftest test-move-to
@@ -131,9 +133,9 @@
             (format nil "*ri* ~A, *pr2* ~A ~A~%"
                     (elt (send *ri* :state :potentio-vector) 5)
                     (elt (send *pr2* :angle-vector) 5)
-                    (eps= (elt (send *ri* :state :potentio-vector) 5) 60.0)))
+                    (eps= (elt (send *ri* :state :potentio-vector) 5) 60.0 1.0)))
       (warning-message 2 msg)
-      (assert (eps= (elt (send *ri* :state :potentio-vector) 5) 60.0) msg)
+      (assert (eps= (elt (send *ri* :state :potentio-vector) 5) 60.0 1.0) msg)
       )
     ))
 
@@ -149,9 +151,9 @@
             (format nil "*ri* ~A, *pr2* ~A ~A~%"
                     (elt (send *ri* :state :potentio-vector) 5)
                     (elt (send *pr2* :angle-vector) 5)
-                    (eps= (elt (send *ri* :state :potentio-vector) 5) -60.0)))
+                    (eps= (elt (send *ri* :state :potentio-vector) 5) -60.0 1.0)))
       (warning-message 2 msg)
-      (assert (eps= (elt (send *ri* :state :potentio-vector) 5) -60.0) msg)
+      (assert (eps= (elt (send *ri* :state :potentio-vector) 5) -60.0 1.0) msg)
       )
     ))
 

pr2eus/test/pr2-ri-test.launch

@@ -137,6 +137,8 @@
         observation_sources: base_scan
     </rosparam>
   </node>
+  <node name="tf_world_publisher" pkg="tf" type="static_transform_publisher"
+	args="0 0 0 0 0 0 /world /map 100" />
 
   <include if="$(arg launch_pr2_base_trajectory_action)"
            file="$(find pr2_base_trajectory_action)/launch/pr2_base_trajectory_action.launch" />
@@ -146,4 +148,6 @@
         args="$(find pr2eus)/test/pr2-ri-test-arm.l" time-limit="800" />
   <test test-name="pr2_ri_test_base" pkg="roseus" type="roseus" retry="1"
         args="$(find pr2eus)/test/pr2-ri-test-base.l" time-limit="800" />
+  <test test-name="pr2_ri_test_simple" pkg="roseus" type="roseus" retry="1"
+        args="$(find pr2eus)/test/pr2-ri-test-simple.l" time-limit="800" />
 </launch>

pr2eus_moveit/test/test-pr2eus-moveit.l

@@ -263,6 +263,38 @@
     (assert (< tm-diff 7) (format nil "start-offset-time is considered multiple times. Traj finishes at ~A" tm-diff))
     ))
 
+;; send target coords between blocks
+(deftest test-collision-object-publisher ()
+  (let ((l (make-cube 500 100 500))
+        (r (make-cube 500 100 500))
+        (co (instance collision-object-publisher :init))
+        (collision-check-result nil))
+    (send *ri* :angle-vector (send *pr2* :reset-pose))
+    (send *ri* :wait-interpolation)
+    (send l :locate #f(800 -200 700) :world)
+    (send r :locate #f(800 200 700) :world)
+    (send co :wipe-all)
+    (send co :add-object l)
+    (send co :add-object r)
+    ;; move left arm between blocks
+    (send *ri* :move-end-coords-plan (make-coords :pos #f(700 0 700)) :move-arm :larm)
+    (ros::rate 10)
+    (while (not (send *ri* :interpolatingp))
+      (send *ri* :spin-once)
+      (ros::sleep))
+    ;; check collision during interpolation
+    (while (send *ri* :interpolatingp)
+      (send *ri* :spin-once)
+      (send *pr2* :angle-vector
+            (send *ri* :state :potentio-vector :wait-until-update t))
+      (setq collision-check-result
+            (or collision-check-result
+                (pqp-collision-check-objects (send *pr2* :links) (list l r))))
+      (ros::sleep))
+    (ros::ros-info "collision occurred? -> ~A~%" collision-check-result)
+    (assert (not collision-check-result) "Collision occurred between pr2 and cubes")
+    ))
+
 (run-all-tests)
 (exit)