Skip to content

Commit

Permalink
Merge pull request #210 from k-okada/base_frame_id
Browse files Browse the repository at this point in the history 
robot-interface.l: fix robot-move-base-interface
  • Loading branch information
@k-okada
k-okada committed Mar 9, 2016
2 parents b309d26 + af18a5a commit 45fdd2d
Show file tree
Hide file tree
Showing 2 changed files with 58 additions and 33 deletions.
14 changes: 0 additions & 14 deletions pr2eus/pr2-interface.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,6 @@
;;
(ros::advertise "j_robotsound" sound_play::SoundRequest 5)
;;
(ros::subscribe "/base_odometry/odom" nav_msgs::Odometry
#'send self :pr2-odom-callback :groupname groupname)
(ros::subscribe "/pressure/r_gripper_motor" pr2_msgs::PressureState
#'send self :pr2-fingertip-callback :rarm-pressure :groupname groupname)
(ros::subscribe "/pressure/l_gripper_motor" pr2_msgs::PressureState
Expand All @@ -77,18 +75,6 @@
(return)))
t)
;;
(:pr2-odom-callback
(msg)
(let ((parsed
(list
(cons :stamp (send msg :header :stamp))
(cons :pose (ros::tf-pose->coords (send msg :pose :pose)))
(cons :velocity (float-vector
(* 1000 (send msg :twist :twist :linear :x))
(* 1000 (send msg :twist :twist :linear :y))
(send msg :twist :twist :angular :z))))))
(send self :set-robot-state1 :odom parsed)))
;;
(:state (&rest args) ;; overwrite for jsk_maps/pr2
(case (car args)
(:worldcoords
Expand Down
77 changes: 58 additions & 19 deletions pr2eus/robot-interface.l
View file
Original file line number Diff line number Diff line change
Expand Up @@ -742,8 +742,6 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
(send robot :angle-vector))
(:torque-vector
(send self :torque-vector))
(:worldcoords
(send *tfl* :lookup-transform (or (cadr args) "/map") "/base_footprint" (ros::time)))
(:gripper
(apply #'send self :gripper (cdr args)))
(t
Expand Down Expand Up @@ -1065,13 +1063,18 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
:super robot-interface
: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
go-pos-unsafe-goal-msg
current-goal-coords)) ;; for simulation-callback
(defmethod robot-move-base-interface
(:init
(&rest args &key
(move-base-action-name "move_base") &allow-other-keys)
(move-base-action-name "move_base") ((:base-frame-id base-frame-id-name) "/base_footprint")
((: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 move-base-action (instance ros::simple-action-client :init
move-base-action-name move_base_msgs::MoveBaseAction
:groupname groupname))
Expand All @@ -1080,8 +1083,24 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
"/base_controller/joint_trajectory_action"
pr2_controllers_msgs::JointTrajectoryAction
:groupname groupname))
(unless (send move-base-trajectory-action :wait-for-server 3)
(ros::ros-warn "move-base-trajectory-action is not found")
(setq move-base-trajectory-action nil))
(ros::subscribe odom-topic-name nav_msgs::Odometry
#'send self :odom-callback :groupname groupname)
))
;;
(:odom-callback
(msg)
(let ((parsed
(list
(cons :stamp (send msg :header :stamp))
(cons :pose (ros::tf-pose->coords (send msg :pose :pose)))
(cons :velocity (float-vector
(* 1000 (send msg :twist :twist :linear :x))
(* 1000 (send msg :twist :twist :linear :y))
(send msg :twist :twist :angular :z))))))
(send self :set-robot-state1 :odom parsed)))
;;
(:go-stop (&optional (force-stop t))
(when joint-action-enable
Expand All @@ -1099,7 +1118,7 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
map-to-base
res
plan-cds-seq)
(setq map-to-base (send *tfl* :lookup-transform "/map" "/base_footprint" (ros::time 0)))
(setq map-to-base (send *tfl* :lookup-transform "/map" base-frame-id (ros::time 0)))
(setq map-to-frame (send *tfl* :lookup-transform "/map" start-frame-id (ros::time 0)))
(send req :start :header :stamp)
(send req :start :header :stamp tm)
Expand Down Expand Up @@ -1146,7 +1165,7 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
(setq move-base-goal-coords coords)
(if (send self :simulation-modep)
(let ()
(cond ((equal frame-id "/base_footprint")
(cond ((equal frame-id base-frame-id)
(setq current-goal-coords (send coords :transform robot :world))) ;; for simulation-callback
(t
(setq current-goal-coords (send coords :copy-worldcoords)))) ;; for simulation-callback
Expand Down Expand Up @@ -1207,17 +1226,17 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
(let (diff diff-len current-coords lret map-goal-coords)
;;
(setq map-goal-coords
(if (string= frame-id "/base_footprint")
(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_footprint" (ros::time-now) 5))
(ros::ros-warn ":move-to wait-for transform /map to /base_footprint -> ~A" lret)
(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 /base_footprint failed")
(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-no-wait nil))
(setq current-coords (send *tfl* :lookup-transform "/map" "/base_footprint" (ros::time 0)))
(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)
Expand Down Expand Up @@ -1251,17 +1270,17 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
))
;;
(setq map-goal-coords
(if (string= frame-id "/base_footprint")
(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_footprint" (ros::time-now) 5))
(ros::ros-warn ":move-to wait-for transform /map to /base_footprint -> ~A" lret)
(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 /base_footprint failed")
(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_footprint" (ros::time 0)))
(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)
Expand Down Expand Up @@ -1290,19 +1309,19 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
(let (c)
(setq c (make-coords :pos (float-vector (* 1000 x) (* y 1000) 0)
:rpy (float-vector (deg2rad d) 0 0)))
(send self :move-to c :retry 1 :frame-id "/base_footprint")
(send self :move-to c :retry 1 :frame-id base-frame-id)
))
(:go-pos-no-wait
(x y &optional (d 0)) ;; [m] [m] [degree]
(let (c)
(setq c (make-coords :pos (float-vector (* 1000 x) (* y 1000) 0)
:rpy (float-vector (deg2rad d) 0 0)))
(send self :move-to c :retry 1 :frame-id "/base_footprint" :no-wait t)
(send self :move-to c :retry 1 :frame-id base-frame-id :no-wait t)
))
(:go-wait
()
(let ()
(send self :move-to-wait :retry 1 :frame-id "/base_footprint" :no-wait nil)
(send self :move-to-wait :retry 1 :frame-id base-frame-id :no-wait nil)
))
(:go-velocity
(x y d ;; [m/sec] [m/sec] [rad/sec]
Expand All @@ -1318,6 +1337,9 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
(send robot :newcoords (midcoords (/ curr-tm (float msec)) orig-coords coords))
(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))
(let ((goal (send self :move-trajectory x y d msec :stop stop)))
(prog1
(send move-base-trajectory-action :send-goal goal)
Expand All @@ -1339,6 +1361,9 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
(send robot :newcoords (midcoords (/ curr-tm 1000.0) orig-coords coords))
(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))
(let (msec step (maxvel 0.295) (maxrad 0.495))
;; package://pr2_base_trajectory_action/config/pr2_base_link.yaml
;; 80% of maxvel = 0.3[m/sec]
Expand All @@ -1354,6 +1379,9 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
(:go-pos-unsafe-wait ()
(let (x y d msec step goal (maxvel 0.295) (maxrad 0.495) (counter 0))
(if (null go-pos-unsafe-goal-msg) (return-from :go-pos-unsafe-wait nil))
(unless move-base-trajectory-action
(ros::ros-warn ":go-pose-unsafe-wait is disabled. (move-base-trajectory-action is not found)")
(return-from :go-pos-unsafe-wait t))
(while (< counter 3) ;; magic number 3 times
(let ((acret
(send move-base-trajectory-action :wait-for-result)))
Expand Down Expand Up @@ -1494,14 +1522,25 @@ send-action [ send message to action server, it means robot will move ]"
(send msg :points (list pt1 pt2)))
))
(send goal :goal :trajectory msg)
(when send-action
(when send-action and move-base-trajectory-action
(send move-base-trajectory-action :send-goal goal)
(let ((acret
(send move-base-trajectory-action :wait-for-result)))
(unless acret (return-from :move-trajectory nil))
(send move-base-trajectory-action :spin-once)))
;;
goal))
;;
(:state
(&rest args)
(prog1
(send-super* :state args)
(case (car args)
(:worldcoords
(unless joint-action-enable
(return-from :state (send self :worldcoords)))
(return-from :state (send *tfl* :lookup-transform (or (cadr args) "/map") base-frame-id (ros::time)))))))

)

;;;;
Expand Down

0 comments on commit 45fdd2d

Please sign in to comment.