Merge branch 'master' into fetch-fix-dock

jsk-ros-pkg · Nov 20, 2020 · d1b661c · d1b661c
2 parents f7fa753 + 5330de6
commit d1b661c
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 1 deletion.
diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l
@@ -95,6 +95,10 @@
    (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args))))
    ;; for simulation mode
    (when (send self :simulation-modep)
+     (if (member :use-torso args)
+       (setq args (append (subseq args 0 (position :use-torso args))
+                          (if (> (length args) (+ 2 (position :use-torso args)))
+                            (subseq args (+ (position :use-torso args) 2))))))
      (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args)))
    ;;
    (when (not (numberp tm))
@@ -125,6 +129,10 @@
    (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args))))
    ;; for simulation mode
    (when (send self :simulation-modep)
+     (if (member :use-torso args)
+       (setq args (append (subseq args 0 (position :use-torso args))
+                          (if (> (length args) (+ 2 (position :use-torso args)))
+                            (subseq args (+ (position :use-torso args) 2))))))
      (return-from :angle-vector-sequence
                   (send* self :angle-vector-sequence-raw avs tms ctype start-time args)))
    (unless (and (listp tms) (every #'numberp tms))
@@ -363,7 +371,11 @@ Example: (send self :gripper :position) => 0.00"
 (defun fetch-init (&optional (create-viewer))
   (unless (boundp '*fetch*) (fetch) (send *fetch* :reset-pose))
   (unless (ros::ok) (ros::roseus "fetch_eus_interface"))
-  (unless (boundp '*co*) (setq *co* (instance collision-object-publisher :init)))
+  (unless (boundp '*co*)
+    (ros::ros-warn ";; collision-object-publisher wait for \"apply_planning_scene\" service for ~A sec~%" 5)
+    (if (ros::wait-for-service "apply_planning_scene" 5)
+      (setq *co* (instance collision-object-publisher :init))
+      (ros::ros-warn ";; could not find \"apply_planning_scene\" skip creating *co*~%")))
   (unless (boundp '*ri*) (setq *ri* (instance fetch-interface :init)))
 
   (ros::spin-once)

diff --git a/jsk_fetch_robot/fetcheus/test/test-fetcheus.l b/jsk_fetch_robot/fetcheus/test/test-fetcheus.l
@@ -126,6 +126,20 @@
             (format nil ":go-velocity returns t with argument ~A" go-vel-arg))
     ))
 
+(deftest fetch-use-torso
+  (let ()
+    (setq *ri* (instance fetch-interface :init))
+    (assert (send *ri* :angle-vector (send *fetch* :angle-vector) 3000 :use-torso t)
+            "failed to do :angle-vector with :use-torso in kinematics simulator")
+    (assert (send *ri* :angle-vector-sequence
+                  (list (send *fetch* :init-pose) (send *fetch* :angle-vector))
+                  (list 3000 3000) :use-torso t)
+            "failed to do :angle-vector-sequence with :use-torso in kinematics simulator")
+    ))
+
+(deftest fetch-init-test
+  (let ()
+    (fetch-init)))
 
 (run-all-tests)
 (exit)