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
00058 import roslib; roslib.load_manifest('srs_symbolic_grounding')
00059 from srs_symbolic_grounding.srv import *
00060
00061 from std_msgs.msg import *
00062 from geometry_msgs.msg import *
00063 from nav_msgs.msg import *
00064 from nav_msgs.srv import *
00065
00066 import rospy
00067 import math
00068 import tf
00069 from tf.transformations import euler_from_quaternion
00070
00071 '''
00072 def getWorkspaceOnMap():
00073 print 'test get all workspace (furnitures basically here) from map'
00074 try:
00075 requestNewTask = rospy.ServiceProxy('get_workspace_on_map', GetWorkspaceOnMap)
00076 res = requestNewTask('ipa-kitchen-map', True)
00077 return res
00078 except rospy.ServiceException, e:
00079 print "Service call failed: %s"%e
00080 '''
00081
00082 def getMapClient():
00083
00084 try:
00085 reqMap = rospy.ServiceProxy('static_map', GetMap)
00086 res = reqMap()
00087 return res
00088 except rospy.ServiceException, e:
00089 print "Service call failed: %s"%e
00090
00091
00092
00093 def obstacleCheck(sbpl, fgl, po_x, po_y):
00094 obstacle_checked_scan_base_pose_list = list()
00095 wall_checked_scan_base_pose_list = list()
00096 all_checked_scan_base_pose_list = list()
00097 scan_base_pose_list = sbpl
00098 furniture_geometry_list = fgl
00099 parent_obj_x = po_x
00100 parent_obj_y = po_y
00101
00102
00103 dist_to_obstacles = 0.4
00104
00105
00106 index_1 = 0
00107 while index_1 < len(scan_base_pose_list):
00108 index_2 = 0
00109 while index_2 < len(furniture_geometry_list):
00110 th = math.atan((scan_base_pose_list[index_1].y - furniture_geometry_list[index_2].pose.y) / (scan_base_pose_list[index_1].x - furniture_geometry_list[index_2].pose.x + 0.0001))
00111 if scan_base_pose_list[index_1].x < furniture_geometry_list[index_2].pose.x and scan_base_pose_list[index_1].y > furniture_geometry_list[index_2].pose.y:
00112 th = math.pi + th
00113 if scan_base_pose_list[index_1].x < furniture_geometry_list[index_2].pose.x and scan_base_pose_list[index_1].y < furniture_geometry_list[index_2].pose.y:
00114 th = -math.pi + th
00115 delta_x = math.sqrt((scan_base_pose_list[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (scan_base_pose_list[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.cos(th - furniture_geometry_list[index_2].pose.theta)
00116 delta_y = math.sqrt((scan_base_pose_list[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (scan_base_pose_list[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.sin(th - furniture_geometry_list[index_2].pose.theta)
00117 if (delta_x <= -(furniture_geometry_list[index_2].w / 2.0 + dist_to_obstacles) or delta_x >= (furniture_geometry_list[index_2].w / 2.0 + dist_to_obstacles)) or (delta_y <= -(furniture_geometry_list[index_2].l / 2.0 + dist_to_obstacles) or delta_y >= (furniture_geometry_list[index_2].l / 2.0 + dist_to_obstacles)):
00118 index_2 += 1
00119 else:
00120 break
00121 if index_2 == len(furniture_geometry_list):
00122 obstacle_checked_scan_base_pose_list.append(scan_base_pose_list[index_1])
00123 index_1 += 1
00124
00125
00126 if obstacle_checked_scan_base_pose_list:
00127
00128
00129 data = getMapClient()
00130
00131
00132 dist_to_walls = 0.5
00133 threshold = 10.0
00134 step_angle = 5.0
00135
00136
00137 index_3 = 0
00138 while index_3 < len(obstacle_checked_scan_base_pose_list):
00139 map_index_list = list()
00140 n = 0
00141 while n < int(360.0 / step_angle):
00142 wall_check_point_x = obstacle_checked_scan_base_pose_list[index_3].x + dist_to_walls * math.cos(n * step_angle / 180.0 * math.pi)
00143 wall_check_point_y = obstacle_checked_scan_base_pose_list[index_3].y + dist_to_walls * math.sin(n * step_angle / 180.0 * math.pi)
00144
00145
00146
00147 map_index = int((wall_check_point_y - data.map.info.origin.position.y) / data.map.info.resolution) * data.map.info.width + int((wall_check_point_x - data.map.info.origin.position.x) / data.map.info.resolution)
00148 map_index_list.append(map_index)
00149 n += 1
00150
00151 map_index = int((obstacle_checked_scan_base_pose_list[index_3].y - data.map.info.origin.position.y) / data.map.info.resolution) * data.map.info.width + int((obstacle_checked_scan_base_pose_list[index_3].x - data.map.info.origin.position.x) / data.map.info.resolution)
00152 map_index_list.append(map_index)
00153
00154
00155 index_4 = 0
00156 while index_4 < len(map_index_list):
00157
00158 if -1 < data.map.data[map_index_list[index_4]] < threshold:
00159 index_4 += 1
00160 else:
00161 break
00162 if index_4 == len(map_index_list):
00163 wall_checked_scan_base_pose_list.append(obstacle_checked_scan_base_pose_list[index_3])
00164 index_3 += 1
00165 '''
00166 if wall_checked_scan_base_pose_list:
00167 step_dist = 0.05
00168 map_index_list = list()
00169 threshold = 20
00170 index_5 = 0
00171 while index_5 < len(wall_checked_scan_base_pose_list):
00172 dist = math.sqrt((parent_obj_y - wall_checked_scan_base_pose_list[index_5].y) ** 2 + (parent_obj_x - wall_checked_scan_base_pose_list[index_5].x) ** 2)
00173 n = 0
00174 while n < int((dist - 0.5) / step_dist):
00175 wall_check_point_x = wall_checked_scan_base_pose_list[index_5].x - (0.5 + n * step_dist) * math.cos(wall_checked_scan_base_pose_list[index_5].theta)
00176 wall_check_point_y = wall_checked_scan_base_pose_list[index_5].y - (0.5 + n * step_dist) * math.sin(wall_checked_scan_base_pose_list[index_5].theta)
00177 print([wall_check_point_x, wall_check_point_y, wall_checked_scan_base_pose_list[index_5].theta])
00178 map_index = int((wall_check_point_y - data.map.info.origin.position.y) / data.map.info.resolution) * data.map.info.width + int((wall_check_point_x - data.map.info.origin.position.x) / data.map.info.resolution)
00179 map_index_list.append(map_index)
00180 n += 1
00181
00182 index_6 = 0
00183 while index_6 < len(map_index_list):
00184 if -1 < data.map.data[map_index_list[index_6]] < threshold:
00185 index_6 += 1
00186 else:
00187 break
00188 if index_6 == len(map_index_list):
00189 all_checked_scan_base_pose_list.append(wall_checked_scan_base_pose_list[index_5])
00190 index_5 += 1
00191 '''
00192 return wall_checked_scan_base_pose_list
00193
00194
00195
00196
00197
00198 def handle_symbol_grounding_scan_base_region(req):
00199
00200 '''
00201 #record the map for checking
00202 data = getMapClient()
00203 spamWriter = csv.writer(open('map_data.csv', 'wb'), delimiter=' ', quotechar='|', quoting=csv.QUOTE_MINIMAL)
00204 for n in range(80, 250):
00205 spamWriter.writerow(data.map.data[130 + 320 * n : 200 + 320 * n])
00206 n += 1
00207 '''
00208
00209 '''
00210 #test the map
00211 data = getMapClient()
00212 #test points
00213 x = -3.2
00214 y = -0.58
00215 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)
00216 for n in range(-20, 100):
00217 map_line = list()
00218 for m in range (-9, 9):
00219 map_line.append(data.map.data[map_index + n * int(data.map.info.width) + m])
00220 m += 1
00221 print map_line
00222 n += 1
00223 rospy.loginfo([data.map.info.origin.position.x, data.map.info.origin.position.y])
00224 rospy.loginfo([data.map.info.width, data.map.info.height])
00225 '''
00226
00227
00228
00229 '''
00230 #get furniture information from knowledge base
00231 workspace_info = getWorkspaceOnMap()
00232 furniture_geometry_list = list()
00233 furniture_geometry_list = workspace_info.objectsInfo
00234
00235
00236 #transfrom list
00237 index = 0
00238 furniture_geometry_list = list()
00239 while index < len(furniture_geometry_list):
00240 furniture_geometry = FurnitureGeometry()
00241 furniture_geometry.pose.x = furniture_geometry_list[index].pose.position.x
00242 furniture_geometry.pose.y = furniture_geometry_list[index].pose.position.y
00243 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])
00244 furniture_geometry.pose.theta = furniture_pose_rpy[2]
00245 furniture_geometry.l = furniture_geometry_list[index].l
00246 furniture_geometry.w = furniture_geometry_list[index].w
00247 furniture_geometry.h = furniture_geometry_list[index].h
00248 furniture_geometry_list.append(furniture_geometry)
00249 index += 1
00250
00251
00252
00253 '''
00254
00255
00256 parent_obj_x = req.parent_obj_geometry.pose.position.x
00257 parent_obj_y = req.parent_obj_geometry.pose.position.y
00258 parent_obj_rpy = tf.transformations.euler_from_quaternion([req.parent_obj_geometry.pose.orientation.x, req.parent_obj_geometry.pose.orientation.y, req.parent_obj_geometry.pose.orientation.z, req.parent_obj_geometry.pose.orientation.w])
00259 parent_obj_th = parent_obj_rpy[2]
00260 parent_obj_l = req.parent_obj_geometry.l
00261 parent_obj_w = req.parent_obj_geometry.w
00262 parent_obj_h = req.parent_obj_geometry.h
00263
00264
00265
00266
00267
00268
00269
00270
00271 index = 0
00272 furniture_geometry_list = list()
00273 while index < len(req.furniture_geometry_list):
00274 furniture_geometry = FurnitureGeometry()
00275 furniture_geometry.pose.x = req.furniture_geometry_list[index].pose.position.x
00276 furniture_geometry.pose.y = req.furniture_geometry_list[index].pose.position.y
00277 furniture_pose_rpy = tf.transformations.euler_from_quaternion([req.furniture_geometry_list[index].pose.orientation.x, req.furniture_geometry_list[index].pose.orientation.y, req.furniture_geometry_list[index].pose.orientation.z, req.furniture_geometry_list[index].pose.orientation.w])
00278 furniture_geometry.pose.theta = furniture_pose_rpy[2]
00279 furniture_geometry.l = req.furniture_geometry_list[index].l
00280 furniture_geometry.w = req.furniture_geometry_list[index].w
00281 furniture_geometry.h = req.furniture_geometry_list[index].h
00282 furniture_geometry_list.append(furniture_geometry)
00283 index += 1
00284
00285
00286
00287
00288 max_scan_redundancy = 0.1
00289 rb_distance = 0.7
00290 robot_h = 1.4
00291 detection_angle = (40.0 / 180.0) * math.pi
00292 camera_distance = math.sqrt((robot_h - parent_obj_h) ** 2 + (rb_distance - 0.2) ** 2)
00293 detection_w = 2 * (camera_distance * math.tan(0.5 * detection_angle)) - 2.0 * max_scan_redundancy
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303 scan_base_pose_1 = Pose2D()
00304 scan_base_pose_2 = Pose2D()
00305 scan_base_pose_3 = Pose2D()
00306 scan_base_pose_4 = Pose2D()
00307
00308
00309 scan_base_pose_list_1 = list()
00310 scan_base_pose_list_2 = list()
00311 scan_base_pose_list_3 = list()
00312 scan_base_pose_list_4 = list()
00313
00314
00315 if parent_obj_th < 0:
00316 parent_obj_th += 2.0 * math.pi
00317
00318 elif parent_obj_th > 2.0 * math.pi:
00319 parent_obj_th -= 2.0 * math.pi
00320
00321
00322
00323
00324 if ((parent_obj_th >= 0) & (parent_obj_th <= (45.0 / 180.0 * math.pi))) | ((parent_obj_th >= (135.0 / 180.0 * math.pi)) & (parent_obj_th <= (225.0 / 180.0 * math.pi))) | ((parent_obj_th >= (315.0 / 180.0 * math.pi)) & (parent_obj_th < 360)):
00325
00326
00327 step = int((parent_obj_l / detection_w) + 0.99)
00328 detection_w = parent_obj_l / step
00329 for num in range(step):
00330 scan_base_pose_1 = Pose2D()
00331 scan_base_pose_1.x = parent_obj_x - (parent_obj_w * 0.5 + rb_distance) * math.cos(parent_obj_th) - (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00332 scan_base_pose_1.y = parent_obj_y - (parent_obj_w * 0.5 + rb_distance) * math.sin(parent_obj_th) + (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00333 scan_base_pose_1.theta = parent_obj_th + math.pi
00334 if scan_base_pose_1.theta > math.pi:
00335 scan_base_pose_1.theta -= 2.0 * math.pi
00336 elif scan_base_pose_1.theta < -math.pi:
00337 scan_base_pose_1.theta += 2.0 * math.pi
00338 scan_base_pose_list_1.append(scan_base_pose_1)
00339
00340
00341
00342
00343 for num in range(int((parent_obj_l / detection_w) + 0.99)):
00344 scan_base_pose_2 = Pose2D()
00345 scan_base_pose_2.x = parent_obj_x + (parent_obj_w * 0.5 + rb_distance) * math.cos(parent_obj_th) + (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00346 scan_base_pose_2.y = parent_obj_y + (parent_obj_w * 0.5 + rb_distance) * math.sin(parent_obj_th) - (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00347 scan_base_pose_2.theta = parent_obj_th
00348 if scan_base_pose_2.theta > math.pi:
00349 scan_base_pose_2.theta -= 2.0 * math.pi
00350 elif scan_base_pose_2.theta < -math.pi:
00351 scan_base_pose_2.theta += 2.0 * math.pi
00352 scan_base_pose_list_2.append(scan_base_pose_2)
00353
00354
00355
00356 step = int((parent_obj_w / detection_w) + 0.99)
00357 detection_w = parent_obj_w / step
00358 for num in range(step):
00359 scan_base_pose_3 = Pose2D()
00360 scan_base_pose_3.x = parent_obj_x + (parent_obj_l * 0.5 + rb_distance) * math.sin(parent_obj_th) - (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00361 scan_base_pose_3.y = parent_obj_y - (parent_obj_l * 0.5 + rb_distance) * math.cos(parent_obj_th) - (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00362 scan_base_pose_3.theta = parent_obj_th - 0.5 * math.pi
00363 if scan_base_pose_3.theta > math.pi:
00364 scan_base_pose_3.theta -= 2.0 * math.pi
00365 elif scan_base_pose_3.theta < -math.pi:
00366 scan_base_pose_3.theta += 2.0 * math.pi
00367 scan_base_pose_list_3.append(scan_base_pose_3)
00368
00369
00370
00371
00372 for num in range(step):
00373 scan_base_pose_4 = Pose2D()
00374 scan_base_pose_4.x = parent_obj_x - (parent_obj_l * 0.5 + rb_distance) * math.sin(parent_obj_th) + (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00375 scan_base_pose_4.y = parent_obj_y + (parent_obj_l * 0.5 + rb_distance) * math.cos(parent_obj_th) + (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00376 scan_base_pose_4.theta = parent_obj_th + 0.5 * math.pi
00377 if scan_base_pose_4.theta > math.pi:
00378 scan_base_pose_4.theta -= 2.0 * math.pi
00379 elif scan_base_pose_4.theta < -math.pi:
00380 scan_base_pose_4.theta += 2.0 * math.pi
00381 scan_base_pose_list_4.append(scan_base_pose_4)
00382
00383
00384 else:
00385 parent_obj_th -= 0.5 * math.pi
00386 step = int((parent_obj_w / detection_w) + 0.99)
00387 detection_w = parent_obj_w / step
00388 for num in range(step):
00389 scan_base_pose_1 = Pose2D()
00390 scan_base_pose_1.x = parent_obj_x - (parent_obj_l * 0.5 + rb_distance) * math.cos(parent_obj_th) - (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00391 scan_base_pose_1.y = parent_obj_y - (parent_obj_l * 0.5 + rb_distance) * math.sin(parent_obj_th) + (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00392 scan_base_pose_1.theta = parent_obj_th + math.pi
00393 if scan_base_pose_1.theta > math.pi:
00394 scan_base_pose_1.theta -= 2.0 * math.pi
00395 elif scan_base_pose_1.theta < -math.pi:
00396 scan_base_pose_1.theta += 2.0 * math.pi
00397 scan_base_pose_list_1.append(scan_base_pose_1)
00398
00399
00400 for num in range(step):
00401 scan_base_pose_2 = Pose2D()
00402 scan_base_pose_2.x = parent_obj_x + (parent_obj_l * 0.5 + rb_distance) * math.cos(parent_obj_th) + (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00403 scan_base_pose_2.y = parent_obj_y + (parent_obj_l * 0.5 + rb_distance) * math.sin(parent_obj_th) - (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00404 scan_base_pose_2.theta = parent_obj_th
00405 if scan_base_pose_2.theta > math.pi:
00406 scan_base_pose_2.theta -= 2.0 * math.pi
00407 elif scan_base_pose_2.theta < -math.pi:
00408 scan_base_pose_2.theta += 2.0 * math.pi
00409 scan_base_pose_list_2.append(scan_base_pose_2)
00410
00411
00412
00413 step = int((parent_obj_l / detection_w) + 0.99)
00414 detection_w = parent_obj_l / step
00415 for num in range(step):
00416 scan_base_pose_3 = Pose2D()
00417 scan_base_pose_3.x = parent_obj_x + (parent_obj_w * 0.5 + rb_distance) * math.sin(parent_obj_th) - (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00418 scan_base_pose_3.y = parent_obj_y - (parent_obj_w * 0.5 + rb_distance) * math.cos(parent_obj_th) - (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00419 scan_base_pose_3.theta = parent_obj_th - 0.5 * math.pi
00420 if scan_base_pose_1.theta > math.pi:
00421 scan_base_pose_1.theta -= 2.0 * math.pi
00422 elif scan_base_pose_1.theta < -math.pi:
00423 scan_base_pose_1.theta += 2.0 * math.pi
00424 scan_base_pose_list_3.append(scan_base_pose_3)
00425
00426
00427
00428
00429 for num in range(step):
00430 scan_base_pose_4 = Pose2D()
00431 scan_base_pose_4.x = parent_obj_x - (parent_obj_w * 0.5 + rb_distance) * math.sin(parent_obj_th) + (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00432 scan_base_pose_4.y = parent_obj_y + (parent_obj_w * 0.5 + rb_distance) * math.cos(parent_obj_th) + (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00433 scan_base_pose_4.theta = parent_obj_th + 0.5 * math.pi
00434 if scan_base_pose_1.theta > math.pi:
00435 scan_base_pose_1.theta -= 2.0 * math.pi
00436 elif scan_base_pose_1.theta < -math.pi:
00437 scan_base_pose_1.theta += 2.0 * math.pi
00438 scan_base_pose_list_4.append(scan_base_pose_4)
00439
00440
00441
00442
00443
00444
00445
00446 obstacle_checked_scan_base_pose_list_1 = obstacleCheck(scan_base_pose_list_1, furniture_geometry_list, parent_obj_x, parent_obj_y)
00447 obstacle_checked_scan_base_pose_list_2 = obstacleCheck(scan_base_pose_list_2, furniture_geometry_list, parent_obj_x, parent_obj_y)
00448 obstacle_checked_scan_base_pose_list_3 = obstacleCheck(scan_base_pose_list_3, furniture_geometry_list, parent_obj_x, parent_obj_y)
00449 obstacle_checked_scan_base_pose_list_4 = obstacleCheck(scan_base_pose_list_4, furniture_geometry_list, parent_obj_x, parent_obj_y)
00450
00451
00452
00453
00454 max_len = max(len(obstacle_checked_scan_base_pose_list_1), len(obstacle_checked_scan_base_pose_list_2), len(obstacle_checked_scan_base_pose_list_3), len(obstacle_checked_scan_base_pose_list_4))
00455
00456
00457 scan_base_pose_list = list()
00458 if len(obstacle_checked_scan_base_pose_list_1) == max_len:
00459 scan_base_pose_list = obstacle_checked_scan_base_pose_list_1
00460 elif len(obstacle_checked_scan_base_pose_list_2) == max_len:
00461 scan_base_pose_list = obstacle_checked_scan_base_pose_list_2
00462 elif len(obstacle_checked_scan_base_pose_list_3) == max_len:
00463 scan_base_pose_list = obstacle_checked_scan_base_pose_list_3
00464 else:
00465 scan_base_pose_list = obstacle_checked_scan_base_pose_list_4
00466
00467
00468 if not scan_base_pose_list:
00469 print "no valid scan pose."
00470
00471
00472
00473 min_dist = 0.40
00474 R_list = list()
00475 index_1 = 0
00476 while index_1 < len(scan_base_pose_list):
00477 index_2 = 0
00478 dist_list = list()
00479 while index_2 < len(furniture_geometry_list):
00480 th = math.atan((scan_base_pose_list[index_1].y - furniture_geometry_list[index_2].pose.y) / (scan_base_pose_list[index_1].x - furniture_geometry_list[index_2].pose.x + 0.0001))
00481 if scan_base_pose_list[index_1].x < furniture_geometry_list[index_2].pose.x and scan_base_pose_list[index_1].y > furniture_geometry_list[index_2].pose.y:
00482 th = math.pi + th
00483 if scan_base_pose_list[index_1].x < furniture_geometry_list[index_2].pose.x and scan_base_pose_list[index_1].y < furniture_geometry_list[index_2].pose.y:
00484 th = -math.pi + th
00485 delta_x = math.sqrt((scan_base_pose_list[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (scan_base_pose_list[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.cos(th - furniture_geometry_list[index_2].pose.theta)
00486 delta_y = math.sqrt((scan_base_pose_list[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (scan_base_pose_list[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.sin(th - furniture_geometry_list[index_2].pose.theta)
00487 delta_x = abs(delta_x)
00488 delta_y = abs(delta_y)
00489 dist = max((delta_x - furniture_geometry_list[index_2].w / 2.0), (delta_y - furniture_geometry_list[index_2].l / 2.0))
00490 dist_list.append(dist)
00491 index_2 += 1
00492
00493 R = min(dist_list) - min_dist
00494
00495 R_list.append(R)
00496 index_1 += 1
00497 R = min(min(R_list), max_scan_redundancy)
00498
00499 return scan_base_pose_list, R
00500
00501
00502
00503 def symbol_grounding_scan_base_region_server():
00504 rospy.init_node('symbol_grounding_scan_base_region_server')
00505 s = rospy.Service('symbol_grounding_scan_base_region', SymbolGroundingScanBaseRegion, handle_symbol_grounding_scan_base_region)
00506 print "Ready to receive requests."
00507 rospy.spin()
00508
00509
00510
00511 if __name__ == "__main__":
00512 symbol_grounding_scan_base_region_server()
00513
00514