Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
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_scan_base_region_client(parent_obj_geometry, furniture_geometry_list):
00067
00068 rospy.wait_for_service('symbol_grounding_scan_base_region')
00069
00070 symbol_grounding_scan_base_region = rospy.ServiceProxy('symbol_grounding_scan_base_region', SymbolGroundingScanBaseRegion)
00071
00072 try:
00073 resp = list()
00074 resp.append(symbol_grounding_scan_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[2].pose.position.x
00101 parent_obj_geometry.pose.position.y = workspace_info.objectsInfo[2].pose.position.y
00102 parent_obj_geometry.pose.position.z = workspace_info.objectsInfo[2].pose.position.z
00103 parent_obj_geometry.pose.orientation.x = workspace_info.objectsInfo[2].pose.orientation.x
00104 parent_obj_geometry.pose.orientation.y = workspace_info.objectsInfo[2].pose.orientation.y
00105 parent_obj_geometry.pose.orientation.z = workspace_info.objectsInfo[2].pose.orientation.z
00106 parent_obj_geometry.pose.orientation.w = workspace_info.objectsInfo[2].pose.orientation.w
00107 parent_obj_geometry.l = workspace_info.objectsInfo[2].l
00108 parent_obj_geometry.w = workspace_info.objectsInfo[2].w
00109 parent_obj_geometry.h = workspace_info.objectsInfo[2].h
00110 rospy.loginfo(parent_obj_geometry.pose)
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121 furniture_geometry_list = list()
00122 furniture_geometry_list = workspace_info.objectsInfo
00123
00124
00125 print "Requesting scan base region."
00126
00127 result = symbol_grounding_scan_base_region_client(parent_obj_geometry, furniture_geometry_list)
00128
00129 print result
00130
00131
00132
00133
00134