#record the map for checking
data = getMapClient()
spamWriter = csv.writer(open('map_data.csv', 'wb'), delimiter=' ', quotechar='|', quoting=csv.QUOTE_MINIMAL)
for n in range(80, 250):
spamWriter.writerow(data.map.data[130 + 320 * n : 200 + 320 * n])
n += 1
#test the map
data = getMapClient()
#test points
x = -3.2
y = -0.58
map_index = int((y - data.map.info.origin.position.y) / data.map.info.resolution * data.map.info.width + (x - data.map.info.origin.position.x - 6.0) / data.map.info.resolution - 1)
for n in range(-20, 100):
map_line = list()
for m in range (-9, 9):
map_line.append(data.map.data[map_index + n * int(data.map.info.width) + m])
m += 1
print map_line
n += 1
rospy.loginfo([data.map.info.origin.position.x, data.map.info.origin.position.y])
rospy.loginfo([data.map.info.width, data.map.info.height])
#get furniture information from knowledge base
workspace_info = getWorkspaceOnMap()
furniture_geometry_list = list()
furniture_geometry_list = workspace_info.objectsInfo
#transfrom list
index = 0
furniture_geometry_list = list()
while index < len(furniture_geometry_list):
furniture_geometry = FurnitureGeometry()
furniture_geometry.pose.x = furniture_geometry_list[index].pose.position.x
furniture_geometry.pose.y = furniture_geometry_list[index].pose.position.y
furniture_pose_rpy = tf.transformations.euler_from_quaternion([furniture_geometry_list[index].pose.orientation.x, furniture_geometry_list[index].pose.orientation.y, furniture_geometry_list[index].pose.orientation.z, furniture_geometry_list[index].pose.orientation.w])
furniture_geometry.pose.theta = furniture_pose_rpy[2]
furniture_geometry.l = furniture_geometry_list[index].l
furniture_geometry.w = furniture_geometry_list[index].w
furniture_geometry.h = furniture_geometry_list[index].h
furniture_geometry_list.append(furniture_geometry)
index += 1
Definition at line 198 of file symbol_grounding_scan_base_region_server.py.
