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

roseus/test/test-tf.test: tf2_buffer_server output more information #27

Merged
merged 1 commit into from
Mar 29, 2014
Merged

roseus/test/test-tf.test: tf2_buffer_server output more information #27

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
13 changes: 10 additions & 3 deletions roseus/test/test-tf.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -284,28 +284,35 @@

(deftest test-tf2-lookup ()
(setq l (instance ros::buffer-client :init :namespace "tf2_buffer_server"))

(let ((ret (send l :wait-for-server)))
(unless ret
(error "tf2 :wait-for-server failed~%")))
(unix:sleep 1)

(unix:sleep 1)
(setq tm (ros::time))
(setq ret (send l :wait-for-transform "/MANDALAY" "/TOKYO" tm 1))
(warning-message 2 "wait-for-transform at ~A returns ~A~%" tm ret)
(setq ret (send l :wait-for-transform "/MANDALAY" "/TOKYO" tm 10))
(warning-message 2 "wait-for-transform at ~A returns ~A~%" (ros::time-now) ret)

(setq c1 (make-coords :pos #f(1000 0 0) :rpy #f(0 0 0)))
(setq c2 (make-coords :pos #f(2000 2000 3000) :rpy #f(0.1 0.2 0.3)))
;;
(dotimes (j 10)
(warning-message 2 "switch target to /TOKYO~%")
(dotimes (i 10)
(setq tm (ros::time))
(setq ret (send l :wait-for-transform "MANDALAY" "JUPITER" tm 10))
(warning-message 2 "wait-for-transform at ~A returns ~A~%" (ros::time-now) ret)
(setq c (send l :lookup-transform "MANDALAY" "JUPITER" (ros::time)))
(unless (and c (eps= (norm (send c :difference-position c1)) 0)
(eps= (norm (send c :difference-rotation c1)) 0))
(assert (and c (eps= (norm (send c :difference-position c1)) 0) (eps= (norm (send c :difference-rotation c1)) 0)) "tf2: lookup-transform failed ~A ~A" c c1)
))
(warning-message 2 "switch target to /JUPITER~%")
(dotimes (i 10)
(setq tm (ros::time))
(setq ret (send l :wait-for-transform "MANDALAY" "TOKYO" tm 10))
(warning-message 2 "wait-for-transform at ~A returns ~A~%" (ros::time-now) ret)
(setq c (send l :lookup-transform "MANDALAY" "TOKYO" (ros::time)))
(unless (and c (eps= (norm (send c :difference-position c2)) 0)
(eps= (norm (send c :difference-rotation c2)) 0))
Expand Down