Merge pull request #616 from k-okada/test_611

add test code for #611,

Author: k-okada

irteus/irtmodel.l

@@ -2577,18 +2577,19 @@
                                      (if (consp l) (deep-copy-list l) l))
                                  lst)))
                (setq print-args (deep-copy-list args)) ;; generate deep copy list for destructive operation
-               (dotimes (i (/ (length print-args) 2)) ;; escape list
-                 (unless (eq (elt print-args (* 2 i)) :move-target) ;; neglect :move-target because move-target is fixed later
-                   (escape-list print-args (+ 1 (* 2 i)))))
-               (dotimes (i (length print-args)) (escape-robot-link print-args i)) ;; escape robot link
-	       (dotimes (j (count :move-target print-args)) ;; escape move-target
-		 (if (setq i (position :move-target print-args :count (1+ j)))
-		     (cond ((atom (elt print-args (+ i 1)))
-                            (setf (elt print-args (+ i 1)) (get-move-target-transform-list (elt print-args (+ i 1)))))
-			   (t
-			    (setf (elt print-args (+ i 1))
-				  (append '(list)
-					  (mapcar #'(lambda (x) (get-move-target-transform-list x)) (elt print-args (+ i 1)))))))))
+               (dotimes (i (/ (length print-args) 2))
+                 (if (not (eq (elt print-args (* 2 i)) :move-target))
+                     ;; escape args other than :move-target
+                     (progn
+                       (escape-list print-args (+ 1 (* 2 i))) ;; escape list
+                       (escape-robot-link print-args (+ 1 (* 2 i)))) ;; escape robot link
+                     ;; escape :move-target
+                     (cond ((atom (elt print-args (+ 1 (* 2 i))))
+                            (setf (elt print-args (+ 1 (* 2 i))) (get-move-target-transform-list (elt print-args (+ 1 (* 2 i))))))
+                           (t
+                            (setf (elt print-args (+ 1 (* 2 i)))
+                                  (append '(list)
+                                          (mapcar #'(lambda (x) (get-move-target-transform-list x)) (elt print-args (+ 1 (* 2 i))))))))))
 	       (setq command-init `(instance ,(send (class self) :name) :init)
 		     command-setup `(progn  (send r :newcoords (make-coords :4x4 ,(send self :4x4))) (mapc #'(lambda (j a) (send* j :joint-angle a ,joint-args)) (list . ,(mapcar #'(lambda (x) `(send r ,(intern (string-upcase x) *keyword-package*))) (send-all (remove-duplicates (append (send-all union-link-list :joint) joint-list)) :name))) ,(list 'quote av0)) (objects (list r))))
                (setq command-args ;; append target-coords and print-args

irteus/test/test-irt-motion.l

@@ -1287,6 +1287,11 @@
         )
     )))
 
+;; test for https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/474, https://github.com/euslisp/jskeus/pull/611
+(deftest segfault-with-move-target-within-link
+  (let ((robot (instance sample-robot :init)))
+    (assert (null (send robot :rarm :inverse-kinematics (make-coords) :move-target (send robot :link :rleg-link5))))))
+
 (eval-when (load eval)
   (run-all-tests)
   (exit 0))