$search
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