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

robot-interface.l: fix robot-move-base-interface #210

Merged
merged 3 commits into from
Mar 9, 2016
Merged

robot-interface.l: fix robot-move-base-interface #210

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
85fa440
enable to set base_footprint name
k-okada Mar 8, 2016
624cf18
move odom-callback to robot-move-base-interface.l
k-okada Mar 8, 2016
af18a5a
check if move-base-trajectory-action is available
k-okada Mar 8, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 0 additions & 14 deletions pr2eus/pr2-interface.l
View file
Open in desktop
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
Open in desktop
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