outlet_server.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 PKG = 'pr2_plugs_actions' 00039 00040 import roslib; roslib.load_manifest(PKG) 00041 import roslib.message 00042 import rospy 00043 import yaml 00044 00045 from pr2_plugs_msgs.srv import GetOutlets, GetOutletsRequest, GetOutletsResponse 00046 from pr2_plugs_msgs.msg import OutletPose 00047 00048 def serve_plug_locations(req): 00049 rospy.loginfo("Serving plug locations") 00050 try: 00051 poses = rospy.get_param('outlet_approach_poses') 00052 except: 00053 print 'no poses' 00054 poses = {} 00055 if not 'local' in poses: 00056 poses['local'] = {'position':[0,0,0], 'orientation': [0,0,0,0]} 00057 print poses 00058 00059 resp = GetOutletsResponse() 00060 for pose in poses: 00061 op = OutletPose() 00062 op.name = pose 00063 roslib.message.fill_message_args(op.approach_pose, poses[pose]) 00064 resp.poses.append(op) 00065 print resp 00066 00067 return resp 00068 00069 if __name__ == "__main__": 00070 rospy.init_node("outlet_server") 00071 s = rospy.Service("outlet_locations", GetOutlets, serve_plug_locations) 00072 rospy.spin()