From 376f76cc9201efac5dcfc3439f07d394cba3cd07 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 28 Oct 2020 20:35:07 +0900 Subject: [PATCH 1/9] add test to check fetch-init --- jsk_fetch_robot/fetcheus/test/test-fetcheus.l | 3 +++ 1 file changed, 3 insertions(+) diff --git a/jsk_fetch_robot/fetcheus/test/test-fetcheus.l b/jsk_fetch_robot/fetcheus/test/test-fetcheus.l index 26fd089721..6178ed8752 100755 --- a/jsk_fetch_robot/fetcheus/test/test-fetcheus.l +++ b/jsk_fetch_robot/fetcheus/test/test-fetcheus.l @@ -126,6 +126,9 @@ (format nil ":go-velocity returns t with argument ~A" go-vel-arg)) )) +(deftest fetch-init-test + (let () + (fetch-init))) (run-all-tests) (exit) From 44db9ced144501dabcbec71ad6071621a65c8f69 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 28 Oct 2020 20:41:19 +0900 Subject: [PATCH 2/9] check apply_planning_scene before creating collision-object-publisher/*co* --- jsk_fetch_robot/fetcheus/fetch-interface.l | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index d2010473a7..a4ab236cc4 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -363,7 +363,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) From 57ca322a12bc74b84b54661ab4ad86e26225ac7e Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 18 Nov 2020 16:14:04 +0900 Subject: [PATCH 3/9] add test to check *ri* :anlge-vector .. :use-torso t --- jsk_fetch_robot/fetcheus/test/test-fetcheus.l | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/jsk_fetch_robot/fetcheus/test/test-fetcheus.l b/jsk_fetch_robot/fetcheus/test/test-fetcheus.l index 6178ed8752..b21b27cbbd 100755 --- a/jsk_fetch_robot/fetcheus/test/test-fetcheus.l +++ b/jsk_fetch_robot/fetcheus/test/test-fetcheus.l @@ -126,6 +126,12 @@ (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)) + )) + (deftest fetch-init-test (let () (fetch-init))) From f2d60e0167d8bd8b56f189765dddd978f27fc985 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 18 Nov 2020 16:14:27 +0900 Subject: [PATCH 4/9] remove :use-torso before sneding :angle-vector in :simulation-modep --- jsk_fetch_robot/fetcheus/fetch-interface.l | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index a4ab236cc4..75e926a3a5 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -95,6 +95,8 @@ (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)) From 85466df8b7b74c2f9737d3461bcc8776ad92a83e Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 18 Nov 2020 16:56:34 +0900 Subject: [PATCH 5/9] remove tabs --- jsk_fetch_robot/fetcheus/fetch-interface.l | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 75e926a3a5..0b7bc2fc41 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -96,7 +96,7 @@ ;; 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)))))) + (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)) @@ -368,7 +368,7 @@ Example: (send self :gripper :position) => 0.00" (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)) + (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))) From dc2ec56a7c9e1b2cc46592b5ece601a6aa07689b Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 18 Nov 2020 16:56:57 +0900 Subject: [PATCH 6/9] support :use-torso with angle-vector-sequence in kinematics simulator --- jsk_fetch_robot/fetcheus/fetch-interface.l | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 0b7bc2fc41..ae3940f9fa 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -127,6 +127,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)) From f479c02675ceb4b68a58d165e6c03b1fed27a35b Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 18 Nov 2020 16:57:26 +0900 Subject: [PATCH 7/9] refactor long line --- jsk_fetch_robot/fetcheus/fetch-interface.l | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index ae3940f9fa..582101802f 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -96,7 +96,9 @@ ;; 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)))))) + (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)) From e13c7e772a8e204d0bd4dca8e80ba0a694e8ddb0 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 18 Nov 2020 17:34:14 +0900 Subject: [PATCH 8/9] add test for angle-vector-sequence :use-torso --- jsk_fetch_robot/fetcheus/test/test-fetcheus.l | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/jsk_fetch_robot/fetcheus/test/test-fetcheus.l b/jsk_fetch_robot/fetcheus/test/test-fetcheus.l index b21b27cbbd..e593f3251d 100755 --- a/jsk_fetch_robot/fetcheus/test/test-fetcheus.l +++ b/jsk_fetch_robot/fetcheus/test/test-fetcheus.l @@ -130,6 +130,10 @@ (let () (setq *ri* (instance fetch-interface :init)) (assert (send *ri* :angle-vector (send *fetch* :angle-vector) 3000 :use-torso t)) + (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 From bf4f6628dce6f4050a148fc0f3decf87415bd324 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 18 Nov 2020 17:34:32 +0900 Subject: [PATCH 9/9] add assert message --- jsk_fetch_robot/fetcheus/test/test-fetcheus.l | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/jsk_fetch_robot/fetcheus/test/test-fetcheus.l b/jsk_fetch_robot/fetcheus/test/test-fetcheus.l index e593f3251d..39cae6c05c 100755 --- a/jsk_fetch_robot/fetcheus/test/test-fetcheus.l +++ b/jsk_fetch_robot/fetcheus/test/test-fetcheus.l @@ -129,7 +129,8 @@ (deftest fetch-use-torso (let () (setq *ri* (instance fetch-interface :init)) - (assert (send *ri* :angle-vector (send *fetch* :angle-vector) 3000 :use-torso t)) + (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)