From 4638e1951a7583885af5f448bee7417f3260917a Mon Sep 17 00:00:00 2001 From: Yuto Uchimi Date: Thu, 26 Sep 2019 17:27:47 +0900 Subject: [PATCH 01/12] Fix return value of clear-costmap function --- pr2eus/robot-interface.l | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 1d2f0c99a..e8a2fd524 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -1744,8 +1744,11 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i "Send signal to clear costmap for obstacle avoidance to move_base." (let ((srv-name (format nil "~A/clear_costmaps" (send move-base-action :name)))) (if (ros::wait-for-service srv-name 0) - (call-empty-service srv-name))) - t) + (progn + (call-empty-service srv-name) + t) + nil)) + ) (:change-inflation-range (&optional (range 0.2) &key (node-name "/move_base_node") @@ -1880,13 +1883,16 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (defun clear-costmap (&key (node-name "/move_base_node")) "reset local costmap and clear unknown grid around robot" - (if (and (boundp '*ri*) (derivedp *ri* robot-move-base-interface)) - (send *ri* :clear-costmap) + (if (and (and (boundp '*ri*) (derivedp *ri* robot-move-base-interface)) + (send *ri* :clear-costmap)) + t ;; for backward compatibility (let ((srv-name (format nil "/~A/clear_costmaps" node-name))) (if (ros::wait-for-service srv-name 0) - (call-empty-service srv-name)))) - t) + (progn + (call-empty-service srv-name) + t) + nil)))) (defun change-inflation-range (&optional (range 0.2) &key (node-name "/move_base_node") From 766cd7fa11b8b03dadefde961eb82d2a26a7306d Mon Sep 17 00:00:00 2001 From: Yuto Uchimi Date: Wed, 2 Oct 2019 12:37:44 +0900 Subject: [PATCH 02/12] clear-costmap implicitly returns nil --- pr2eus/robot-interface.l | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index e8a2fd524..4cc3a0b73 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -1744,10 +1744,7 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i "Send signal to clear costmap for obstacle avoidance to move_base." (let ((srv-name (format nil "~A/clear_costmaps" (send move-base-action :name)))) (if (ros::wait-for-service srv-name 0) - (progn - (call-empty-service srv-name) - t) - nil)) + (call-empty-service srv-name))) ) (:change-inflation-range (&optional (range 0.2) @@ -1889,10 +1886,7 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i ;; for backward compatibility (let ((srv-name (format nil "/~A/clear_costmaps" node-name))) (if (ros::wait-for-service srv-name 0) - (progn - (call-empty-service srv-name) - t) - nil)))) + (call-empty-service srv-name))))) (defun change-inflation-range (&optional (range 0.2) &key (node-name "/move_base_node") From c728552c3cb3b05402964c5d7f5228debb091612 Mon Sep 17 00:00:00 2001 From: Yuto Uchimi Date: Wed, 2 Oct 2019 15:10:07 +0900 Subject: [PATCH 03/12] Avoid double waiting for service and add doc for return value in clear-costmap --- pr2eus/robot-interface.l | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 4cc3a0b73..7575497e4 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -1741,10 +1741,9 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i ) (:clear-costmap () - "Send signal to clear costmap for obstacle avoidance to move_base." + "Send signal to clear costmap for obstacle avoidance to move_base and return t if succeeded." (let ((srv-name (format nil "~A/clear_costmaps" (send move-base-action :name)))) - (if (ros::wait-for-service srv-name 0) - (call-empty-service srv-name))) + (call-empty-service srv-name)) ) (:change-inflation-range (&optional (range 0.2) @@ -1879,14 +1878,13 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i ;; (defun clear-costmap (&key (node-name "/move_base_node")) - "reset local costmap and clear unknown grid around robot" + "reset local costmap, clear unknown grid around robot and return t if succeeded" (if (and (and (boundp '*ri*) (derivedp *ri* robot-move-base-interface)) (send *ri* :clear-costmap)) t ;; for backward compatibility (let ((srv-name (format nil "/~A/clear_costmaps" node-name))) - (if (ros::wait-for-service srv-name 0) - (call-empty-service srv-name))))) + (call-empty-service srv-name)))) (defun change-inflation-range (&optional (range 0.2) &key (node-name "/move_base_node") From 0d6ed43f944da4a2aaaf21a334e3fcc97f6bd371 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 4 Dec 2019 15:09:01 +0900 Subject: [PATCH 04/12] add test for :go-stop, :go-pos, :go-pos-unsafe, :move-to --- pr2eus/test/pr2-ri-test.l | 150 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 150 insertions(+) diff --git a/pr2eus/test/pr2-ri-test.l b/pr2eus/test/pr2-ri-test.l index 3c1b554f8..becd637cf 100644 --- a/pr2eus/test/pr2-ri-test.l +++ b/pr2eus/test/pr2-ri-test.l @@ -23,6 +23,9 @@ joint-angle)) ) +(defun shortest-angle (d0 d1) + (atan2 (sin (- d0 d1)) (cos (- d0 d1)))) + ;; initialize *pr2* (setq *pr2* (pr2)) @@ -243,6 +246,153 @@ (assert (< tm-diff 2) (format nil "end-coords-interpolation takes too long time ~A" tm-diff)) )) +(deftest test-go-stop + (let ((x 0.3) (y 0.3) (d 90) + pos0 pos1) + (pr2-tuckarm-pose) + (ros::ros-info "send :go-pos ~A ~A ~A, assuming it takse longer than 2 sec" x y d) + (send *ri* :go-pos-no-wait x y d) + (send (instance ros::duration :init 2) :sleep) + (setq pos0 (send *ri* :state :worldcoords "map")) + (ros::ros-info "get current locateion ~A and send :go-stop" pos0) + (send *ri* :go-stop) + (unix:sleep 1) + (assert (equal (send (*ri* . move-base-action) :get-state) actionlib_msgs::GoalStatus::*preempted*)) ;; make sure go-pos is preempted + (setq pos1 (send *ri* :state :worldcoords "map")) + (setq diff-pos (norm (send pos0 :difference-position pos1))) + (ros::ros-info "get current locateion ~A, and diff position ~A" pos1 diff-pos) + ;; diff in 3cm + (assert (< diff-pos 30) (format nil "go-stop does not stop: ~A > 30" diff-pos)))) + +(defun go-pos-func (&key (x 0.3) (y 0.3) (d 90) (func :go-pos)) ;; [m] [m] [degree] + (let (pos0 pos1 pos2 diff-pos diff-rot tm0 tm1) + (pr2-tuckarm-pose) + (setq pos0 (send *ri* :state :worldcoords "map")) + (ros::ros-info "send ~A ~A ~A ~A" func x y d) + (setq tm0 (ros::time-now)) + (send *ri* func x y d) + ;; wait for 2 sec + (send (instance ros::duration :init 2) :sleep) + (setq tm1 (ros::time-now)) + (setq pos1 (send *ri* :state :worldcoords "map")) + (setq diff-pos (send pos0 :difference-position pos1)) + (setq diff-rot (send pos0 :difference-rotation pos1)) + ;; if func is -no-wait, make sure that :go-pos-* returns immediately but move forward + (when (substringp "NO-WAIT" (string func)) + (ros::ros-info "~A returns immediately ~A" func (send (ros::time- tm1 tm0) :to-sec)) + (ros::ros-info " difference-position: ~A -> ~A" diff-pos (norm diff-pos)) + (assert (< (abs (- (send (ros::time- tm1 tm0) :to-sec) 2.0)) 0.5)) + (assert (> (norm diff-pos) 25)) + (ros::ros-info "wait for result..") + ;; wait for result + (cond ((substringp "GO-POS-UNSAFE" (string func)) ;; FIXME + (send *ri* :go-pos-unsafe-wait)) + (t + (send *ri* :move-to-wait :retry 1 :frame-id (*ri* . base-frame-id)))) + (unix:sleep 1) + (setq pos1 (send *ri* :state :worldcoords "map")) + (setq diff-pos (send pos0 :difference-position pos1)) + (setq diff-rot (send pos0 :difference-rotation pos1)) + ) + (ros::ros-info "reached to the goal") + (ros::ros-info " difference-position: ~A -> ~A ~A" diff-pos (- (elt diff-pos 0) (* 1000 x)) (- (elt diff-pos 1) (* 1000 y))) + (ros::ros-info " difference-rotation: ~A -> ~A" diff-rot (shortest-angle (deg2rad d) (elt diff-rot 2))) + ;; diff in 10cm + (assert (< (abs (- (elt diff-pos 0) (* 1000 x))) 100) + (format nil "go-pos moves incorrectly in x axis: ~A != ~A" + (* 1000 x) (elt diff-pos 0))) + (assert (< (abs (- (elt diff-pos 1) (* 1000 y))) 100) + (format nil "go-pos moves incorrectly in y axis: ~A != ~A" + (* 1000 y) (elt diff-pos 1))) + (assert (< (abs (shortest-angle (deg2rad d) (elt diff-rot 2))) 0.8) ;; go-pos-unsafe-no-wait need 0.55... other cases fit within 0.1 + (format nil "go-pos moves incorrectly in d axis: ~A != ~A" + (deg2rad d) (elt diff-rot 2))) + )) + +(deftest test-go-pos + (go-pos-func :func :go-pos)) + +(deftest test-go-pos-no-wait + (go-pos-func :func :go-pos-no-wait)) + +(deftest test-go-pos-unsafe + (go-pos-func :func :go-pos-unsafe)) + +(deftest test-go-pos-unsafe-no-wait + (if (*ri* . move-base-trajectory-action) + (go-pos-func :func :go-pos-unsafe-no-wait) + (ros::ros-warn "SKIP TEST :go-pos-unsafe-no-wait requires move-base-trajectory-action"))) + + +(defun go-velocity-func (&key wait) + (let ((vel-x 1) (vel-y 1) (vel-d 1.57) ;; [m/s] [m/s] [rad/s] + pos0 pos1 pos2 pos3 tm0 tm1 diff-pos vel-diff-pos) + (pr2-tuckarm-pose) + (setq pos0 (send *ri* :state :worldcoords "map")) + (setq tm0 (ros::time-now)) + (ros::ros-info "(before go-velocity) current location ~A" pos0) + (ros::ros-info "send :go-velocity ~A ~A ~A :wait ~A" vel-x vel-y vel-d wait) + (send *ri* :go-velocity vel-x vel-y vel-d 3000 :wait wait) ;; go-velocity blocks for 3 sec if wait is t + (unless wait + (send (instance ros::duration :init 2) :sleep) + (setq tm1 (ros::time-now)) + (setq pos1 (send *ri* :state :worldcoords "map")) + (setq diff-pos (send pos0 :difference-position pos1)) + (setq diff-rot (send pos0 :difference-rotation pos1)) + (ros::ros-info ":go-velocity :wait ~A returns immediately ~A" wait (send (ros::time- tm1 tm0) :to-sec)) + (ros::ros-info " difference-position: ~A -> ~A" diff-pos (norm diff-pos)) + (assert (< (abs (- (send (ros::time- tm1 tm0) :to-sec) 2.0)) 0.5)) + (assert (> (norm diff-pos) 25)) + (send (*ri* . move-base-trajectory-action) :wait-for-result) + ) + (setq tm1 (ros::time-now)) + (unix:sleep 1) ;; make sure robot actually stops + (setq pos1 (send *ri* :state :worldcoords "map")) + (ros::ros-info "(after 1 sec ) current location ~A" pos1) + (setq diff-pos (norm (send pos0 :difference-position pos1))) + (setq vel-diff-pos + (* 1000 (norm (float-vector vel-x vel-y)) + (send (ros::time- tm1 tm0) :to-sec))) + (ros::ros-info " difference-position from position ~A" diff-pos) + (ros::ros-info " difference-position from velocity ~A (~A sec)" vel-diff-pos (send (ros::time- tm1 tm0) :to-sec)) + (assert (> vel-diff-pos diff-pos) + (format nil "go-velocity moves too much: ~A" diff-pos)) + )) + +(deftest test-go-velocity + (if (*ri* . move-base-trajectory-action) + (go-velocity-func) + (ros::ros-warn "SKIP TEST: default behavior of :go-velocity is :wait nil, and this requires move-base-trajectory-action"))) + +(deftest test-go-velocity-wait + (go-velocity-func :wait t)) + +(deftest test-move-to + (let ((x 0.3) (y 0.3) (d 1.57) ;; [m] [m] [rad] + pos0) + (pr2-tuckarm-pose) + (ros::ros-info "send :move-to ~A ~A ~A" x y d) + (send *ri* :move-to + (make-coords :pos (float-vector (* 1000 x) (* 1000 y) 0) + :rpy (float-vector d 0 0)) + :frame-id "map") + (unix:sleep 1) + (setq pos0 (send *ri* :state :worldcoords "map")) + (ros::ros-info "reached to the goal") + (ros::ros-info " difference-position: ~A -> ~A ~A" (send pos0 :worldpos) (- (elt (send pos0 :pos) 0) (* 1000 x)) (- (elt (send pos0 :pos) 1) (* 1000 y))) + (ros::ros-info " difference-rotation: ~A -> ~A" (send pos0 :worldrot) (shortest-angle d (elt (car (send pos0 :rpy-angle)) 0))) + ;; diff in 10cm + (assert (< (abs (- (elt (send pos0 :pos) 0) (* 1000 x))) 100) + (format nil "move-to moves incorrectly in x axis: ~A != ~A" + (* 1000 x) (elt (send pos0 :pos) 0))) + (assert (< (abs (- (elt (send pos0 :pos) 1) (* 1000 y))) 100) + (format nil "move-to moves incorrectly in y axis: ~A != ~A" + (* 1000 y) (elt (send pos0 :pos) 1))) + (assert (< (abs (shortest-angle d (elt (car (send pos0 :rpy-angle)) 0))) 0.1) + (format nil "go-pos-unsafe moves incorrectly in d axis: ~A != ~A" + d (elt (car (send pos0 :rpy-angle)) 0))) + )) + (run-all-tests) (exit) From f894489f2ee9c30a3b0ec80155a589fda945c76f Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 11 Dec 2019 21:24:43 +0900 Subject: [PATCH 05/12] support launch_pr2_base_trajectory_action argument for pr2-ri-test.launch --- pr2eus/test/pr2-ri-test.launch | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/pr2eus/test/pr2-ri-test.launch b/pr2eus/test/pr2-ri-test.launch index 8babc4271..b5d2ed08f 100644 --- a/pr2eus/test/pr2-ri-test.launch +++ b/pr2eus/test/pr2-ri-test.launch @@ -8,6 +8,9 @@ + + @@ -135,6 +138,9 @@ + + From 21235355d36c7c8679ba5a50cdf439c1ed438ad7 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 12 Dec 2019 01:34:47 +0900 Subject: [PATCH 06/12] increase time limit from 800 to 1200 --- pr2eus/test/pr2-ri-test.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pr2eus/test/pr2-ri-test.launch b/pr2eus/test/pr2-ri-test.launch index b5d2ed08f..a703f5065 100644 --- a/pr2eus/test/pr2-ri-test.launch +++ b/pr2eus/test/pr2-ri-test.launch @@ -143,5 +143,5 @@ + args="$(find pr2eus)/test/pr2-ri-test.l" time-limit="1200" /> From d7f0b70fc77f745a55fdcbed06a39f243e0813d0 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 12 Dec 2019 12:26:23 +0900 Subject: [PATCH 07/12] avoid print violate max/min-angle that exceeds 4M log limit --- pr2eus/test/pr2-ri-test.l | 26 ++++++-------------------- 1 file changed, 6 insertions(+), 20 deletions(-) diff --git a/pr2eus/test/pr2-ri-test.l b/pr2eus/test/pr2-ri-test.l index becd637cf..b515f3ea5 100644 --- a/pr2eus/test/pr2-ri-test.l +++ b/pr2eus/test/pr2-ri-test.l @@ -2,26 +2,12 @@ (load "package://pr2eus/pr2-interface.l") ;; avoid print violate max/min-angle that exceeds 4M log limit -(unless (assoc :joint-angle-org (send rotational-joint :methods)) - (rplaca (assoc :joint-angle (send rotational-joint :methods)) :joint-angle-org)) -(defmethod rotational-joint - (:joint-angle - (&optional v &key relative &allow-other-keys) - (let () - (when v - (when (and joint-min-max-table joint-min-max-target) - (setq min-angle (send self :joint-min-max-table-min-angle) - max-angle (send self :joint-min-max-table-max-angle))) - (if relative (setq v (+ v joint-angle))) - (cond ((> v max-angle) - (setq v max-angle))) - (cond ((< v min-angle) - (setq v min-angle))) - (setq joint-angle v) - (send child-link :replace-coords default-coords) - (send child-link :rotate (deg2rad joint-angle) axis)) - joint-angle)) - ) +(setf (symbol-function 'warning-message-org) + (symbol-function 'warning-message)) +(defun warning-message (color format &rest mesg) + (unless (or (substringp "violate min-angle" format) + (substringp "violate max-angle" format)) + (apply #'warning-message-org color format mesg))) (defun shortest-angle (d0 d1) (atan2 (sin (- d0 d1)) (cos (- d0 d1)))) From fab1480f0af529b284296395e3b917e1bbe9f18c Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 12 Dec 2019 15:08:04 +0900 Subject: [PATCH 08/12] split pr2-ri-test.l to pr2-ri-test-arm.l and pr2-ri-test-base.l --- .../test/{pr2-ri-test.l => pr2-ri-test-arm.l} | 150 --------------- pr2eus/test/pr2-ri-test-base.l | 177 ++++++++++++++++++ pr2eus/test/pr2-ri-test.launch | 6 +- 3 files changed, 181 insertions(+), 152 deletions(-) rename pr2eus/test/{pr2-ri-test.l => pr2-ri-test-arm.l} (61%) create mode 100644 pr2eus/test/pr2-ri-test-base.l diff --git a/pr2eus/test/pr2-ri-test.l b/pr2eus/test/pr2-ri-test-arm.l similarity index 61% rename from pr2eus/test/pr2-ri-test.l rename to pr2eus/test/pr2-ri-test-arm.l index b515f3ea5..244c863ad 100644 --- a/pr2eus/test/pr2-ri-test.l +++ b/pr2eus/test/pr2-ri-test-arm.l @@ -9,9 +9,6 @@ (substringp "violate max-angle" format)) (apply #'warning-message-org color format mesg))) -(defun shortest-angle (d0 d1) - (atan2 (sin (- d0 d1)) (cos (- d0 d1)))) - ;; initialize *pr2* (setq *pr2* (pr2)) @@ -232,153 +229,6 @@ (assert (< tm-diff 2) (format nil "end-coords-interpolation takes too long time ~A" tm-diff)) )) -(deftest test-go-stop - (let ((x 0.3) (y 0.3) (d 90) - pos0 pos1) - (pr2-tuckarm-pose) - (ros::ros-info "send :go-pos ~A ~A ~A, assuming it takse longer than 2 sec" x y d) - (send *ri* :go-pos-no-wait x y d) - (send (instance ros::duration :init 2) :sleep) - (setq pos0 (send *ri* :state :worldcoords "map")) - (ros::ros-info "get current locateion ~A and send :go-stop" pos0) - (send *ri* :go-stop) - (unix:sleep 1) - (assert (equal (send (*ri* . move-base-action) :get-state) actionlib_msgs::GoalStatus::*preempted*)) ;; make sure go-pos is preempted - (setq pos1 (send *ri* :state :worldcoords "map")) - (setq diff-pos (norm (send pos0 :difference-position pos1))) - (ros::ros-info "get current locateion ~A, and diff position ~A" pos1 diff-pos) - ;; diff in 3cm - (assert (< diff-pos 30) (format nil "go-stop does not stop: ~A > 30" diff-pos)))) - -(defun go-pos-func (&key (x 0.3) (y 0.3) (d 90) (func :go-pos)) ;; [m] [m] [degree] - (let (pos0 pos1 pos2 diff-pos diff-rot tm0 tm1) - (pr2-tuckarm-pose) - (setq pos0 (send *ri* :state :worldcoords "map")) - (ros::ros-info "send ~A ~A ~A ~A" func x y d) - (setq tm0 (ros::time-now)) - (send *ri* func x y d) - ;; wait for 2 sec - (send (instance ros::duration :init 2) :sleep) - (setq tm1 (ros::time-now)) - (setq pos1 (send *ri* :state :worldcoords "map")) - (setq diff-pos (send pos0 :difference-position pos1)) - (setq diff-rot (send pos0 :difference-rotation pos1)) - ;; if func is -no-wait, make sure that :go-pos-* returns immediately but move forward - (when (substringp "NO-WAIT" (string func)) - (ros::ros-info "~A returns immediately ~A" func (send (ros::time- tm1 tm0) :to-sec)) - (ros::ros-info " difference-position: ~A -> ~A" diff-pos (norm diff-pos)) - (assert (< (abs (- (send (ros::time- tm1 tm0) :to-sec) 2.0)) 0.5)) - (assert (> (norm diff-pos) 25)) - (ros::ros-info "wait for result..") - ;; wait for result - (cond ((substringp "GO-POS-UNSAFE" (string func)) ;; FIXME - (send *ri* :go-pos-unsafe-wait)) - (t - (send *ri* :move-to-wait :retry 1 :frame-id (*ri* . base-frame-id)))) - (unix:sleep 1) - (setq pos1 (send *ri* :state :worldcoords "map")) - (setq diff-pos (send pos0 :difference-position pos1)) - (setq diff-rot (send pos0 :difference-rotation pos1)) - ) - (ros::ros-info "reached to the goal") - (ros::ros-info " difference-position: ~A -> ~A ~A" diff-pos (- (elt diff-pos 0) (* 1000 x)) (- (elt diff-pos 1) (* 1000 y))) - (ros::ros-info " difference-rotation: ~A -> ~A" diff-rot (shortest-angle (deg2rad d) (elt diff-rot 2))) - ;; diff in 10cm - (assert (< (abs (- (elt diff-pos 0) (* 1000 x))) 100) - (format nil "go-pos moves incorrectly in x axis: ~A != ~A" - (* 1000 x) (elt diff-pos 0))) - (assert (< (abs (- (elt diff-pos 1) (* 1000 y))) 100) - (format nil "go-pos moves incorrectly in y axis: ~A != ~A" - (* 1000 y) (elt diff-pos 1))) - (assert (< (abs (shortest-angle (deg2rad d) (elt diff-rot 2))) 0.8) ;; go-pos-unsafe-no-wait need 0.55... other cases fit within 0.1 - (format nil "go-pos moves incorrectly in d axis: ~A != ~A" - (deg2rad d) (elt diff-rot 2))) - )) - -(deftest test-go-pos - (go-pos-func :func :go-pos)) - -(deftest test-go-pos-no-wait - (go-pos-func :func :go-pos-no-wait)) - -(deftest test-go-pos-unsafe - (go-pos-func :func :go-pos-unsafe)) - -(deftest test-go-pos-unsafe-no-wait - (if (*ri* . move-base-trajectory-action) - (go-pos-func :func :go-pos-unsafe-no-wait) - (ros::ros-warn "SKIP TEST :go-pos-unsafe-no-wait requires move-base-trajectory-action"))) - - -(defun go-velocity-func (&key wait) - (let ((vel-x 1) (vel-y 1) (vel-d 1.57) ;; [m/s] [m/s] [rad/s] - pos0 pos1 pos2 pos3 tm0 tm1 diff-pos vel-diff-pos) - (pr2-tuckarm-pose) - (setq pos0 (send *ri* :state :worldcoords "map")) - (setq tm0 (ros::time-now)) - (ros::ros-info "(before go-velocity) current location ~A" pos0) - (ros::ros-info "send :go-velocity ~A ~A ~A :wait ~A" vel-x vel-y vel-d wait) - (send *ri* :go-velocity vel-x vel-y vel-d 3000 :wait wait) ;; go-velocity blocks for 3 sec if wait is t - (unless wait - (send (instance ros::duration :init 2) :sleep) - (setq tm1 (ros::time-now)) - (setq pos1 (send *ri* :state :worldcoords "map")) - (setq diff-pos (send pos0 :difference-position pos1)) - (setq diff-rot (send pos0 :difference-rotation pos1)) - (ros::ros-info ":go-velocity :wait ~A returns immediately ~A" wait (send (ros::time- tm1 tm0) :to-sec)) - (ros::ros-info " difference-position: ~A -> ~A" diff-pos (norm diff-pos)) - (assert (< (abs (- (send (ros::time- tm1 tm0) :to-sec) 2.0)) 0.5)) - (assert (> (norm diff-pos) 25)) - (send (*ri* . move-base-trajectory-action) :wait-for-result) - ) - (setq tm1 (ros::time-now)) - (unix:sleep 1) ;; make sure robot actually stops - (setq pos1 (send *ri* :state :worldcoords "map")) - (ros::ros-info "(after 1 sec ) current location ~A" pos1) - (setq diff-pos (norm (send pos0 :difference-position pos1))) - (setq vel-diff-pos - (* 1000 (norm (float-vector vel-x vel-y)) - (send (ros::time- tm1 tm0) :to-sec))) - (ros::ros-info " difference-position from position ~A" diff-pos) - (ros::ros-info " difference-position from velocity ~A (~A sec)" vel-diff-pos (send (ros::time- tm1 tm0) :to-sec)) - (assert (> vel-diff-pos diff-pos) - (format nil "go-velocity moves too much: ~A" diff-pos)) - )) - -(deftest test-go-velocity - (if (*ri* . move-base-trajectory-action) - (go-velocity-func) - (ros::ros-warn "SKIP TEST: default behavior of :go-velocity is :wait nil, and this requires move-base-trajectory-action"))) - -(deftest test-go-velocity-wait - (go-velocity-func :wait t)) - -(deftest test-move-to - (let ((x 0.3) (y 0.3) (d 1.57) ;; [m] [m] [rad] - pos0) - (pr2-tuckarm-pose) - (ros::ros-info "send :move-to ~A ~A ~A" x y d) - (send *ri* :move-to - (make-coords :pos (float-vector (* 1000 x) (* 1000 y) 0) - :rpy (float-vector d 0 0)) - :frame-id "map") - (unix:sleep 1) - (setq pos0 (send *ri* :state :worldcoords "map")) - (ros::ros-info "reached to the goal") - (ros::ros-info " difference-position: ~A -> ~A ~A" (send pos0 :worldpos) (- (elt (send pos0 :pos) 0) (* 1000 x)) (- (elt (send pos0 :pos) 1) (* 1000 y))) - (ros::ros-info " difference-rotation: ~A -> ~A" (send pos0 :worldrot) (shortest-angle d (elt (car (send pos0 :rpy-angle)) 0))) - ;; diff in 10cm - (assert (< (abs (- (elt (send pos0 :pos) 0) (* 1000 x))) 100) - (format nil "move-to moves incorrectly in x axis: ~A != ~A" - (* 1000 x) (elt (send pos0 :pos) 0))) - (assert (< (abs (- (elt (send pos0 :pos) 1) (* 1000 y))) 100) - (format nil "move-to moves incorrectly in y axis: ~A != ~A" - (* 1000 y) (elt (send pos0 :pos) 1))) - (assert (< (abs (shortest-angle d (elt (car (send pos0 :rpy-angle)) 0))) 0.1) - (format nil "go-pos-unsafe moves incorrectly in d axis: ~A != ~A" - d (elt (car (send pos0 :rpy-angle)) 0))) - )) - (run-all-tests) (exit) diff --git a/pr2eus/test/pr2-ri-test-base.l b/pr2eus/test/pr2-ri-test-base.l new file mode 100644 index 000000000..adf027bce --- /dev/null +++ b/pr2eus/test/pr2-ri-test-base.l @@ -0,0 +1,177 @@ +(require :unittest "lib/llib/unittest.l") +(load "package://pr2eus/pr2-interface.l") + +;; avoid print violate max/min-angle that exceeds 4M log limit +(setf (symbol-function 'warning-message-org) + (symbol-function 'warning-message)) +(defun warning-message (color format &rest mesg) + (unless (or (substringp "violate min-angle" format) + (substringp "violate max-angle" format)) + (apply #'warning-message-org color format mesg))) + +(defun shortest-angle (d0 d1) + (atan2 (sin (- d0 d1)) (cos (- d0 d1)))) + +;; initialize *pr2* + +(setq *pr2* (pr2)) + +(while (or (not (boundp '*ri*)) (send *ri* :simulation-modep)) + (setq *ri* (instance pr2-interface :init))) + +(when (send *ri* :simulation-modep) + (ros::ros-warn "*ri* is running with simulation mode, something goes wrong ....") + (sys::exit 1)) + +(init-unit-test) + +(deftest test-go-stop + (let ((x 0.3) (y 0.3) (d 90) + pos0 pos1) + (pr2-tuckarm-pose) + (ros::ros-info "send :go-pos ~A ~A ~A, assuming it takse longer than 2 sec" x y d) + (send *ri* :go-pos-no-wait x y d) + (send (instance ros::duration :init 2) :sleep) + (setq pos0 (send *ri* :state :worldcoords "map")) + (ros::ros-info "get current locateion ~A and send :go-stop" pos0) + (send *ri* :go-stop) + (unix:sleep 1) + (assert (equal (send (*ri* . move-base-action) :get-state) actionlib_msgs::GoalStatus::*preempted*)) ;; make sure go-pos is preempted + (setq pos1 (send *ri* :state :worldcoords "map")) + (setq diff-pos (norm (send pos0 :difference-position pos1))) + (ros::ros-info "get current locateion ~A, and diff position ~A" pos1 diff-pos) + ;; diff in 3cm + (assert (< diff-pos 30) (format nil "go-stop does not stop: ~A > 30" diff-pos)))) + +(defun go-pos-func (&key (x 0.3) (y 0.3) (d 90) (func :go-pos)) ;; [m] [m] [degree] + (let (pos0 pos1 pos2 diff-pos diff-rot tm0 tm1) + (pr2-tuckarm-pose) + (setq pos0 (send *ri* :state :worldcoords "map")) + (ros::ros-info "send ~A ~A ~A ~A" func x y d) + (setq tm0 (ros::time-now)) + (send *ri* func x y d) + ;; wait for 2 sec + (send (instance ros::duration :init 2) :sleep) + (setq tm1 (ros::time-now)) + (setq pos1 (send *ri* :state :worldcoords "map")) + (setq diff-pos (send pos0 :difference-position pos1)) + (setq diff-rot (send pos0 :difference-rotation pos1)) + ;; if func is -no-wait, make sure that :go-pos-* returns immediately but move forward + (when (substringp "NO-WAIT" (string func)) + (ros::ros-info "~A returns immediately ~A" func (send (ros::time- tm1 tm0) :to-sec)) + (ros::ros-info " difference-position: ~A -> ~A" diff-pos (norm diff-pos)) + (assert (< (abs (- (send (ros::time- tm1 tm0) :to-sec) 2.0)) 0.5)) + (assert (> (norm diff-pos) 25)) + (ros::ros-info "wait for result..") + ;; wait for result + (cond ((substringp "GO-POS-UNSAFE" (string func)) ;; FIXME + (send *ri* :go-pos-unsafe-wait)) + (t + (send *ri* :move-to-wait :retry 1 :frame-id (*ri* . base-frame-id)))) + (unix:sleep 1) + (setq pos1 (send *ri* :state :worldcoords "map")) + (setq diff-pos (send pos0 :difference-position pos1)) + (setq diff-rot (send pos0 :difference-rotation pos1)) + ) + (ros::ros-info "reached to the goal") + (ros::ros-info " difference-position: ~A -> ~A ~A" diff-pos (- (elt diff-pos 0) (* 1000 x)) (- (elt diff-pos 1) (* 1000 y))) + (ros::ros-info " difference-rotation: ~A -> ~A" diff-rot (shortest-angle (deg2rad d) (elt diff-rot 2))) + ;; diff in 10cm + (assert (< (abs (- (elt diff-pos 0) (* 1000 x))) 100) + (format nil "go-pos moves incorrectly in x axis: ~A != ~A" + (* 1000 x) (elt diff-pos 0))) + (assert (< (abs (- (elt diff-pos 1) (* 1000 y))) 100) + (format nil "go-pos moves incorrectly in y axis: ~A != ~A" + (* 1000 y) (elt diff-pos 1))) + (assert (< (abs (shortest-angle (deg2rad d) (elt diff-rot 2))) 0.8) ;; go-pos-unsafe-no-wait need 0.55... other cases fit within 0.1 + (format nil "go-pos moves incorrectly in d axis: ~A != ~A" + (deg2rad d) (elt diff-rot 2))) + )) + +(deftest test-go-pos + (go-pos-func :func :go-pos)) + +(deftest test-go-pos-no-wait + (go-pos-func :func :go-pos-no-wait)) + +(deftest test-go-pos-unsafe + (go-pos-func :func :go-pos-unsafe)) + +(deftest test-go-pos-unsafe-no-wait + (if (*ri* . move-base-trajectory-action) + (go-pos-func :func :go-pos-unsafe-no-wait) + (ros::ros-warn "SKIP TEST :go-pos-unsafe-no-wait requires move-base-trajectory-action"))) + + +(defun go-velocity-func (&key wait) + (let ((vel-x 1) (vel-y 1) (vel-d 1.57) ;; [m/s] [m/s] [rad/s] + pos0 pos1 pos2 pos3 tm0 tm1 diff-pos vel-diff-pos) + (pr2-tuckarm-pose) + (setq pos0 (send *ri* :state :worldcoords "map")) + (setq tm0 (ros::time-now)) + (ros::ros-info "(before go-velocity) current location ~A" pos0) + (ros::ros-info "send :go-velocity ~A ~A ~A :wait ~A" vel-x vel-y vel-d wait) + (send *ri* :go-velocity vel-x vel-y vel-d 3000 :wait wait) ;; go-velocity blocks for 3 sec if wait is t + (unless wait + (send (instance ros::duration :init 2) :sleep) + (setq tm1 (ros::time-now)) + (setq pos1 (send *ri* :state :worldcoords "map")) + (setq diff-pos (send pos0 :difference-position pos1)) + (setq diff-rot (send pos0 :difference-rotation pos1)) + (ros::ros-info ":go-velocity :wait ~A returns immediately ~A" wait (send (ros::time- tm1 tm0) :to-sec)) + (ros::ros-info " difference-position: ~A -> ~A" diff-pos (norm diff-pos)) + (assert (< (abs (- (send (ros::time- tm1 tm0) :to-sec) 2.0)) 0.5)) + (assert (> (norm diff-pos) 25)) + (send (*ri* . move-base-trajectory-action) :wait-for-result) + ) + (setq tm1 (ros::time-now)) + (unix:sleep 1) ;; make sure robot actually stops + (setq pos1 (send *ri* :state :worldcoords "map")) + (ros::ros-info "(after 1 sec ) current location ~A" pos1) + (setq diff-pos (norm (send pos0 :difference-position pos1))) + (setq vel-diff-pos + (* 1000 (norm (float-vector vel-x vel-y)) + (send (ros::time- tm1 tm0) :to-sec))) + (ros::ros-info " difference-position from position ~A" diff-pos) + (ros::ros-info " difference-position from velocity ~A (~A sec)" vel-diff-pos (send (ros::time- tm1 tm0) :to-sec)) + (assert (> vel-diff-pos diff-pos) + (format nil "go-velocity moves too much: ~A" diff-pos)) + )) + +(deftest test-go-velocity + (if (*ri* . move-base-trajectory-action) + (go-velocity-func) + (ros::ros-warn "SKIP TEST: default behavior of :go-velocity is :wait nil, and this requires move-base-trajectory-action"))) + +(deftest test-go-velocity-wait + (go-velocity-func :wait t)) + +(deftest test-move-to + (let ((x 0.3) (y 0.3) (d 1.57) ;; [m] [m] [rad] + pos0) + (pr2-tuckarm-pose) + (ros::ros-info "send :move-to ~A ~A ~A" x y d) + (send *ri* :move-to + (make-coords :pos (float-vector (* 1000 x) (* 1000 y) 0) + :rpy (float-vector d 0 0)) + :frame-id "map") + (unix:sleep 1) + (setq pos0 (send *ri* :state :worldcoords "map")) + (ros::ros-info "reached to the goal") + (ros::ros-info " difference-position: ~A -> ~A ~A" (send pos0 :worldpos) (- (elt (send pos0 :pos) 0) (* 1000 x)) (- (elt (send pos0 :pos) 1) (* 1000 y))) + (ros::ros-info " difference-rotation: ~A -> ~A" (send pos0 :worldrot) (shortest-angle d (elt (car (send pos0 :rpy-angle)) 0))) + ;; diff in 10cm + (assert (< (abs (- (elt (send pos0 :pos) 0) (* 1000 x))) 100) + (format nil "move-to moves incorrectly in x axis: ~A != ~A" + (* 1000 x) (elt (send pos0 :pos) 0))) + (assert (< (abs (- (elt (send pos0 :pos) 1) (* 1000 y))) 100) + (format nil "move-to moves incorrectly in y axis: ~A != ~A" + (* 1000 y) (elt (send pos0 :pos) 1))) + (assert (< (abs (shortest-angle d (elt (car (send pos0 :rpy-angle)) 0))) 0.1) + (format nil "go-pos-unsafe moves incorrectly in d axis: ~A != ~A" + d (elt (car (send pos0 :rpy-angle)) 0))) + )) + +(run-all-tests) +(exit) + diff --git a/pr2eus/test/pr2-ri-test.launch b/pr2eus/test/pr2-ri-test.launch index a703f5065..f2564d6d7 100644 --- a/pr2eus/test/pr2-ri-test.launch +++ b/pr2eus/test/pr2-ri-test.launch @@ -142,6 +142,8 @@ file="$(find pr2_base_trajectory_action)/launch/pr2_base_trajectory_action.launch" /> - + + From affbcb100cb268fb86e73608714d4518db13600b Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 13 Dec 2019 17:00:46 +0900 Subject: [PATCH 09/12] update travsi_to 0.5.7 --- .travis | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis b/.travis index 555557aaa..74e968928 160000 --- a/.travis +++ b/.travis @@ -1 +1 @@ -Subproject commit 555557aaa06d036784da055b1ca4d04ff11d3e74 +Subproject commit 74e968928601f613b2cec821c1a5975baafc1127 From 8662d55bfb6d6ca5d0566ad7dc2109f78fbd1a61 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 14 Dec 2019 01:05:23 +0000 Subject: [PATCH 10/12] add missing dwa_local_planner to pr2eus/package.xml --- pr2eus/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/pr2eus/package.xml b/pr2eus/package.xml index aff577417..9eda8ad94 100644 --- a/pr2eus/package.xml +++ b/pr2eus/package.xml @@ -43,6 +43,7 @@ pr2_navigation_slam pr2_navigation_config robot_state_publisher + dwa_local_planner From e125b0161457e2678e7a363f09a657fcfe16c5a3 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 4 Nov 2019 13:37:33 +0900 Subject: [PATCH 11/12] update :go-stop for robot without move-base-trajectory-action --- pr2eus/robot-interface.l | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 9cabd8750..7bc5cc174 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -1283,9 +1283,12 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i ;; (:go-stop (&optional (force-stop t)) (when joint-action-enable - (send move-base-action :cancel-all-goals) (prog1 - (send move-base-trajectory-action :cancel-all-goals) + (if move-base-trajectory-action + (and + (send move-base-action :cancel-all-goals) + (send move-base-trajectory-action :cancel-all-goals)) + (send move-base-action :cancel-all-goals)) (if force-stop (send self :go-velocity 0 0 0))) )) From 1b591bb87bb1d903b0979fd9e91c54875de92f8c Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 11 Dec 2019 22:34:26 +0900 Subject: [PATCH 12/12] update :go-velocity, :go-pos-unsafe-no-wait for robto without move-base-trajectory-action, add :send-cmd-vel-raw --- pr2eus/robot-interface.l | 67 +++++++++++++++++++++++++++++++++++++--- 1 file changed, 62 insertions(+), 5 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 7bc5cc174..18f25b3da 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -1239,7 +1239,7 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i :slots (move-base-action move-base-trajectory-action move-base-goal-msg move-base-goal-coords move-base-goal-map-to-frame base-frame-id - odom-topic move-base-trajectory-joint-names + cmd-vel-topic odom-topic move-base-trajectory-joint-names go-pos-unsafe-goal-msg current-goal-coords)) ;; for simulation-callback (defmethod robot-move-base-interface @@ -1248,10 +1248,12 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (move-base-action-name "move_base") ((:base-frame-id base-frame-id-name) "base_footprint") (base-controller-action-name "/base_controller/follow_joint_trajectory") (base-controller-joint-names (list "base_link_x" "base_link_y" "base_link_pan")) + ((:cmd-vel-topi cmd-vel-topic-name) "/base_controller/command") ((:odom-topic odom-topic-name) "/base_odometry/odom") &allow-other-keys) (prog1 (send-super* :init args) (setq base-frame-id base-frame-id-name) (setq odom-topic odom-topic-name) + (setq cmd-vel-topic cmd-vel-topic-name) (setq move-base-action (instance ros::simple-action-client :init move-base-action-name move_base_msgs::MoveBaseAction :groupname groupname)) @@ -1505,6 +1507,18 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (let () (send self :move-to-wait :retry 1 :frame-id base-frame-id :no-wait nil) )) + (:send-cmd-vel-raw + (x y d) ;; [m] [m] [degree] + (when (send self :simulation-modep) + (return-from :send-cmd-vel-raw t)) + (unless (ros::get-topic-publisher cmd-vel-topic) + (ros::advertise cmd-vel-topic geometry_msgs::Twist 1) + (unix:sleep 1)) + (let ((msg (instance geometry_msgs::Twist :init))) + (send msg :linear :x x) + (send msg :linear :y y) + (send msg :angular :z d) + (ros::publish cmd-vel-topic msg))) (:go-velocity (x y d ;; [m/sec] [m/sec] [rad/sec] &optional (msec 1000) ;; msec is total animation time [msec] @@ -1520,8 +1534,18 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (if viewer (send self :draw-objects)))) (return-from :go-velocity t)) (unless move-base-trajectory-action - (ros::ros-warn ":go-velocity is disabled. (move-base-trajectory-action is not found)") - (return-from :go-velocity t)) + (unless wait + (ros::ros-error ":go-velocity :wait nil requries move-base-trajectory-action, call :go-velocity with :wait t")) + (ros::rate 100) + (let ((start-time (ros::time-now))) + (while (and (ros::ok) + (< (* 1000.0 (send (ros::time- (ros::time-now) start-time) :to-sec)) msec)) + (send self :spin-once) + (send self :send-cmd-vel-raw x y d) + (ros::sleep))) + (when stop + (send self :send-cmd-vel-raw 0 0 0)) + (return-from :go-velocity t)) ;; when move-base-trajectory-action not found (setq x (* x (/ msec 1000.0)) y (* y (/ msec 1000.0)) d (* d (/ msec 1000.0))) @@ -1547,8 +1571,41 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (if viewer (send self :draw-objects)))) (return-from :go-pos-unsafe-no-wait t)) (unless move-base-trajectory-action - (ros::ros-warn ":go-pose-unsafe-no-wait is disabled. (move-base-trajectory-action is not found)") - (return-from :go-pos-unsafe-no-wait t)) + (ros::ros-warn "pseudo :go-pose-unsafe-no-wait is called because move-base-trajectory-action is not found") + (ros::rate 100) + (let (org-cds cur-cds diffpos diffrot x-err y-err d-err + (translation-threshold 0.05) (rotation-threshold (deg2rad 5)) + (translation-gain 1.0) (rotation-gain 1.0) + (stop t)) + (send self :spin-once) + (setq org-cds (send self :state :odom :pose)) + (while (ros::ok) + (send self :spin-once) + (setq cur-cds (send self :state :odom :pose)) + (setq diffpos (send org-cds :difference-position cur-cds)) + (setq x-err (- x (* 0.001 (elt diffpos 0)))) + (setq y-err (- y (* 0.001 (elt diffpos 1)))) + (when (and (< (abs x-err) translation-threshold) + (< (abs y-err) translation-threshold)) + (when stop (send self :send-cmd-vel-raw 0 0 0)) ;; stop + (return)) + (let ((x-vel (* translation-gain x-err)) + (y-vel (* translation-gain y-err))) + (send self :send-cmd-vel-raw x-vel y-vel 0)) + (ros::sleep)) + (while (ros::ok) + (send self :spin-once) + (setq cur-cds (send self :state :odom :pose)) + (setq diffrot (send org-cds :difference-rotation cur-cds)) + (setq d-err (shortest-angle (deg2rad d) (elt diffrot 2))) + (when (< (abs d-err) rotation-threshold) + (when stop (send self :send-cmd-vel-raw 0 0 0)) ;; stop + (return)) + (let ((d-vel (* rotation-gain d-err))) + (send self :send-cmd-vel-raw 0 0 d-vel)) + (ros::sleep)) + (ros::rate 10)) + (return-from :go-pos-unsafe-no-wait t)) ;; unless move-base-trajectory-action (let ((maxvel 0.295) (maxrad 0.495) msec) ;; package://pr2_base_trajectory_action/config/pr2_base_link.yaml