-
Notifications
You must be signed in to change notification settings - Fork 41
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'master' into use-go-pos-unsafe
Showing
6 changed files
with
270 additions
and
40 deletions.
There are no files selected for viewing
Submodule .travis
updated
23 files
+17 −9 | .travis.yml | |
+162 −0 | CHANGELOG.rst | |
+5 −0 | CMakeLists.txt | |
+25 −6 | README.md | |
+7 −0 | docker.sh | |
+67 −0 | docker/Dockerfile.ros-ubuntu:12.04 | |
+5 −0 | docker/Dockerfile.ros-ubuntu:12.04-base | |
+33 −0 | docker/Dockerfile.ros-ubuntu:14.04 | |
+5 −0 | docker/Dockerfile.ros-ubuntu:14.04-base | |
+15 −0 | docker/Dockerfile.ros-ubuntu:14.04-pcl | |
+37 −0 | docker/Dockerfile.ros-ubuntu:14.04-pcl1.8 | |
+26 −0 | docker/Dockerfile.ros-ubuntu:16.04 | |
+5 −0 | docker/Dockerfile.ros-ubuntu:16.04-base | |
+14 −0 | docker/Dockerfile.ros-ubuntu:16.04-pcl | |
+26 −0 | docker/Dockerfile.ros-ubuntu:18.04 | |
+5 −0 | docker/Dockerfile.ros-ubuntu:18.04-base | |
+14 −0 | docker/Dockerfile.ros-ubuntu:18.04-pcl | |
+56 −0 | docker/Makefile | |
+6 −1 | package.xml | |
+2 −2 | rosdep-install.sh | |
+17 −0 | test/test_env_var.py | |
+117 −20 | travis.sh | |
+171 −91 | travis_jenkins.py |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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) | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters