Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[pr2eus] use :go-pos-unsafe in :move-to for non-holonomic robot #406

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
Open

[pr2eus] use :go-pos-unsafe in :move-to for non-holonomic robot #406

Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
23452be
use :go-pos-unsafe in :move-to for non-holonomic robot
knorth55 Oct 25, 2019
0312d9b
fix tf echo time in :move-to-wait
knorth55 Oct 29, 2019
ce2de01
fix deg/rad mistake in :move-to
knorth55 Nov 8, 2019
eba2929
set timestamp and wait transform
knorth55 Nov 8, 2019
fd519df
format ros-warn
knorth55 Nov 8, 2019
25c47ec
Merge branch 'master' into use-go-pos-unsafe
knorth55 May 8, 2020
68f8d6c
Merge branch 'master' into use-go-pos-unsafe
knorth55 Sep 7, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
91 changes: 34 additions & 57 deletions pr2eus/robot-interface.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1419,42 +1419,40 @@ 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)
;;
(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
)
(dotimes (i 2)
(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 timestamp)
:transform (send coords :copy-worldcoords)))) ;; goal-coords in /map coordinates
(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)
(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)
(if (< diff-len 200) ;; move_base thre = 200mm
(let* ((x (/ (elt (send diff :worldpos) 0) 1000))
(y (/ (elt (send diff :worldpos) 1) 1000))
(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)))
(progn
(ros::ros-error "too far from goal position ~A mm (> 200mm)" diff-len)
;; move-to succeeded but away from 200 mm
Expand All @@ -1464,28 +1462,7 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
(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)
))
(return-from :move-to-wait nil))))))
(setq move-base-goal-msg nil) ;; :move-to-wait has been called
ret))

Expand Down