From 85fa440c6be53759e53433c50410490617a5a533 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 8 Mar 2016 22:38:08 +0900 Subject: [PATCH 1/3] enable to set base_footprint name --- pr2eus/robot-interface.l | 47 +++++++++++++++++++++++++--------------- 1 file changed, 29 insertions(+), 18 deletions(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index 19c00a88e..fbc6dcb55 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -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 @@ -1065,13 +1063,15 @@ 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 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") &allow-other-keys) (prog1 (send-super* :init args) + (setq base-frame-id base-frame-id-name) (setq move-base-action (instance ros::simple-action-client :init move-base-action-name move_base_msgs::MoveBaseAction :groupname groupname)) @@ -1099,7 +1099,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) @@ -1146,7 +1146,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 @@ -1207,17 +1207,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) @@ -1251,17 +1251,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) @@ -1290,19 +1290,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] @@ -1502,6 +1502,17 @@ send-action [ send message to action server, it means robot will move ]" (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))))))) + ) ;;;; From 624cf183c158621c4b59dc65b71e670761503072 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 8 Mar 2016 23:05:27 +0900 Subject: [PATCH 2/3] move odom-callback to robot-move-base-interface.l --- pr2eus/pr2-interface.l | 14 -------------- pr2eus/robot-interface.l | 18 +++++++++++++++++- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/pr2eus/pr2-interface.l b/pr2eus/pr2-interface.l index fe2fe89d8..895eaa211 100644 --- a/pr2eus/pr2-interface.l +++ b/pr2eus/pr2-interface.l @@ -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 @@ -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 diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index fbc6dcb55..f842b649c 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -1064,14 +1064,17 @@ 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 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") ((:base-frame-id base-frame-id-name) "/base_footprint") &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)) @@ -1080,8 +1083,21 @@ 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)) + (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 From af18a5a92ca13f328fac0931f4d50ebacb832efb Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 8 Mar 2016 23:48:58 +0900 Subject: [PATCH 3/3] check if move-base-trajectory-action is available --- pr2eus/robot-interface.l | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index f842b649c..60fe2dd8c 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -1083,6 +1083,9 @@ 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) )) @@ -1334,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) @@ -1355,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] @@ -1370,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))) @@ -1510,7 +1522,7 @@ 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)))