cart_pushing_executive: tour.py Source File

00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 # Author: Bhaskara Marthi
00035 
00036 import roslib
00037 roslib.load_manifest('cart_pushing_executive')
00038 import rospy
00039 import time
00040 import actionlib
00041 import tf
00042 
00043 from move_base_msgs.msg import *
00044 from geometry_msgs.msg import *
00045 from roslib.msg import *
00046 
00047 goals = [
00048     [(27.65, 17.29, 0.0), (0.0, 0.0, 0.57, 0.82)],
00049     [(25.1, 24.0, 0), (0, 0, 1.0, 0.0)],
00050     [(18.6, 24.4, 0), (0, 0, -0.83, 0.55)],
00051     [(18.7, 18.8, 0), (0, 0, -0.18, 0.98)],
00052     [(27.8, 17.9, 0), (0, 0, 0.54, 0.84)],
00053     [(26.1, 23.6, 0), (0, 0, 0.98, 0.20)],
00054     [(17.9, 18.9, 0), (0, 0, 0.71, 0.71)],
00055     [(18.462, 24.971, 0), (0, 0, 0.643354674336, 0.765568261496)],
00056     [(18.7669181824, 29.0535163879, 0), (0, 0, 0.551830767493, 0.83395611638)],
00057     [(20.036, 32.439, 0.0), (0, 0, 0.706, 0.708)],
00058     [(20.858581543, 39.1036987305, 0), (0, 0, 0.601096665154, 0.799176325438)],
00059     [(20.6498146057, 36.3583908081, 0), (0, 0, -0.760489363805, 0.64935038888)],
00060     [(19.9862194061, 33.1941947937, 0), (0, 0, -0.70200284829, 0.712174136706)],
00061     [(19.2100734711, 30.0183944702, 0), (0, 0, -0.836560676379, 0.547874287348)],
00062     [(18.6010341644, 26.2357540131, 0), (0, 0, -0.609730442973, 0.79260884862)]
00063     ]
00064 
00065 def squared_dist(p1, p2):
00066     dx = p1.point.x - p2[0]
00067     dy = p1.point.y - p2[1]
00068     return dx*dx + dy*dy
00069 
00070 def mb_goal(goal):
00071     return MoveBaseGoal(PoseStamped(Header(frame_id = 'map'),
00072                                       Pose(Point(goal[0][0], goal[0][1],
00073                                                  goal[0][2]),
00074                                            Quaternion(goal[1][0], goal[1][1],
00075                                                       goal[1][2], goal[1][3]))))
00076 if __name__ == '__main__':
00077     rospy.init_node('willow_tour_node')
00078     client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
00079     rospy.loginfo("Waiting for move_base")
00080     client.wait_for_server()
00081     rospy.loginfo("Sending goals")
00082     tf = tf.TransformListener()
00083     rospy.sleep(2)
00084 
00085     p = PointStamped()
00086     p.header.frame_id = 'base_footprint'
00087     p.header.stamp = rospy.Time()
00088     pos = tf.transformPoint('map', p)
00089     
00090     best_dist = 1000000
00091     current = 0
00092     for (i, g) in enumerate(goals):
00093         if squared_dist(pos, g[0]) < best_dist:
00094             best_dist = squared_dist(pos, g[0])
00095             current = i
00096     rospy.loginfo('Closest waypoint is %s: %s', current, goals[current])
00097     
00098     for c in range(5):
00099         while current < len(goals):
00100             mb = mb_goal(goals[current])
00101             rospy.loginfo("Sending goal %s" % goals[current])
00102             client.send_goal(mb)
00103             client.wait_for_result()
00104             current +=1
00105         current = 0
00106 
00107     rospy.loginfo('Done with tour')
00108 
00109 
00110