[fetcheus] add :add-headbox-collision-object and :delete-headbox-coll…

…ision-object

jsk_fetch_robot/fetcheus/fetch-interface.l

@@ -30,8 +30,10 @@
      (setq moveit-robot (instance fetch-robot :init))
      (send self :set-moveit-environment (instance fetch-moveit-environment :init :robot moveit-robot))
      (when (and (boundp '*co*) default-collision-object)
+       (send self :delete-headbox-collision-object)
        (send self :delete-keepout-collision-object)
        (send self :delete-ground-collision-object)
+       (send self :add-headbox-collision-object)
        (send self :add-keepout-collision-object)
        (send self :add-ground-collision-object))
      ))
@@ -196,6 +198,17 @@ Example: (send self :gripper :position) => 0.00"
     (send-super* :speak-en text :topic-name topic-name args))
   (:speak-jp (text &rest args &key (topic-name "robotsound_jp") &allow-other-keys)
     (send-super* :speak-jp text :topic-name topic-name args))
+  (:add-headbox-collision-object ()
+    (let ()
+      ;; fetch が reset-pose をとっていることが前提
+      (setq *fetch-headbox* (make-cube 100 201 120))
+      (send *fetch-headbox* :translate #f(0 0 1150))
+      (send *fetch* :head_pan_link_lk :assoc *fetch-headbox*)
+      (send *co* :add-attached-object *fetch-headbox* "head_pan_link"
+                 :frame_id "head_pan_link"
+                 :object_id "fetchheadbox")))
+  (:delete-headbox-collision-object ()
+    (send *co* :delete-attached-object-by-id "fetchheadbox"))
   (:add-keepout-collision-object ()
     (let ((cube (make-cube 200 350 50))
           (keepout (make-cylinder 300 50)))