$search
00001 #!/usr/bin/env python 00002 ################################################################# 00003 ##\file 00004 # 00005 # \note 00006 # Copyright (c) 2012 \n 00007 # University of Bedfordshire \n\n 00008 # 00009 ################################################################# 00010 # 00011 # \note 00012 # Project name: care-o-bot 00013 # \note 00014 # ROS stack name: srs_public 00015 # \note 00016 # ROS package name: srs_symbolic_grounding 00017 # 00018 # \author 00019 # Author: Beisheng Liu, email:beisheng.liu@beds.ac.uk 00020 # \author 00021 # Supervised by: Dayou Li, email:dayou.li@beds.ac.uk 00022 # 00023 # \date Date of creation: Mar 2012 00024 # 00025 # \brief 00026 # test client for symbol_grounding_deliver_base_region_server 00027 # 00028 ################################################################# 00029 # 00030 # Redistribution and use in source and binary forms, with or without 00031 # modification, are permitted provided that the following conditions are met: 00032 # 00033 # - Redistributions of source code must retain the above copyright 00034 # notice, this list of conditions and the following disclaimer. \n 00035 # - Redistributions in binary form must reproduce the above copyright 00036 # notice, this list of conditions and the following disclaimer in the 00037 # documentation and/or other materials provided with the distribution. \n 00038 # - Neither the name of the University of Bedfordshire nor the names of its 00039 # contributors may be used to endorse or promote products derived from 00040 # this software without specific prior written permission. \n 00041 # 00042 # This program is free software: you can redistribute it and/or modify 00043 # it under the terms of the GNU Lesser General Public License LGPL as 00044 # published by the Free Software Foundation, either version 3 of the 00045 # License, or (at your option) any later version. 00046 # 00047 # This program is distributed in the hope that it will be useful, 00048 # but WITHOUT ANY WARRANTY; without even the implied warranty of 00049 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00050 # GNU Lesser General Public License LGPL for more details. 00051 # 00052 # You should have received a copy of the GNU Lesser General Public 00053 # License LGPL along with this program. 00054 # If not, see <http://www.gnu.org/licenses/>. 00055 # 00056 ################################################################# 00057 import roslib; roslib.load_manifest('srs_symbolic_grounding') 00058 00059 from srs_symbolic_grounding.srv import * 00060 from srs_symbolic_grounding.msg import * 00061 from geometry_msgs.msg import * 00062 import rospy 00063 from srs_msgs.msg import SRSSpatialInfo 00064 00065 00066 def symbol_grounding_deliver_base_region_client(parent_obj_geometry, furniture_geometry_list): 00067 00068 rospy.wait_for_service('symbol_grounding_deliver_base_region') 00069 00070 symbol_grounding_deliver_base_region = rospy.ServiceProxy('symbol_grounding_deliver_base_region', SymbolGroundingDeliverBaseRegion) 00071 00072 try: 00073 resp = list() 00074 resp.append(symbol_grounding_deliver_base_region(parent_obj_geometry, furniture_geometry_list)) 00075 return resp 00076 00077 except rospy.ServiceException, e: 00078 00079 print "Service call failed: %s" %e 00080 00081 00082 00083 def getWorkspaceOnMap(): 00084 print 'test get all workspace (furnitures basically here) from map' 00085 try: 00086 requestNewTask = rospy.ServiceProxy('get_workspace_on_map', GetWorkspaceOnMap) 00087 res = requestNewTask('ipa-kitchen-map', True) 00088 return res 00089 except rospy.ServiceException, e: 00090 print "Service call failed: %s"%e 00091 00092 00093 00094 if __name__ == "__main__": 00095 00096 workspace_info = getWorkspaceOnMap() 00097 00098 parent_obj_geometry = SRSSpatialInfo() 00099 00100 parent_obj_geometry.pose.position.x = workspace_info.objectsInfo[1].pose.position.x 00101 parent_obj_geometry.pose.position.y = workspace_info.objectsInfo[1].pose.position.y 00102 parent_obj_geometry.pose.position.z = workspace_info.objectsInfo[1].pose.position.z 00103 parent_obj_geometry.pose.orientation.x = workspace_info.objectsInfo[1].pose.orientation.x 00104 parent_obj_geometry.pose.orientation.y = workspace_info.objectsInfo[1].pose.orientation.y 00105 parent_obj_geometry.pose.orientation.z = workspace_info.objectsInfo[1].pose.orientation.z 00106 parent_obj_geometry.pose.orientation.w = workspace_info.objectsInfo[1].pose.orientation.w 00107 parent_obj_geometry.l = workspace_info.objectsInfo[1].l 00108 parent_obj_geometry.w = workspace_info.objectsInfo[1].w 00109 parent_obj_geometry.h = workspace_info.objectsInfo[1].h 00110 rospy.loginfo(parent_obj_geometry.pose) 00111 00112 #parent_obj_geometry.pose.position.x = 8.6 00113 #parent_obj_geometry.pose.position.y = 2.5 00114 #parent_obj_geometry.pose.orientation.x = 0 00115 #parent_obj_geometry.pose.orientation.y = 0 00116 #parent_obj_geometry.pose.orientation.z = -0.999783754349 00117 #parent_obj_geometry.pose.orientation.w = 0.0207948293537 00118 #parent_obj_geometry.l = 1.5 00119 #parent_obj_geometry.w = 0.4 00120 00121 furniture_geometry_list = list() 00122 furniture_geometry_list = workspace_info.objectsInfo 00123 00124 00125 print "Requesting deliver base region." 00126 00127 result = symbol_grounding_deliver_base_region_client(parent_obj_geometry, furniture_geometry_list) 00128 00129 print result 00130 00131 00132