navigation_task.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 import roslib; roslib.load_manifest('navigation_task')
00039 import rospy
00040 import smach
00041 import smach_ros
00042 import actionlib
00043 import random
00044 
00045 from smach_ros import SimpleActionState
00046 
00047 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
00048 from continuous_ops_msgs.msg import TaskAction
00049 from geometry_msgs.msg import PoseStamped, Pose, Quaternion, Point
00050 from pr2_common_action_msgs.msg import TuckArmsAction, TuckArmsGoal
00051 from std_msgs.msg import Header
00052 
00053 nav_goals = [
00054     [(19.380, 24.061, 0.000), (0.000, 0.000, 0.953, -0.304)],
00055     [(27.193, 15.448, 0.000), (0.000, 0.000, 0.526, 0.851)],
00056     [(29.872, 21.884, 0.000), (0.000, 0.000, 0.979, 0.206)],
00057     [(18.632, 26.155, 0.000), (0.000, 0.000, 0.807, 0.590)],
00058     [(20.772, 38.251, 0.000), (0.000, 0.000, 0.980, 0.201)],
00059     [(5.184, 22.094, 0.000), (0.000, 0.000, 0.559, 0.829)],
00060     [(17.346, 54.900, 0.000), (0.000, 0.000, 0.160, 0.987)],
00061     [(25.170, 52.124, 0.000), (0.000, 0.000, -0.099, 0.995)],
00062     [(20.409, 38.265, 0.000), (0.000, 0.000, -0.678, 0.735)],
00063     [(24.109, 20.406, 0.000), (0.000, 0.000, 0.973, 0.230)]
00064 ]
00065 
00066 #define state navigate
00067 class NavigateSM(smach.State):
00068     def __init__(self, goals, desired_num_goals):
00069         smach.State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'])
00070         self.goals = goals
00071         self.desired_num_goals = desired_num_goals
00072         self.goal_count = 0
00073         #make sure to randomize the order of the goals
00074         random.shuffle(self.goals)
00075         rospy.loginfo("Goals are: %s" % goals)
00076         self.client = actionlib.SimpleActionClient('pr2_move_base', MoveBaseAction)
00077         self.goal_timeout = rospy.Duration.from_sec(rospy.get_param('~goal_timeout', 300.0))
00078 
00079     def mb_goal(self, goal):
00080         return MoveBaseGoal(PoseStamped(Header(frame_id = 'map'),
00081                                       Pose(Point(goal[0][0], goal[0][1],
00082                                                  goal[0][2]),
00083                                            Quaternion(goal[1][0], goal[1][1],
00084                                                       goal[1][2], goal[1][3])))) 
00085 
00086     def execute(self, userdata):
00087         #randomize the goals for execution
00088         random.shuffle(self.goals)
00089 
00090         #reset the goal count
00091         self.goal_count = 0
00092         rospy.loginfo("Waiting for server")
00093         #make sure that the action server is up 
00094         self.client.wait_for_server()
00095         rospy.loginfo("Server is up")
00096         for goal in self.goals:
00097             #if we've been to enough goals... then we'll succeed
00098             if self.desired_num_goals != 0 and self.goal_count >= self.desired_num_goals:
00099                 rospy.loginfo("We've been to %d goals... success." % self.goal_count)
00100                 return 'succeeded'
00101 
00102             #send goal to move_base here
00103             rospy.loginfo("Sending goal %s" % goal)
00104             self.client.send_goal(self.mb_goal(goal))
00105             start_time = rospy.Time.now()
00106 
00107             r = rospy.Rate(10.0)
00108 
00109             #wait for the goal to finish (we don't care how, we'll just move on to the next goal)
00110             #for a very small amount of time the client should really just have an 
00111             #is_done method that wouldn't require this
00112             while not self.client.wait_for_result(rospy.Duration.from_sec(0.001)):
00113                 if self.preempt_requested():
00114                     self.service_preempt()
00115                     #preempt move_base here
00116                     self.client.cancel_goal()
00117                     return 'preempted'
00118 
00119                 #check to see if the goal has exceeded its timeout
00120                 if start_time + self.goal_timeout < rospy.Time.now():
00121                     rospy.logwarn('Navigation task timed out trying to get to a goal after %.2f seconds, sending a new goal' % self.goal_timeout.to_sec())
00122                     break
00123 
00124                 r.sleep()
00125 
00126             #increment the goal count
00127             self.goal_count += 1
00128 
00129         return 'succeeded'
00130 
00131 def main():
00132     rospy.init_node('navigation_task')
00133     #Create a state machine with our one state, this state machine will never actually succeed or abort right now
00134     sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
00135     with sm:
00136         tuck_goal = TuckArmsGoal()
00137         tuck_goal.tuck_left=True
00138         tuck_goal.tuck_right=True
00139         tucking_succeeded = False
00140         smach.StateMachine.add('TUCK_ARMS', SimpleActionState('tuck_arms', TuckArmsAction,
00141                                                               goal=tuck_goal),
00142                                transitions={'succeeded':'NAVIGATE'})
00143         smach.StateMachine.add('NAVIGATE', NavigateSM(nav_goals, 0), transitions = {'succeeded':'NAVIGATE','preempted':'preempted'})
00144 
00145     asw = smach_ros.ActionServerWrapper(
00146         'navigation_task', TaskAction,
00147         wrapped_container = sm,
00148         succeeded_outcomes = ['succeeded'],
00149         aborted_outcomes = ['aborted'],
00150         preempted_outcomes = ['preempted'])
00151 
00152     asw.run_server()
00153 
00154     rospy.spin()
00155 
00156 if __name__ == '__main__':
00157     main()


navigation_task
Author(s): Eitan Marder-Eppstein
autogenerated on Fri Dec 6 2013 21:20:24