semanticmodel: semanticmodel: willow

00001 #! /usr/bin/python
00002 
00003 #***********************************************************
00004 #* Software License Agreement (BSD License)
00005 #*
00006 #*  Copyright (c) 2009, Willow Garage, Inc.
00007 #*  All rights reserved.
00008 #*
00009 #*  Redistribution and use in source and binary forms, with or without
00010 #*  modification, are permitted provided that the following conditions
00011 #*  are met:
00012 #*
00013 #*   * Redistributions of source code must retain the above copyright
00014 #*     notice, this list of conditions and the following disclaimer.
00015 #*   * Redistributions in binary form must reproduce the above
00016 #*     copyright notice, this list of conditions and the following
00017 #*     disclaimer in the documentation and/or other materials provided
00018 #*     with the distribution.
00019 #*   * Neither the name of Willow Garage, Inc. nor the names of its
00020 #*     contributors may be used to endorse or promote products derived
00021 #*     from this software without specific prior written permission.
00022 #*
00023 #*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 #*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 #*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 #*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 #*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 #*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 #*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 #*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 #*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 #*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 #*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 #*  POSSIBILITY OF SUCH DAMAGE.
00035 #* 
00036 #* Author: Bhaskara Marthi
00037 #***********************************************************
00038 
00039 # Runs a tour of Willow map
00040 # Note will not work right now; you just have to remove the dependency
00041 # on continuous_ops_msgs
00042 
00043 PKG='semanticmodel'
00044 import roslib
00045 roslib.load_manifest(PKG)
00046 import rospy
00047 import time
00048 import actionlib
00049 import math
00050 
00051 from move_base_msgs.msg import *
00052 from geometry_msgs.msg import *
00053 import pr2_common_action_msgs.msg as act
00054 import actionlib_msgs.msg as am
00055 import continuous_ops_msgs.msg as cont_ops
00056 import std_msgs.msg as sm
00057 
00058 
00059 # Original goals; who knows where from.
00060 #goals = [
00061 #    [(19.380, 24.061, 0.000), (0.000, 0.000, 0.953, -0.304)],
00062 #    [(27.193, 15.448, 0.000), (0.000, 0.000, 0.526, 0.851)],
00063 #    [(29.872, 21.884, 0.000), (0.000, 0.000, 0.979, 0.206)],
00064 #    [(18.632, 26.155, 0.000), (0.000, 0.000, 0.807, 0.590)],
00065 #    [(20.772, 38.251, 0.000), (0.000, 0.000, 0.980, 0.201)],
00066 #    [(5.184, 22.094, 0.000), (0.000, 0.000, 0.559, 0.829)],
00067 #    [(17.346, 54.900, 0.000), (0.000, 0.000, 0.160, 0.987)],
00068 #    [(25.170, 52.124, 0.000), (0.000, 0.000, -0.099, 0.995)],
00069 #    [(20.409, 38.265, 0.000), (0.000, 0.000, -0.678, 0.735)],
00070 #    [(24.109, 20.406, 0.000), (0.000, 0.000, 0.973, 0.230)]
00071 #]
00072 places = {\
00073 "whiteboard": ("green room looking towards whiteboard from across", (17.52, 18.11, 1.25)),
00074 "magazines": ("looking at magazines in green room", (18.30, 25.81, 2.82)),
00075 "bigscreen": ("looking at big screen", (17.81, 22.15, 2.74)),
00076 "cafe1": ("at entrance to cafeteria", (36.11, 21.77, 1.94)),
00077 "coffee": ("looking diagonally at coffee machine", (34.56, 16.11, 1.22)),
00078 "cafe2": ("looking out side entrance", (27.83, 10.84, 2.78)),
00079 "glass": ("looking out glass door", (26.75, 15.04, 1.86)),
00080 "whitelab": ("looking towards white lab", (33.39, 30.88, 1.20)),
00081 "frontdoor": ("looking towards front door", (41.45, 20.78, 1.15)),
00082 "pooltable": ("looking at pool table from narrow hallway", (19.90, 32.74, 1.67)),
00083 "bike": ("looking out bike exit", (17.12, 55.21, 2.69)),
00084 "research": ("looking out research exit", (05.00, 21.40, 1.75)),
00085 "fishbowl": ("fishbowl, looking at pool table", (12.18, 32.55, 0.55)),
00086 "boondocks": ("boondocks, looking at white lab", (41.23, 49.47, 0.31)),
00087 "looking-at-fridge": ("pool room, looking at fridge", (20.4, 36.32, 0.448)),
00088 }
00089 
00090 
00091 # Mac's goals.
00092 goals = [
00093     [(20.11, 25.77, 0.0), (0.0, 0.0, -0.83, 0.55)],
00094     [(17.85, 19.21, 0.0), (0.0, 0.0, -0.19, 0.98)],
00095     [(27.06, 15.57, 0.0), (0.0, 0.0, 0.56, 0.82)],
00096     [(35.37, 20.52, 0.0), (0, 0, -0.96, -0.25)],
00097     [(31.26, 25.06, 0.0), (0, 0, -0.829, 0.55)],
00098     [(19.78, 33.32, 0.0), (0, 0, 0.6, 0.79)],
00099     [(20.76, 37,84, 0.0), (0, 0, 0.98, 0.17)],
00100     [(19.22, 24.29, 0.0), (0, 0, -0.71, 0.69)]
00101 ]
00102 
00103 def convert_2d_pose(p):
00104   return [(p[0], p[1], 0), (0, 0, math.sin(p[2]/2), math.cos(p[2]/2))]
00105 
00106 # Bhaskara goals (test)
00107 wp = ["whiteboard", "bigscreen", "magazines", "pooltable", "looking-at-fridge", "research", "bike", "boondocks",
00108       "cafe1", "coffee", "cafe2", "glass", "frontdoor", "whitelab", "whiteboard"
00109       ]
00110 
00111 
00112 goals = [convert_2d_pose(places[p][1]) for p in wp]
00113 print "The goals are ", [places[p] for p in wp]
00114     
00115 
00116 
00117 def mb_goal(goal):
00118     return MoveBaseGoal(PoseStamped(sm.Header(frame_id = 'map'),
00119                                       Pose(Point(goal[0][0], goal[0][1],
00120                                                  goal[0][2]),
00121                                            Quaternion(goal[1][0], goal[1][1],
00122                                                       goal[1][2], goal[1][3]))))
00123                                            
00124 
00125 ACTION_SERVER=None
00126 
00127 
00128 
00129 
00130 
00131 
00132 def do_the_tour(msg=None):
00133     global ACTION_SERVER
00134     tuck_client = actionlib.SimpleActionClient('tuck_arms', act.TuckArmsAction)
00135     rospy.loginfo("Waiting for tuck_arms")
00136     tuck_client.wait_for_server()
00137     while not rospy.is_shutdown():
00138         rospy.loginfo("Sending tuck goal")
00139         res = tuck_client.send_goal_and_wait(act.TuckArmsGoal(True, True),
00140                 execute_timeout=rospy.Duration(20.0))
00141         rospy.loginfo("Tuck result was {0}".format(res))
00142         if res == am.GoalStatus.SUCCEEDED:
00143            break
00144 
00145       
00146 
00147 
00148     client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
00149     rospy.loginfo("Waiting for move_base")
00150     client.wait_for_server()
00151     rospy.loginfo("Sending goals")
00152 
00153     for idx, goal in list(enumerate(goals))[:]:
00154         mb = mb_goal(goal)
00155         rospy.loginfo("Sending goal {1} at {0}".format(goal, wp[idx]))
00156         client.send_goal(mb)
00157         while not client.wait_for_result(rospy.Duration(1.0)):
00158             if rospy.is_shutdown():
00159                 rospy.loginfo("Tour node shutting down, cancelling nav goals")
00160                 client.cancel_all_goals()
00161                 return
00162             if ACTION_SERVER.is_preempt_requested():
00163                 rospy.loginfo("Tour has been preempted.  Cancelling")
00164                 client.cancel_all_goals()
00165                 ACTION_SERVER.set_preempted()
00166                 return 
00167         rospy.sleep(3.0)
00168      
00169     rospy.loginfo('Done with tour')
00170     ACTION_SERVER.set_succeeded()
00171 
00172 
00173 
00174 if __name__ == '__main__':
00175     rospy.init_node('willow_tour_node')
00176     ACTION_SERVER = actionlib.SimpleActionServer("semantic_passive", cont_ops.TaskAction,
00177             execute_cb = do_the_tour)
00178     ACTION_SERVER.start()
00179
willow_tour.py
