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