From 23452be1eb9307339cafa30cda61ed2184f9a5f4 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Fri, 25 Oct 2019 23:58:35 +0900 Subject: [PATCH 1/5] use :go-pos-unsafe in :move-to for non-holonomic robot --- pr2eus/robot-interface.l | 82 +++++++++++++--------------------------- 1 file changed, 26 insertions(+), 56 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 35ae8138f..8737da864 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -1409,73 +1409,43 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (setq ret t)) (incf count)) (ros::ros-info "move-to : ~A" (if ret 'succeeded 'failed)) - ;; (when (and ret correction) - (let (diff diff-len current-coords lret map-goal-coords) - ;; - (setq map-goal-coords - (if (string= frame-id base-frame-id) - (send (send map-to-frame :copy-worldcoords) :transform (send coords :worldcoords)) - (send (send *tfl* :lookup-transform "map" frame-id (ros::time 0)) - :transform (send coords :copy-worldcoords)))) ;; goal-coords in /map coordinates - (setq lret (send *tfl* :wait-for-transform "map" base-frame-id (ros::time-now) 5)) - (ros::ros-warn ":move-to wait-for transform map to ~A -> ~A" base-frame-id lret) - (when (null lret) - (ros::ros-error ":move-to wait-for transform map to ~A failed" base-frame-id) - (setq move-base-goal-msg nil) - (return-from :move-to-wait nil)) - (setq current-coords (send *tfl* :lookup-transform "map" base-frame-id (ros::time 0))) - (setq diff (send current-coords :transformation map-goal-coords)) - (ros::ros-warn ":move-to current-coords ~A" current-coords) - (ros::ros-warn " mapgoal-coords ~A" map-goal-coords) - (ros::ros-warn " error-coords ~A" diff) - (ros::ros-warn " target-coords ~A" coords) - ;; + (let* ((map-goal-coords + (if (string= frame-id base-frame-id) + (send (send map-to-frame :copy-worldcoords) :transform (send coords :worldcoords)) + (send (send *tfl* :lookup-transform "map" frame-id (ros::time 0)) + :transform (send coords :copy-worldcoords)))) ;; goal-coords in /map coordinates + (lret (send *tfl* :wait-for-transform "map" base-frame-id (ros::time-now) 5)) + (current-coords (send *tfl* :lookup-transform "map" base-frame-id (ros::time 0))) + (diff (send current-coords :transformation map-goal-coords)) + (diff-len (norm (subseq (send diff :worldpos) 0 2)))) (dotimes (i 2) - (if (< (setq diff-len (norm (subseq (send diff :worldpos) 0 2))) 200) ;; move_base thre = 200mm - (let* ((msec (* diff-len 10)) - (x (/ (elt (send diff :worldpos) 0) msec)) - (y (/ (elt (send diff :worldpos) 1) msec)) - (d (/ (elt (car (rpy-angle (send diff :worldrot))) 0) (/ msec 1000)))) - (ros::ros-warn ":move-to -> :go-velocity x:~A y:~A d:~A msec:~A" x y d msec) - (unix:usleep (* 400 1000)) ;; 400ms ??? - (let ((acret (send self :go-velocity x y d msec :wait t))) - (unless acret - (setq move-base-goal-msg nil) - (return-from :move-to-wait nil))) - ;;(unix::usleep (* (round msec) 1000)) ;; why time wait - ) - (progn - (ros::ros-error "too far from goal position ~A mm (> 200mm)" diff-len) - ;; move-to succeeded but away from 200 mm - (ros::ros-error ":move-to try to send /move_base_simple/goal") - (ros::advertise "/move_base_simple/goal" geometry_msgs::PoseStamped 1) - (send move-base-goal-msg :goal :target_pose :header :seq (1+ count)) - (ros::publish "/move_base_simple/goal" (send move-base-goal-msg :goal :target_pose)) - (unix:sleep 3) - (setq move-base-goal-msg nil) - (return-from :move-to-wait nil) - )) - ;; - (setq map-goal-coords - (if (string= frame-id base-frame-id) - (send (send map-to-frame :copy-worldcoords) :transform (send coords :worldcoords)) - (send (send *tfl* :lookup-transform "map" frame-id (ros::time 0)) - :transform (send coords :copy-worldcoords)))) ;; goal-coords in /map coordinates - (setq lret (send *tfl* :wait-for-transform "map" base-frame-id (ros::time-now) 5)) (ros::ros-warn ":move-to wait-for transform map to ~A -> ~A" base-frame-id lret) (when (null lret) (ros::ros-error ":move-to wait-for transform map to ~A failed" base-frame-id) (setq move-base-goal-msg nil) (return-from :move-to-wait nil)) - (setq current-coords (send *tfl* :lookup-transform "map" base-frame-id (ros::time 0))) - (setq diff (send current-coords :transformation map-goal-coords)) (ros::ros-warn ":move-to current-coords ~A" current-coords) (ros::ros-warn " mapgoal-coords ~A" map-goal-coords) (ros::ros-warn " error-coords ~A" diff) (ros::ros-warn " target-coords ~A" coords) - ) ;; (do (i 2) - )) + (if (< diff-len 200) ;; move_base thre = 200mm + (let* ((x (/ (elt (send diff :worldpos) 0) 1000)) + (y (/ (elt (send diff :worldpos) 1) 1000)) + (d (elt (car (rpy-angle (send diff :worldrot))) 0))) + (unless (send self :go-pos-unsafe x y d) + (setq move-base-goal-msg nil) + (return-from :move-to-wait nil))) + (progn + (ros::ros-error "too far from goal position ~A mm (> 200mm)" diff-len) + ;; move-to succeeded but away from 200 mm + (ros::ros-error ":move-to try to send /move_base_simple/goal") + (ros::advertise "/move_base_simple/goal" geometry_msgs::PoseStamped 1) + (send move-base-goal-msg :goal :target_pose :header :seq (1+ count)) + (ros::publish "/move_base_simple/goal" (send move-base-goal-msg :goal :target_pose)) + (unix:sleep 3) + (setq move-base-goal-msg nil) + (return-from :move-to-wait nil)))))) (setq move-base-goal-msg nil) ;; :move-to-wait has been called ret)) From 0312d9bacd2b21c62d4f090a800ad5fb1ef1e4ec Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 29 Oct 2019 20:02:28 +0900 Subject: [PATCH 2/5] fix tf echo time in :move-to-wait --- pr2eus/robot-interface.l | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 8737da864..53652b8b7 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -1410,16 +1410,17 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (incf count)) (ros::ros-info "move-to : ~A" (if ret 'succeeded 'failed)) (when (and ret correction) - (let* ((map-goal-coords - (if (string= frame-id base-frame-id) - (send (send map-to-frame :copy-worldcoords) :transform (send coords :worldcoords)) - (send (send *tfl* :lookup-transform "map" frame-id (ros::time 0)) - :transform (send coords :copy-worldcoords)))) ;; goal-coords in /map coordinates - (lret (send *tfl* :wait-for-transform "map" base-frame-id (ros::time-now) 5)) - (current-coords (send *tfl* :lookup-transform "map" base-frame-id (ros::time 0))) - (diff (send current-coords :transformation map-goal-coords)) - (diff-len (norm (subseq (send diff :worldpos) 0 2)))) - (dotimes (i 2) + (dotimes (i 2) + (let* ((map-goal-coords + (if (string= frame-id base-frame-id) + (send (send map-to-frame :copy-worldcoords) :transform (send coords :worldcoords)) + (send (send *tfl* :lookup-transform "map" frame-id (ros::time 0)) + :transform (send coords :copy-worldcoords)))) ;; goal-coords in /map coordinates + (timestamp (ros::time-now)) + (lret (send *tfl* :wait-for-transform "map" base-frame-id timestamp 5)) + (current-coords (send *tfl* :lookup-transform "map" base-frame-id timestamp)) + (diff (send current-coords :transformation map-goal-coords)) + (diff-len (norm (subseq (send diff :worldpos) 0 2)))) (ros::ros-warn ":move-to wait-for transform map to ~A -> ~A" base-frame-id lret) (when (null lret) (ros::ros-error ":move-to wait-for transform map to ~A failed" base-frame-id) From ce2de01e258310b80b47afece7b4fba97220db0a Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 9 Nov 2019 01:59:12 +0900 Subject: [PATCH 3/5] fix deg/rad mistake in :move-to --- pr2eus/robot-interface.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 53652b8b7..00c759953 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -1433,7 +1433,7 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (if (< diff-len 200) ;; move_base thre = 200mm (let* ((x (/ (elt (send diff :worldpos) 0) 1000)) (y (/ (elt (send diff :worldpos) 1) 1000)) - (d (elt (car (rpy-angle (send diff :worldrot))) 0))) + (d (rad2deg (elt (car (rpy-angle (send diff :worldrot))) 0)))) (unless (send self :go-pos-unsafe x y d) (setq move-base-goal-msg nil) (return-from :move-to-wait nil))) From eba2929bc97dba8e1feb1854cc8e546e624d1cea Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 9 Nov 2019 03:55:51 +0900 Subject: [PATCH 4/5] set timestamp and wait transform --- pr2eus/robot-interface.l | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 00c759953..92e404a1e 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -1411,17 +1411,23 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (ros::ros-info "move-to : ~A" (if ret 'succeeded 'failed)) (when (and ret correction) (dotimes (i 2) - (let* ((map-goal-coords + (let* ((timestamp (ros::time-now)) + (mret (send *tfl* :wait-for-transform "map" frame-id timestamp 5)) + (map-goal-coords (if (string= frame-id base-frame-id) (send (send map-to-frame :copy-worldcoords) :transform (send coords :worldcoords)) - (send (send *tfl* :lookup-transform "map" frame-id (ros::time 0)) + (send (send *tfl* :lookup-transform "map" frame-id timestamp) :transform (send coords :copy-worldcoords)))) ;; goal-coords in /map coordinates - (timestamp (ros::time-now)) (lret (send *tfl* :wait-for-transform "map" base-frame-id timestamp 5)) (current-coords (send *tfl* :lookup-transform "map" base-frame-id timestamp)) (diff (send current-coords :transformation map-goal-coords)) (diff-len (norm (subseq (send diff :worldpos) 0 2)))) + (ros::ros-warn ":move-to wait-for transform map to ~A -> ~A" frame-id mret) (ros::ros-warn ":move-to wait-for transform map to ~A -> ~A" base-frame-id lret) + (when (null mret) + (ros::ros-error ":move-to wait-for transform map to ~A failed" frame-id) + (setq move-base-goal-msg nil) + (return-from :move-to-wait nil)) (when (null lret) (ros::ros-error ":move-to wait-for transform map to ~A failed" base-frame-id) (setq move-base-goal-msg nil) From fd519df2924c383cce381be154076f314efe1127 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Sat, 9 Nov 2019 03:56:04 +0900 Subject: [PATCH 5/5] format ros-warn --- pr2eus/robot-interface.l | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 92e404a1e..173eca888 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -1434,8 +1434,8 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i (return-from :move-to-wait nil)) (ros::ros-warn ":move-to current-coords ~A" current-coords) (ros::ros-warn " mapgoal-coords ~A" map-goal-coords) - (ros::ros-warn " error-coords ~A" diff) - (ros::ros-warn " target-coords ~A" coords) + (ros::ros-warn " error-coords ~A" diff) + (ros::ros-warn " target-coords ~A" coords) (if (< diff-len 200) ;; move_base thre = 200mm (let* ((x (/ (elt (send diff :worldpos) 0) 1000)) (y (/ (elt (send diff :worldpos) 1) 1000))