$search
00001 #!/usr/bin/env python 00002 ################################################################# 00003 ##\file 00004 # 00005 # \note 00006 # Copyright (c) 2012 \n 00007 # University of Bedfordshire \n\n 00008 # 00009 ################################################################# 00010 # 00011 # \note 00012 # Project name: care-o-bot 00013 # \note 00014 # ROS stack name: srs_public 00015 # \note 00016 # ROS package name: srs_symbolic_grounding 00017 # 00018 # \author 00019 # Author: Beisheng Liu, email:beisheng.liu@beds.ac.uk 00020 # \author 00021 # Supervised by: Dayou Li, email:dayou.li@beds.ac.uk 00022 # 00023 # \date Date of creation: Mar 2012 00024 # 00025 # \brief 00026 # Grounding explore object commands 00027 # 00028 ################################################################# 00029 # 00030 # Redistribution and use in source and binary forms, with or without 00031 # modification, are permitted provided that the following conditions are met: 00032 # 00033 # - Redistributions of source code must retain the above copyright 00034 # notice, this list of conditions and the following disclaimer. \n 00035 # - Redistributions in binary form must reproduce the above copyright 00036 # notice, this list of conditions and the following disclaimer in the 00037 # documentation and/or other materials provided with the distribution. \n 00038 # - Neither the name of the University of Bedfordshire nor the names of its 00039 # contributors may be used to endorse or promote products derived from 00040 # this software without specific prior written permission. \n 00041 # 00042 # This program is free software: you can redistribute it and/or modify 00043 # it under the terms of the GNU Lesser General Public License LGPL as 00044 # published by the Free Software Foundation, either version 3 of the 00045 # License, or (at your option) any later version. 00046 # 00047 # This program is distributed in the hope that it will be useful, 00048 # but WITHOUT ANY WARRANTY; without even the implied warranty of 00049 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00050 # GNU Lesser General Public License LGPL for more details. 00051 # 00052 # You should have received a copy of the GNU Lesser General Public 00053 # License LGPL along with this program. 00054 # If not, see <http://www.gnu.org/licenses/>. 00055 # 00056 ################################################################# 00057 import roslib; roslib.load_manifest('srs_symbolic_grounding') 00058 00059 from srs_symbolic_grounding.srv import SymbolGroundingExploreBasePose 00060 #from srs_symbolic_grounding.msg import * 00061 from std_msgs.msg import * 00062 from geometry_msgs.msg import * 00063 import rospy 00064 import math 00065 import tf 00066 from tf.transformations import euler_from_quaternion 00067 ''' 00068 def getWorkspaceOnMap(): 00069 print 'test get all workspace (furnitures basically here) from map' 00070 try: 00071 requestNewTask = rospy.ServiceProxy('get_workspace_on_map', GetWorkspaceOnMap) 00072 res = requestNewTask('ipa-kitchen-map', True) 00073 return res 00074 except rospy.ServiceException, e: 00075 print "Service call failed: %s"%e 00076 ''' 00077 00078 def handle_symbol_grounding_explore_base_pose(req): 00079 00080 parent_obj_x = req.parent_obj_geometry.pose.position.x 00081 parent_obj_y = req.parent_obj_geometry.pose.position.y 00082 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]) 00083 parent_obj_th = parent_obj_rpy[2] 00084 parent_obj_l = req.parent_obj_geometry.l 00085 parent_obj_w = req.parent_obj_geometry.w 00086 parent_obj_h = req.parent_obj_geometry.h 00087 00088 rospy.loginfo(req.parent_obj_geometry) 00089 00090 ''' 00091 #get furniture information from knowledge base 00092 workspace_info = getWorkspaceOnMap() 00093 furniture_geometry_list = list() 00094 furniture_geometry_list = workspace_info.objectsInfo 00095 00096 00097 #transfrom list 00098 index = 0 00099 furniture_geometry_list = list() 00100 while index < len(furniture_geometry_list): 00101 furniture_geometry = FurnitureGeometry() 00102 furniture_geometry.pose.x = furniture_geometry_list[index].pose.position.x 00103 furniture_geometry.pose.y = furniture_geometry_list[index].pose.position.y 00104 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]) 00105 furniture_geometry.pose.theta = furniture_pose_rpy[2] 00106 furniture_geometry.l = furniture_geometry_list[index].l 00107 furniture_geometry.w = furniture_geometry_list[index].w 00108 furniture_geometry.h = furniture_geometry_list[index].h 00109 furniture_geometry_list.append(furniture_geometry) 00110 index += 1 00111 ''' 00112 #transfrom list 00113 index = 0 00114 furniture_geometry_list = list() 00115 while index < len(req.furniture_geometry_list): 00116 furniture_geometry = FurnitureGeometry() 00117 furniture_geometry.pose.x = req.furniture_geometry_list[index].pose.position.x 00118 furniture_geometry.pose.y = req.furniture_geometry_list[index].pose.position.y 00119 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]) 00120 furniture_geometry.pose.theta = furniture_pose_rpy[2] 00121 furniture_geometry.l = req.furniture_geometry_list[index].l 00122 furniture_geometry.w = req.furniture_geometry_list[index].w 00123 furniture_geometry.h = req.furniture_geometry_list[index].h 00124 furniture_geometry_list.append(furniture_geometry) 00125 index += 1 00126 00127 00128 #get rb_distance 00129 00130 #get detection width 00131 rb_distance = 1.0 00132 robot_h = 1.4 00133 detection_angle = (50.0 / 180.0) * math.pi 00134 camera_distance = math.sqrt((robot_h - parent_obj_h) ** 2 + (rb_distance - 0.2) ** 2) 00135 detection_w = 2 * (camera_distance * math.tan(0.5 * detection_angle)) 00136 00137 00138 00139 00140 00141 00142 00143 00144 explore_base_pose_1 = Pose2D() 00145 explore_base_pose_2 = Pose2D() 00146 explore_base_pose_3 = Pose2D() 00147 explore_base_pose_4 = Pose2D() 00148 00149 00150 explore_base_pose_list_1 = list() 00151 explore_base_pose_list_2 = list() 00152 explore_base_pose_list_3 = list() 00153 explore_base_pose_list_4 = list() 00154 00155 wall_checked_explore_base_pose_list_1 = list() 00156 wall_checked_explore_base_pose_list_2 = list() 00157 wall_checked_explore_base_pose_list_3 = list() 00158 wall_checked_explore_base_pose_list_4 = list() 00159 00160 obstacle_checked_explore_base_pose_list_1 = list() 00161 obstacle_checked_explore_base_pose_list_2 = list() 00162 obstacle_checked_explore_base_pose_list_3 = list() 00163 obstacle_checked_explore_base_pose_list_4 = list() 00164 00165 00166 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)): 00167 00168 for num in range(int((parent_obj_l / detection_w) + 0.99)): 00169 00170 explore_base_pose_1 = Pose2D() 00171 explore_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) 00172 explore_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) 00173 explore_base_pose_1.theta = parent_obj_th + math.pi 00174 explore_base_pose_list_1.append(explore_base_pose_1) 00175 00176 00177 #obstacle check 1 00178 00179 00180 index = 0 00181 while index < len(explore_base_pose_list_1): 00182 if ((-2.7 <= explore_base_pose_list_1[index].x <= 1.6) and (-1.7 <= explore_base_pose_list_1[index].y <= 1.2)) or ((1.6 <= explore_base_pose_list_1[index].x <= 3.2) and (-1.7 <= explore_base_pose_list_1[index].y <= 0.7)): 00183 wall_checked_explore_base_pose_list_1.append(explore_base_pose_list_1[index]) 00184 index += 1 00185 00186 00187 else: 00188 index_1 = 0 00189 while index_1 < len(wall_checked_explore_base_pose_list_1): 00190 index_2 = 0 00191 while index_2 < len(furniture_geometry_list): 00192 delta_x = math.sqrt((wall_checked_explore_base_pose_list_1[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_1[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.cos(wall_checked_explore_base_pose_list_1[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00193 delta_y = math.sqrt((wall_checked_explore_base_pose_list_1[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_1[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.sin(wall_checked_explore_base_pose_list_1[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00194 if (delta_x <= -(furniture_geometry_list[index_2].w / 2.0 + 0.5) or delta_x >= (furniture_geometry_list[index_2].w / 2.0 + 0.5)) or (delta_y <= -(furniture_geometry_list[index_2].l / 2.0 + 0.5) or delta_y >= (furniture_geometry_list[index_2].l / 2.0 + 0.5)): 00195 index_2 += 1 00196 else: 00197 index_1 += 1 00198 break 00199 obstacle_checked_explore_base_pose_list_1.append(wall_checked_explore_base_pose_list_1[index_1]) 00200 index_1 += 1 00201 00202 00203 for num in range(int((parent_obj_l / detection_w) + 0.99)): 00204 explore_base_pose_2 = Pose2D() 00205 explore_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) 00206 explore_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) 00207 explore_base_pose_2.theta = parent_obj_th 00208 explore_base_pose_list_2.append(explore_base_pose_2) 00209 00210 00211 #obstacle check 2 00212 00213 00214 index = 0 00215 while index < len(explore_base_pose_list_2): 00216 if ((-2.7 <= explore_base_pose_list_2[index].x <= 1.6) and (-1.7 <= explore_base_pose_list_2[index].y <= 1.2)) or ((1.6 <= explore_base_pose_list_2[index].x <= 3.2) and (-1.7 <= explore_base_pose_list_2[index].y <= 0.7)): 00217 wall_checked_explore_base_pose_list_2.append(explore_base_pose_list_2[index]) 00218 index += 1 00219 00220 00221 else: 00222 index_1 = 0 00223 while index_1 < len(wall_checked_explore_base_pose_list_2): 00224 index_2 = 0 00225 while index_2 < len(furniture_geometry_list): 00226 delta_x = math.sqrt((wall_checked_explore_base_pose_list_2[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_2[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.cos(wall_checked_explore_base_pose_list_1[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00227 delta_y = math.sqrt((wall_checked_explore_base_pose_list_2[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_2[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.sin(wall_checked_explore_base_pose_list_2[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00228 if (delta_x <= -(furniture_geometry_list[index_2].w / 2.0 + 0.5) or delta_x >= (furniture_geometry_list[index_2].w / 2.0 + 0.5)) or (delta_y <= -(furniture_geometry_list[index_2].l / 2.0 + 0.5) or delta_y >= (furniture_geometry_list[index_2].l / 2.0 + 0.5)): 00229 index_2 += 1 00230 else: 00231 index_1 += 1 00232 break 00233 obstacle_checked_explore_base_pose_list_2.append(wall_checked_explore_base_pose_list_2[index_1]) 00234 index_1 += 1 00235 00236 for num in range(int((parent_obj_w / detection_w) + 0.99)): 00237 00238 explore_base_pose_3 = Pose2D() 00239 explore_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) 00240 explore_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) 00241 explore_base_pose_3.theta = parent_obj_th - 0.5 * math.pi 00242 explore_base_pose_list_3.append(explore_base_pose_3) 00243 00244 #obstacle check 3 00245 00246 00247 00248 index = 0 00249 while index < len(explore_base_pose_list_3): 00250 if ((-2.7 <= explore_base_pose_list_3[index].x <= 1.6) and (-1.7 <= explore_base_pose_list_3[index].y <= 1.2)) or ((1.6 <= explore_base_pose_list_3[index].x <= 3.2) and (-1.7 <= explore_base_pose_list_3[index].y <= 0.7)): 00251 wall_checked_explore_base_pose_list_3.append(explore_base_pose_list_3[index]) 00252 index += 1 00253 00254 00255 else: 00256 index_1 = 0 00257 while index_1 < len(wall_checked_explore_base_pose_list_3): 00258 index_2 = 0 00259 while index_2 < len(furniture_geometry_list): 00260 delta_x = math.sqrt((wall_checked_explore_base_pose_list_3[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_3[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.cos(wall_checked_explore_base_pose_list_3[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00261 delta_y = math.sqrt((wall_checked_explore_base_pose_list_3[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_3[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.sin(wall_checked_explore_base_pose_list_3[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00262 if (delta_x <= -(furniture_geometry_list[index_2].w / 2.0 + 0.5) or delta_x >= (furniture_geometry_list[index_2].w / 2.0 + 0.5)) or (delta_y <= -(furniture_geometry_list[index_2].l / 2.0 + 0.5) or delta_y >= (furniture_geometry_list[index_2].l / 2.0 + 0.5)): 00263 index_2 += 1 00264 else: 00265 index_1 += 1 00266 break 00267 obstacle_checked_explore_base_pose_list_3.append(wall_checked_explore_base_pose_list_3[index_1]) 00268 index_1 += 1 00269 00270 for num in range(int((parent_obj_w / detection_w) + 0.99)): 00271 explore_base_pose_4 = Pose2D() 00272 explore_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) 00273 explore_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) 00274 explore_base_pose_4.theta = parent_obj_th + 0.5 * math.pi 00275 explore_base_pose_list_4.append(explore_base_pose_4) 00276 00277 #obstacle check 4 00278 00279 00280 00281 index = 0 00282 while index < len(explore_base_pose_list_4): 00283 if ((-2.7 <= explore_base_pose_list_4[index].x <= 1.6) and (-1.7 <= explore_base_pose_list_4[index].y <= 1.2)) or ((1.6 <= explore_base_pose_list_4[index].x <= 3.2) and (-1.7 <= explore_base_pose_list_4[index].y <= 0.7)): 00284 wall_checked_explore_base_pose_list_4.append(explore_base_pose_list_4[index]) 00285 index += 1 00286 00287 00288 else: 00289 index_1 = 0 00290 while index_1 < len(wall_checked_explore_base_pose_list_4): 00291 index_2 = 0 00292 while index_2 < len(furniture_geometry_list): 00293 delta_x = math.sqrt((wall_checked_explore_base_pose_list_4[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_4[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.cos(wall_checked_explore_base_pose_list_1[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00294 delta_y = math.sqrt((wall_checked_explore_base_pose_list_4[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_4[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.sin(wall_checked_explore_base_pose_list_4[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00295 if (delta_x <= -(furniture_geometry_list[index_2].w / 2.0 + 0.5) or delta_x >= (furniture_geometry_list[index_2].w / 2.0 + 0.5)) or (delta_y <= -(furniture_geometry_list[index_2].l / 2.0 + 0.5) or delta_y >= (furniture_geometry_list[index_2].l / 2.0 + 0.5)): 00296 index_2 += 1 00297 else: 00298 index_1 += 1 00299 break 00300 obstacle_checked_explore_base_pose_list_4.append(wall_checked_explore_base_pose_list_4[index_1]) 00301 index_1 += 1 00302 00303 else: 00304 00305 for num in range(int((parent_obj_w / detection_w) + 0.99)): 00306 00307 explore_base_pose_1 = Pose2D() 00308 explore_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) 00309 explore_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) 00310 explore_base_pose_1.theta = parent_obj_th + math.pi 00311 explore_base_pose_list_1.append(explore_base_pose_1) 00312 00313 00314 #obstacle check 1 00315 00316 00317 index = 0 00318 while index < len(explore_base_pose_list_1): 00319 if ((-2.7 <= explore_base_pose_list_1[index].x <= 1.6) and (-1.7 <= explore_base_pose_list_1[index].y <= 1.2)) or ((1.6 <= explore_base_pose_list_1[index].x <= 3.2) and (-1.7 <= explore_base_pose_list_1[index].y <= 0.7)): 00320 wall_checked_explore_base_pose_list_1.append(explore_base_pose_list_1[index]) 00321 index += 1 00322 00323 00324 else: 00325 index_1 = 0 00326 while index_1 < len(wall_checked_explore_base_pose_list_1): 00327 index_2 = 0 00328 while index_2 < len(furniture_geometry_list): 00329 delta_x = math.sqrt((wall_checked_explore_base_pose_list_1[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_1[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.cos(wall_checked_explore_base_pose_list_1[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00330 delta_y = math.sqrt((wall_checked_explore_base_pose_list_1[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_1[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.sin(wall_checked_explore_base_pose_list_1[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00331 if (delta_x <= -(furniture_geometry_list[index_2].w / 2.0 + 0.5) or delta_x >= (furniture_geometry_list[index_2].w / 2.0 + 0.5)) or (delta_y <= -(furniture_geometry_list[index_2].l / 2.0 + 0.5) or delta_y >= (furniture_geometry_list[index_2].l / 2.0 + 0.5)): 00332 index_2 += 1 00333 else: 00334 index_1 += 1 00335 break 00336 obstacle_checked_explore_base_pose_list_1.append(wall_checked_explore_base_pose_list_1[index_1]) 00337 index_1 += 1 00338 00339 00340 for num in range(int((parent_obj_w / detection_w) + 0.99)): 00341 explore_base_pose_2 = Pose2D() 00342 explore_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) 00343 explore_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) 00344 explore_base_pose_2.theta = parent_obj_th 00345 explore_base_pose_list_2.append(explore_base_pose_2) 00346 00347 00348 #obstacle check 2 00349 00350 00351 index = 0 00352 while index < len(explore_base_pose_list_2): 00353 if ((-2.7 <= explore_base_pose_list_2[index].x <= 1.6) and (-1.7 <= explore_base_pose_list_2[index].y <= 1.2)) or ((1.6 <= explore_base_pose_list_2[index].x <= 3.2) and (-1.7 <= explore_base_pose_list_2[index].y <= 0.7)): 00354 wall_checked_explore_base_pose_list_2.append(explore_base_pose_list_2[index]) 00355 index += 1 00356 00357 00358 else: 00359 index_1 = 0 00360 while index_1 < len(wall_checked_explore_base_pose_list_2): 00361 index_2 = 0 00362 while index_2 < len(furniture_geometry_list): 00363 delta_x = math.sqrt((wall_checked_explore_base_pose_list_2[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_2[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.cos(wall_checked_explore_base_pose_list_1[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00364 delta_y = math.sqrt((wall_checked_explore_base_pose_list_2[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_2[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.sin(wall_checked_explore_base_pose_list_2[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00365 if (delta_x <= -(furniture_geometry_list[index_2].w / 2.0 + 0.5) or delta_x >= (furniture_geometry_list[index_2].w / 2.0 + 0.5)) or (delta_y <= -(furniture_geometry_list[index_2].l / 2.0 + 0.5) or delta_y >= (furniture_geometry_list[index_2].l / 2.0 + 0.5)): 00366 index_2 += 1 00367 else: 00368 index_1 += 1 00369 break 00370 obstacle_checked_explore_base_pose_list_2.append(wall_checked_explore_base_pose_list_2[index_1]) 00371 index_1 += 1 00372 00373 for num in range(int((parent_obj_l / detection_w) + 0.99)): 00374 00375 explore_base_pose_3 = Pose2D() 00376 explore_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) 00377 explore_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) 00378 explore_base_pose_3.theta = parent_obj_th - 0.5 * math.pi 00379 explore_base_pose_list_3.append(explore_base_pose_3) 00380 00381 #obstacle check 3 00382 00383 00384 00385 index = 0 00386 while index < len(explore_base_pose_list_3): 00387 if ((-2.7 <= explore_base_pose_list_3[index].x <= 1.6) and (-1.7 <= explore_base_pose_list_3[index].y <= 1.2)) or ((1.6 <= explore_base_pose_list_3[index].x <= 3.2) and (-1.7 <= explore_base_pose_list_3[index].y <= 0.7)): 00388 wall_checked_explore_base_pose_list_3.append(explore_base_pose_list_3[index]) 00389 index += 1 00390 00391 00392 else: 00393 index_1 = 0 00394 while index_1 < len(wall_checked_explore_base_pose_list_3): 00395 index_2 = 0 00396 while index_2 < len(furniture_geometry_list): 00397 delta_x = math.sqrt((wall_checked_explore_base_pose_list_3[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_3[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.cos(wall_checked_explore_base_pose_list_3[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00398 delta_y = math.sqrt((wall_checked_explore_base_pose_list_3[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_3[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.sin(wall_checked_explore_base_pose_list_3[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00399 if (delta_x <= -(furniture_geometry_list[index_2].w / 2.0 + 0.5) or delta_x >= (furniture_geometry_list[index_2].w / 2.0 + 0.5)) or (delta_y <= -(furniture_geometry_list[index_2].l / 2.0 + 0.5) or delta_y >= (furniture_geometry_list[index_2].l / 2.0 + 0.5)): 00400 index_2 += 1 00401 else: 00402 index_1 += 1 00403 break 00404 obstacle_checked_explore_base_pose_list_3.append(wall_checked_explore_base_pose_list_3[index_1]) 00405 index_1 += 1 00406 00407 for num in range(int((parent_obj_l / detection_w) + 0.99)): 00408 explore_base_pose_4 = Pose2D() 00409 explore_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) 00410 explore_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) 00411 explore_base_pose_4.theta = parent_obj_th + 0.5 * math.pi 00412 explore_base_pose_list_4.append(explore_base_pose_4) 00413 00414 #obstacle check 4 00415 00416 00417 00418 index = 0 00419 while index < len(explore_base_pose_list_4): 00420 if ((-2.7 <= explore_base_pose_list_4[index].x <= 1.6) and (-1.7 <= explore_base_pose_list_4[index].y <= 1.2)) or ((1.6 <= explore_base_pose_list_4[index].x <= 3.2) and (-1.7 <= explore_base_pose_list_4[index].y <= 0.7)): 00421 wall_checked_explore_base_pose_list_4.append(explore_base_pose_list_4[index]) 00422 index += 1 00423 00424 00425 else: 00426 index_1 = 0 00427 while index_1 < len(wall_checked_explore_base_pose_list_4): 00428 index_2 = 0 00429 while index_2 < len(furniture_geometry_list): 00430 delta_x = math.sqrt((wall_checked_explore_base_pose_list_4[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_4[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.cos(wall_checked_explore_base_pose_list_1[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00431 delta_y = math.sqrt((wall_checked_explore_base_pose_list_4[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (wall_checked_explore_base_pose_list_4[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.sin(wall_checked_explore_base_pose_list_4[index_1].theta - furniture_geometry_list[index_2].pose.theta) 00432 if (delta_x <= -(furniture_geometry_list[index_2].w / 2.0 + 0.5) or delta_x >= (furniture_geometry_list[index_2].w / 2.0 + 0.5)) or (delta_y <= -(furniture_geometry_list[index_2].l / 2.0 + 0.5) or delta_y >= (furniture_geometry_list[index_2].l / 2.0 + 0.5)): 00433 index_2 += 1 00434 else: 00435 index_1 += 1 00436 break 00437 obstacle_checked_explore_base_pose_list_4.append(wall_checked_explore_base_pose_list_4[index_1]) 00438 index_1 += 1 00439 00440 rospy.loginfo([obstacle_checked_explore_base_pose_list_1, obstacle_checked_explore_base_pose_list_2, obstacle_checked_explore_base_pose_list_3, obstacle_checked_explore_base_pose_list_4]) 00441 max_len = max(len(obstacle_checked_explore_base_pose_list_1), len(obstacle_checked_explore_base_pose_list_2), len(obstacle_checked_explore_base_pose_list_3), len(obstacle_checked_explore_base_pose_list_4)) 00442 00443 00444 if len(obstacle_checked_explore_base_pose_list_1) == max_len: 00445 explore_base_pose_list = [obstacle_checked_explore_base_pose_list_1] 00446 elif len(obstacle_checked_explore_base_pose_list_2) == max_len: 00447 explore_base_pose_list = [obstacle_checked_explore_base_pose_list_2] 00448 elif len(obstacle_checked_explore_base_pose_list_3) == max_len: 00449 explore_base_pose_list = [obstacle_checked_explore_base_pose_list_3] 00450 else: 00451 explore_base_pose_list = [obstacle_checked_explore_base_pose_list_4] 00452 00453 00454 if not explore_base_pose_list: 00455 print "no valid explore pose." 00456 00457 else: 00458 00459 return explore_base_pose_list 00460 00461 00462 00463 def symbol_grounding_explore_base_pose_server(): 00464 rospy.init_node('symbol_grounding_explore_base_pose_server') 00465 s = rospy.Service('symbol_grounding_explore_base_pose', SymbolGroundingExploreBasePose, handle_symbol_grounding_explore_base_pose) 00466 print "Ready to receive requests." 00467 rospy.spin() 00468 00469 00470 00471 if __name__ == "__main__": 00472 symbol_grounding_explore_base_pose_server() 00473 00474