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 import csv
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.5
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))
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_pose(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 rb_distance = 1.0
00289 robot_h = 1.4
00290 detection_angle = (30.0 / 180.0) * math.pi
00291 camera_distance = math.sqrt((robot_h - parent_obj_h) ** 2 + (rb_distance - 0.2) ** 2)
00292 detection_w = 2 * (camera_distance * math.tan(0.5 * detection_angle))
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302 scan_base_pose_1 = Pose2D()
00303 scan_base_pose_2 = Pose2D()
00304 scan_base_pose_3 = Pose2D()
00305 scan_base_pose_4 = Pose2D()
00306
00307
00308 scan_base_pose_list_1 = list()
00309 scan_base_pose_list_2 = list()
00310 scan_base_pose_list_3 = list()
00311 scan_base_pose_list_4 = list()
00312
00313
00314 if parent_obj_th < 0:
00315 parent_obj_th += 2.0 * math.pi
00316
00317 elif parent_obj_th > 2.0 * math.pi:
00318 parent_obj_th -= 2.0 * math.pi
00319
00320
00321
00322
00323 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)):
00324
00325
00326 step = int((parent_obj_l / detection_w) + 0.99)
00327 detection_w = parent_obj_l / step
00328 for num in range(step):
00329 scan_base_pose_1 = Pose2D()
00330 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)
00331 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)
00332 scan_base_pose_1.theta = parent_obj_th + math.pi
00333 if scan_base_pose_1.theta > math.pi:
00334 scan_base_pose_1.theta -= 2.0 * math.pi
00335 elif scan_base_pose_1.theta < -math.pi:
00336 scan_base_pose_1.theta += 2.0 * math.pi
00337 scan_base_pose_list_1.append(scan_base_pose_1)
00338
00339
00340
00341
00342 for num in range(int((parent_obj_l / detection_w) + 0.99)):
00343 scan_base_pose_2 = Pose2D()
00344 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)
00345 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)
00346 scan_base_pose_2.theta = parent_obj_th
00347 if scan_base_pose_2.theta > math.pi:
00348 scan_base_pose_2.theta -= 2.0 * math.pi
00349 elif scan_base_pose_2.theta < -math.pi:
00350 scan_base_pose_2.theta += 2.0 * math.pi
00351 scan_base_pose_list_2.append(scan_base_pose_2)
00352
00353
00354
00355 step = int((parent_obj_w / detection_w) + 0.99)
00356 detection_w = parent_obj_w / step
00357 for num in range(step):
00358 scan_base_pose_3 = Pose2D()
00359 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)
00360 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)
00361 scan_base_pose_3.theta = parent_obj_th - 0.5 * math.pi
00362 if scan_base_pose_3.theta > math.pi:
00363 scan_base_pose_3.theta -= 2.0 * math.pi
00364 elif scan_base_pose_3.theta < -math.pi:
00365 scan_base_pose_3.theta += 2.0 * math.pi
00366 scan_base_pose_list_3.append(scan_base_pose_3)
00367
00368
00369
00370
00371 for num in range(step):
00372 scan_base_pose_4 = Pose2D()
00373 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)
00374 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)
00375 scan_base_pose_4.theta = parent_obj_th + 0.5 * math.pi
00376 if scan_base_pose_4.theta > math.pi:
00377 scan_base_pose_4.theta -= 2.0 * math.pi
00378 elif scan_base_pose_4.theta < -math.pi:
00379 scan_base_pose_4.theta += 2.0 * math.pi
00380 scan_base_pose_list_4.append(scan_base_pose_4)
00381
00382
00383 else:
00384 parent_obj_th -= 0.5 * math.pi
00385 step = int((parent_obj_w / detection_w) + 0.99)
00386 detection_w = parent_obj_w / step
00387 for num in range(step):
00388 scan_base_pose_1 = Pose2D()
00389 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)
00390 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)
00391 scan_base_pose_1.theta = parent_obj_th + math.pi
00392 if scan_base_pose_1.theta > math.pi:
00393 scan_base_pose_1.theta -= 2.0 * math.pi
00394 elif scan_base_pose_1.theta < -math.pi:
00395 scan_base_pose_1.theta += 2.0 * math.pi
00396 scan_base_pose_list_1.append(scan_base_pose_1)
00397
00398
00399 for num in range(step):
00400 scan_base_pose_2 = Pose2D()
00401 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)
00402 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)
00403 scan_base_pose_2.theta = parent_obj_th
00404 if scan_base_pose_2.theta > math.pi:
00405 scan_base_pose_2.theta -= 2.0 * math.pi
00406 elif scan_base_pose_2.theta < -math.pi:
00407 scan_base_pose_2.theta += 2.0 * math.pi
00408 scan_base_pose_list_2.append(scan_base_pose_2)
00409
00410
00411
00412 step = int((parent_obj_l / detection_w) + 0.99)
00413 detection_w = parent_obj_l / step
00414 for num in range(step):
00415 scan_base_pose_3 = Pose2D()
00416 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)
00417 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)
00418 scan_base_pose_3.theta = parent_obj_th - 0.5 * math.pi
00419 if scan_base_pose_1.theta > math.pi:
00420 scan_base_pose_1.theta -= 2.0 * math.pi
00421 elif scan_base_pose_1.theta < -math.pi:
00422 scan_base_pose_1.theta += 2.0 * math.pi
00423 scan_base_pose_list_3.append(scan_base_pose_3)
00424
00425
00426
00427
00428 for num in range(step):
00429 scan_base_pose_4 = Pose2D()
00430 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)
00431 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)
00432 scan_base_pose_4.theta = parent_obj_th + 0.5 * math.pi
00433 if scan_base_pose_1.theta > math.pi:
00434 scan_base_pose_1.theta -= 2.0 * math.pi
00435 elif scan_base_pose_1.theta < -math.pi:
00436 scan_base_pose_1.theta += 2.0 * math.pi
00437 scan_base_pose_list_4.append(scan_base_pose_4)
00438
00439
00440
00441
00442
00443
00444
00445 obstacle_checked_scan_base_pose_list_1 = obstacleCheck(scan_base_pose_list_1, furniture_geometry_list, parent_obj_x, parent_obj_y)
00446 obstacle_checked_scan_base_pose_list_2 = obstacleCheck(scan_base_pose_list_2, furniture_geometry_list, parent_obj_x, parent_obj_y)
00447 obstacle_checked_scan_base_pose_list_3 = obstacleCheck(scan_base_pose_list_3, furniture_geometry_list, parent_obj_x, parent_obj_y)
00448 obstacle_checked_scan_base_pose_list_4 = obstacleCheck(scan_base_pose_list_4, furniture_geometry_list, parent_obj_x, parent_obj_y)
00449
00450
00451
00452
00453 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))
00454
00455
00456 if len(obstacle_checked_scan_base_pose_list_1) == max_len:
00457 scan_base_pose_list = [obstacle_checked_scan_base_pose_list_1]
00458 elif len(obstacle_checked_scan_base_pose_list_2) == max_len:
00459 scan_base_pose_list = [obstacle_checked_scan_base_pose_list_2]
00460 elif len(obstacle_checked_scan_base_pose_list_3) == max_len:
00461 scan_base_pose_list = [obstacle_checked_scan_base_pose_list_3]
00462 else:
00463 scan_base_pose_list = [obstacle_checked_scan_base_pose_list_4]
00464
00465
00466 if not scan_base_pose_list:
00467 print "no valid scan pose."
00468
00469
00470 return scan_base_pose_list
00471
00472
00473
00474 def symbol_grounding_scan_base_pose_server():
00475 rospy.init_node('symbol_grounding_scan_base_pose_server')
00476 s = rospy.Service('symbol_grounding_scan_base_pose', SymbolGroundingScanBasePose, handle_symbol_grounding_scan_base_pose)
00477 print "Ready to receive requests."
00478 rospy.spin()
00479
00480
00481
00482 if __name__ == "__main__":
00483 symbol_grounding_scan_base_pose_server()
00484
00485