Functions
def	getMapClient
def	handle_symbol_grounding_scan_base_pose
def	obstacleCheck
def	symbol_grounding_scan_base_pose_server

Function Documentation

def symbol_grounding_scan_base_pose_server.getMapClient ( )

Definition at line 82 of file symbol_grounding_scan_base_pose_server.py.

def symbol_grounding_scan_base_pose_server.handle_symbol_grounding_scan_base_pose ( req )

#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_pose_server.py.

def symbol_grounding_scan_base_pose_server.obstacleCheck	(	sbpl,
		fgl,
		po_x,
		po_y
	)

Definition at line 93 of file symbol_grounding_scan_base_pose_server.py.

def symbol_grounding_scan_base_pose_server.symbol_grounding_scan_base_pose_server ( )

Definition at line 474 of file symbol_grounding_scan_base_pose_server.py.

Functions

Function Documentation