Merge pull request #423 from knorth55/fix-tutorials

[pr2eus_moveit] fix collision-object-sample.l and :add-object in collision-object-publisher.l

pr2eus_moveit/euslisp/collision-object-publisher.l

@@ -105,6 +105,7 @@
        (let ((org-cds (send obj :copy-worldcoords)))
          (send obj :reset-coords)
          (send obj :worldcoords)
+         (mapcar #'(lambda (body) (send body :worldcoords)) (send obj :bodies))
          (let ((fs (body-to-faces obj))
                (geom (instance shape_msgs::mesh :init))
                idx-lst vertices (cntr 0))

pr2eus_moveit/tutorials/collision-object-sample.l

@@ -7,11 +7,12 @@
 
   (setq *table* (instance foldable-desk-object :init))
   (setq *bottle* (instance petbottle-object :init))
-  (send *bottle* :translate #f(-100.0 140.0 705.0))
+  (send *table* :translate #f(1000.0 0.0 0.0))
+  (send *bottle* :translate #f(900.0 140.0 705.0))
 
   (setq *robot* robot)
   (setq *link-name* link-name)
-  (objects (list *table* *bottle*))
+  (objects (list *robot* *table* *bottle*))
   )
 
 (defun pub-objects ()
@@ -26,9 +27,9 @@
 (defun pub-obj (obj &optional (robot *robot*) (link-name *link-name*)) ;; need link
   (let ((cds (send (send robot
                          (intern (format nil "~A_LK" (string-upcase link-name))
-                                 *keyword-package*) :worldcoords)
+                                 *keyword-package*) :copy-worldcoords)
                    :transformation (send obj :worldcoords))))
-    (send *co* :add-object obj :frame_id link-name :relative-pose cds)
+    (send *co* :add-object obj :frame-id link-name :relative-pose cds)
     obj))
 
 (warn "