diff --git a/.travis b/.travis
index 555557aaa..74e968928 160000
--- a/.travis
+++ b/.travis
@@ -1 +1 @@
-Subproject commit 555557aaa06d036784da055b1ca4d04ff11d3e74
+Subproject commit 74e968928601f613b2cec821c1a5975baafc1127
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
diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l
index 0b686ecb5..136595fbe 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))
@@ -1283,9 +1285,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)))
))
@@ -1479,6 +1484,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]
@@ -1494,8 +1511,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)))
@@ -1521,8 +1548,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
@@ -1718,11 +1778,10 @@ 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)))
- t)
+ (call-empty-service srv-name))
+ )
(:change-inflation-range
(&optional (range 0.2)
&key (node-name "/move_base_node")
@@ -1856,14 +1915,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"
- (if (and (boundp '*ri*) (derivedp *ri* robot-move-base-interface))
- (send *ri* :clear-costmap)
+ "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))))
- t)
+ (call-empty-service srv-name))))
(defun change-inflation-range (&optional (range 0.2)
&key (node-name "/move_base_node")
diff --git a/pr2eus/test/pr2-ri-test.l b/pr2eus/test/pr2-ri-test-arm.l
similarity index 93%
rename from pr2eus/test/pr2-ri-test.l
rename to pr2eus/test/pr2-ri-test-arm.l
index 3c1b554f8..244c863ad 100644
--- a/pr2eus/test/pr2-ri-test.l
+++ b/pr2eus/test/pr2-ri-test-arm.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)))
;; initialize *pr2*
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 8babc4271..f2564d6d7 100644
--- a/pr2eus/test/pr2-ri-test.launch
+++ b/pr2eus/test/pr2-ri-test.launch
@@ -8,6 +8,9 @@
+
+
@@ -135,7 +138,12 @@
+
+
-
+
+