Skip to content

Commit

Permalink
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
Browse files Browse the repository at this point in the history
k-okada authored Dec 14, 2019
2 parents 3ce4e52 + 5d30e29 commit 0a713b5
Showing 6 changed files with 270 additions and 40 deletions.
1 change: 1 addition & 0 deletions pr2eus/package.xml
Original file line number Diff line number Diff line change
@@ -43,6 +43,7 @@
<test_depend>pr2_navigation_slam</test_depend>
<test_depend>pr2_navigation_config</test_depend>
<test_depend>robot_state_publisher</test_depend>
<test_depend>dwa_local_planner</test_depend>

<export>
<pr2eus robot-name="pr2" interface-file="${prefix}/pr2-interface.l" />
92 changes: 75 additions & 17 deletions pr2eus/robot-interface.l
Original file line number Diff line number Diff line change
@@ -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")
26 changes: 6 additions & 20 deletions pr2eus/test/pr2-ri-test.l → pr2eus/test/pr2-ri-test-arm.l
Original file line number Diff line number Diff line change
@@ -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*

177 changes: 177 additions & 0 deletions pr2eus/test/pr2-ri-test-base.l
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)

12 changes: 10 additions & 2 deletions pr2eus/test/pr2-ri-test.launch
Original file line number Diff line number Diff line change
@@ -8,6 +8,9 @@
<env name="DISPLAY" value=":0.0" if="$(arg gui)"/>
<env name="DISPLAY" value="" unless="$(arg gui)"/>

<arg name="launch_pr2_base_trajectory_action" default="false"
doc="launch pr2_base_trajectory_action for debugging" />

<!-- start empty world -->
<node name="gazebo" pkg="gazebo_ros" type="gzserver" args="-r worlds/empty.world" respawn="true" output="log"/>
<group if="$(arg gui)">
@@ -135,7 +138,12 @@
</rosparam>
</node>

<include if="$(arg launch_pr2_base_trajectory_action)"
file="$(find pr2_base_trajectory_action)/launch/pr2_base_trajectory_action.launch" />

<!-- start test -->
<test test-name="pr2_ri_test" pkg="roseus" type="roseus" retry="1"
args="$(find pr2eus)/test/pr2-ri-test.l" time-limit="800" />
<test test-name="pr2_ri_test_arm" pkg="roseus" type="roseus" retry="1"
args="$(find pr2eus)/test/pr2-ri-test-arm.l" time-limit="800" />
<test test-name="pr2_ri_test_base" pkg="roseus" type="roseus" retry="1"
args="$(find pr2eus)/test/pr2-ri-test-base.l" time-limit="800" />
</launch>

0 comments on commit 0a713b5

Please sign in to comment.