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 PKG='semanticmodel' 00038 import roslib 00039 roslib.load_manifest(PKG) 00040 import rospy 00041 import time 00042 import actionlib 00043 00044 from move_base_msgs.msg import * 00045 from geometry_msgs.msg import * 00046 from roslib.msg import * 00047 00048 # Original goals; who knows where from. 00049 #goals = [ 00050 # [(19.380, 24.061, 0.000), (0.000, 0.000, 0.953, -0.304)], 00051 # [(27.193, 15.448, 0.000), (0.000, 0.000, 0.526, 0.851)], 00052 # [(29.872, 21.884, 0.000), (0.000, 0.000, 0.979, 0.206)], 00053 # [(18.632, 26.155, 0.000), (0.000, 0.000, 0.807, 0.590)], 00054 # [(20.772, 38.251, 0.000), (0.000, 0.000, 0.980, 0.201)], 00055 # [(5.184, 22.094, 0.000), (0.000, 0.000, 0.559, 0.829)], 00056 # [(17.346, 54.900, 0.000), (0.000, 0.000, 0.160, 0.987)], 00057 # [(25.170, 52.124, 0.000), (0.000, 0.000, -0.099, 0.995)], 00058 # [(20.409, 38.265, 0.000), (0.000, 0.000, -0.678, 0.735)], 00059 # [(24.109, 20.406, 0.000), (0.000, 0.000, 0.973, 0.230)] 00060 #] 00061 00062 # Mac's goals. 00063 #goals = [ 00064 # [(20.11, 25.77, 0.0), (0.0, 0.0, -0.83, 0.55)], 00065 # [(17.85, 19.21, 0.0), (0.0, 0.0, -0.19, 0.98)], 00066 # [(27.06, 15.57, 0.0), (0.0, 0.0, 0.56, 0.82)], 00067 # [(35.37, 20.52, 0.0), (0, 0, -0.96, -0.25)], 00068 # [(31.26, 25.06, 0.0), (0, 0, -0.829, 0.55)], 00069 # [(19.78, 33.32, 0.0), (0, 0, 0.6, 0.79)], 00070 # [(20.76, 37,84, 0.0), (0, 0, 0.98, 0.17)], 00071 # [(19.22, 24.29, 0.0), (0, 0, -0.71, 0.69)] 00072 #] 00073 00074 # Mac's (long tour) goals. 00075 goals = [ 00076 [(18.1692905426, 19.1994800568, 0.0), 00077 (0.0, 0.0, -0.192855206976, 0.98122722605)], 00078 [(29.8882846832, 21.8147888184, 0.0), 00079 (0.0, 0.0, 0.556712602762, 0.830705169074)], 00080 [(36.4701004028, 22.6947193146, 0.0), 00081 (0.0, 0.0, -0.838297314816, 0.545213363714)], 00082 [(34.0851974487, 16.0869045258, 0.0), 00083 (0.0, 0.0, 0.904685123893, 0.426080774744)], 00084 [(28.3673648834, 10.1040353775, 0.0), 00085 (0.0, 0.0, 0.976806145604, 0.214125556417)], 00086 [(28.1669883728, 17.825050354, 0.0), 00087 (0.0, 0.0, 0.527215004905, 0.84973192161)], 00088 [(23.5591754913, 24.3424053192, 0.0), 00089 (0.0, 0.0, 0.980031762909, 0.19884100103)], 00090 [(19.0632476807, 33.7512588501, 0.0), 00091 (0.0, 0.0, 0.250532110313, 0.968108290277)], 00092 [(8.69736289978, 30.8772182465, 0.0), 00093 (0.0, 0.0, -0.834096085225, 0.551619180787)], 00094 [(7.90707015991, 31.473531723, 0.0), 00095 (0.0, 0.0, 0.652970028173, 0.757383748378)], 00096 [(17.3875350952, 55.0858535767, 0.0), 00097 (0.0, 0.0, 0.0781252043804, 0.996943555293)], 00098 [(25.609790802, 52.1704063416, 0.0), 00099 (0.0, 0.0, -0.831385731323, 0.555695749266)], 00100 [(20.7248096466, 38.6204910278, 0.0), 00101 (0.0, 0.0, -0.727519456268, 0.686087050418)], 00102 [(17.118265152, 17.8405857086, 0.0), 00103 (0.0, 0.0, 0.418973493025, 0.907998464835)] 00104 ] 00105 00106 00107 def mb_goal(goal): 00108 return MoveBaseGoal(PoseStamped(Header(frame_id = 'map'), 00109 Pose(Point(goal[0][0], goal[0][1], 00110 goal[0][2]), 00111 Quaternion(goal[1][0], goal[1][1], 00112 goal[1][2], goal[1][3])))) 00113 if __name__ == '__main__': 00114 rospy.init_node('willow_tour_node') 00115 client = actionlib.SimpleActionClient('move_base', MoveBaseAction) 00116 rospy.loginfo("Waiting for move_base") 00117 client.wait_for_server() 00118 rospy.loginfo("Sending goals") 00119 00120 for goal in goals: 00121 mb = mb_goal(goal) 00122 rospy.loginfo("Sending goal %s" % goal) 00123 client.send_goal(mb) 00124 client.wait_for_result() 00125 00126 rospy.loginfo('Done with tour') 00127