pr2_move_base.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 #***********************************************************
00003 #* Software License Agreement (BSD License)
00004 #*
00005 #*  Copyright (c) 2009, Willow Garage, Inc.
00006 #*  All rights reserved.
00007 #*
00008 #*  Redistribution and use in source and binary forms, with or without
00009 #*  modification, are permitted provided that the following conditions
00010 #*  are met:
00011 #*
00012 #*   * Redistributions of source code must retain the above copyright
00013 #*     notice, this list of conditions and the following disclaimer.
00014 #*   * Redistributions in binary form must reproduce the above
00015 #*     copyright notice, this list of conditions and the following
00016 #*     disclaimer in the documentation and/or other materials provided
00017 #*     with the distribution.
00018 #*   * Neither the name of Willow Garage, Inc. nor the names of its
00019 #*     contributors may be used to endorse or promote products derived
00020 #*     from this software without specific prior written permission.
00021 #*
00022 #*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 #*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 #*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 #*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 #*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 #*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 #*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 #*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 #*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 #*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 #*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 #*  POSSIBILITY OF SUCH DAMAGE.
00034 #* 
00035 #* Author: Eitan Marder-Eppstein
00036 #***********************************************************
00037 
00038 PKG = "pr2_move_base"
00039 
00040 import roslib; roslib.load_manifest(PKG) 
00041 
00042 import rospy
00043 import actionlib
00044 from std_msgs.msg import Bool
00045 from pr2_msgs.msg import LaserTrajCmd
00046 from pr2_msgs.msg import PowerState
00047 from pr2_msgs.srv import SetLaserTrajCmd
00048 from actionlib_msgs.msg import GoalStatus
00049 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseResult, MoveBaseFeedback
00050 from pr2_controllers_msgs.msg import SingleJointPositionAction, SingleJointPositionGoal
00051 from pr2_common_action_msgs.msg import TuckArmsAction, TuckArmsGoal
00052 import dynamic_reconfigure.client
00053 from pr2_controllers_msgs.msg import PointHeadAction, PointHeadGoal
00054 
00055 def feedback_cb(feedback):
00056     server.publish_feedback(feedback)
00057 
00058 def set_tilt_profile(position, time_from_start):
00059     cmd = LaserTrajCmd()
00060     cmd.profile = 'blended_linear'
00061     cmd.position = position
00062     cmd.time_from_start = [rospy.Time.from_sec(x) for x in time_from_start]
00063     cmd.max_velocity = 10
00064     cmd.max_acceleration = 30
00065     try:
00066         tilt_profile_client.call(cmd)
00067     except rospy.ServiceException, e:
00068         rospy.logerr("Couldn't set the profile on the laser. Exception %s" % e)
00069         return False
00070     return True
00071 
00072 def configure_head():
00073     head_client = actionlib.SimpleActionClient('head_traj_controller/point_head_action', PointHeadAction)
00074     head_client.wait_for_server()
00075     point_head_goal = PointHeadGoal()
00076     point_head_goal.target.header.frame_id = 'base_link'
00077     point_head_goal.target.point.x = 3.0
00078     point_head_goal.target.point.y = 0.0
00079     point_head_goal.target.point.z = 1.0
00080     point_head_goal.pointing_frame = 'head_tilt_link'
00081     point_head_goal.pointing_axis.x = 1
00082     point_head_goal.pointing_axis.y = 0
00083     point_head_goal.pointing_axis.z = 0
00084 
00085     head_client.send_goal(point_head_goal)
00086     head_client.wait_for_result(rospy.Duration(5.0))
00087 
00088 
00089 def configure_laser():
00090     #TODO: remove hack to get things working in gazebo
00091     try: 
00092         rospy.wait_for_service('tilt_hokuyo_node/set_parameters', 1.0)
00093     except rospy.exceptions.ROSException, e:
00094         rospy.logerr("Couldn't set parameters %s" % e)
00095         return
00096     #end TODO
00097 
00098     client = dynamic_reconfigure.client.Client('tilt_hokuyo_node')
00099     global old_config
00100     old_config = client.get_configuration(2.0)
00101     new_config = {'skip': 0, 'intensity': 0, 'min_ang': -1.57, 'max_ang': 1.57, 'calibrate_time': 1, 'cluster': 1, 'time_offset': 0.0} 
00102     rospy.loginfo('Setting laser to the navigation configuration: %s' % new_config)
00103     client.update_configuration(new_config)
00104 
00105 def restore_laser():
00106     #TODO: remove hack to get things working in gazebo
00107     try: 
00108         rospy.wait_for_service('tilt_hokuyo_node/set_parameters', 1.0)
00109     except rospy.exceptions.ROSException, e:
00110         rospy.logerr("Couldn't set the profile on the laser. Exception %s" % e)
00111         return
00112     #end TODO
00113 
00114     rospy.loginfo('Setting laser back to this configuration: %s' % old_config)
00115     client = dynamic_reconfigure.client.Client('tilt_hokuyo_node')
00116     client.update_configuration(old_config)
00117 
00118 def on_shutdown():
00119     restore_laser()
00120     set_tilt_profile([0.0, 0.0], [0.0, 1.0])
00121 
00122 def execute_cb(goal):
00123     rospy.loginfo("Received a goal")
00124 
00125     # check for network cable
00126     if network_connected:
00127         server.set_aborted(None, "Dangerous to navigate with network cable connected.")
00128         return
00129 
00130     # check for power cable
00131     if ac_power_connected:
00132         server.set_aborted(None, "Dangerous to navigate with AC power cable connected.")
00133         return
00134 
00135     # check for power cable
00136     # TODO
00137 
00138     # start the arms tucking
00139     tuck_arms_client = actionlib.SimpleActionClient('tuck_arms', TuckArmsAction)
00140     tuck_arms_client.wait_for_server()
00141     tuck_goal = TuckArmsGoal()
00142     tuck_goal.tuck_left=True
00143     tuck_goal.tuck_right=True
00144     tuck_arms_client.send_goal(tuck_goal)
00145 
00146     # start the torso lowering
00147     torso_client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction)
00148     torso_client.wait_for_server()
00149     torso_down_goal = SingleJointPositionGoal()
00150     torso_down_goal.position = 0
00151     torso_client.send_goal(torso_down_goal)
00152 
00153     # configure the tilting laser
00154     if not set_tilt_profile([1.05,  -.7, 1.05], [0.0, 1.8, 2.0125 + .3]):
00155         server.set_aborted(MoveBaseResult(), "Couldn't set the profile on the laser")
00156         return
00157 
00158     configure_laser()
00159     configure_head()
00160 
00161     # wait for tuck_arms to finish. (Don't wait for torso to lower, that is a Nice To Have)
00162     while not tuck_arms_client.wait_for_result(rospy.Duration(0.1)):
00163         if server.is_preempt_requested():
00164             if not server.is_new_goal_available():
00165                 tuck_arms_client.cancel_goal()
00166 
00167     tuck_state = tuck_arms_client.get_state()
00168     if tuck_state != GoalStatus.SUCCEEDED:
00169         if tuck_state == GoalStatus.PREEMPTED:
00170             server.set_preempted(MoveBaseResult(), "Tuck-arms preempted")
00171         elif tuck_state == GoalStatus.ABORTED:
00172             server.set_aborted(MoveBaseResult(), "Tuck-arms aborted")
00173         return
00174 
00175     # Now everything should be ready so send the navigation goal.
00176     move_base_client.send_goal(goal, None, None, feedback_cb)
00177 
00178     while not move_base_client.wait_for_result(rospy.Duration(0.1)):
00179         #in the unlikely event of network cable being plugged in while moving, stop moving.
00180         if network_connected:
00181             move_base_client.cancel_goal()
00182             server.set_aborted(None, "Dangerous to navigate with network cable connected.")
00183             return
00184 
00185         #in the unlikely event of ac power cable being plugged in while moving, stop moving.
00186         if ac_power_connected:
00187             move_base_client.cancel_goal()
00188             server.set_aborted(None, "Dangerous to navigate with ac power cable connected.")
00189             return
00190 
00191         if server.is_preempt_requested():
00192             if not server.is_new_goal_available():
00193                 rospy.loginfo("Preempt requested without new goal, cancelling move_base goal.")
00194                 move_base_client.cancel_goal()
00195 
00196             server.set_preempted(MoveBaseResult(), "Got preempted by a new goal")
00197             return
00198 
00199 
00200     terminal_state = move_base_client.get_state()
00201     result = move_base_client.get_result()
00202     if terminal_state == GoalStatus.PREEMPTED:
00203         server.set_preempted(result)
00204     elif terminal_state == GoalStatus.SUCCEEDED:
00205         server.set_succeeded(result)
00206     elif terminal_state == GoalStatus.ABORTED:
00207         server.set_aborted(result)
00208     else:
00209         server.set_aborted(result, "Unknown result from move_base")
00210 
00211 def handle_network_connected( connectedness_msg ):
00212     global network_connected
00213     network_connected = connectedness_msg.data
00214 
00215 def handle_power_state( power_state_msg ):
00216     global ac_power_connected
00217     ac_power_connected = (power_state_msg.AC_present > 0)
00218 
00219 if __name__ == '__main__':
00220     name = 'pr2_move_base'
00221     rospy.init_node(name)
00222 
00223     #listen for messages about the network cable being plugged in.
00224     global network_connected
00225     network_connected = False #default presumes we are fine if we never hear anything.
00226     rospy.Subscriber("network/connected", Bool, handle_network_connected)
00227 
00228     #listen for messages about the power cable being plugged in.
00229     global ac_power_connected
00230     ac_power_connected = False #default presumes we are fine if we never hear anything.
00231     rospy.Subscriber("power_state", PowerState, handle_power_state)
00232 
00233     #create the action client to move_base
00234     move_base_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
00235     move_base_client.wait_for_server()
00236     move_base_goal = MoveBaseGoal()
00237 
00238     #create a client to the tilt laser
00239     rospy.wait_for_service('laser_tilt_controller/set_traj_cmd')
00240     tilt_profile_client = rospy.ServiceProxy('laser_tilt_controller/set_traj_cmd', SetLaserTrajCmd)
00241 
00242     server = actionlib.SimpleActionServer(name, MoveBaseAction, execute_cb)
00243     rospy.loginfo('%s: Action server running', name)
00244 
00245     global old_config
00246     old_config = {}
00247 
00248     #we'll set the tilt profile and configure the laser by default when we start up
00249     #NOTE: IF YOU CHANGE THIS, ALSO MAKE SURE TO CHANGE THE tilt_lasers_filters.yaml FILE IN pr2_navigation_perception
00250     set_tilt_profile([1.05,  -.7, 1.05], [0.0, 1.8, 2.0125 + .3])
00251     configure_laser()
00252 
00253     rospy.on_shutdown(on_shutdown)
00254 
00255     rospy.spin()


pr2_move_base
Author(s): Eitan Marder-Eppstein
autogenerated on Mon Dec 2 2013 11:49:07