diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index d2010473a7..582101802f 100644 --- 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 index 26fd089721..39cae6c05c 100755 --- 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)