$search
00001 #!/usr/bin/env python 00002 00003 ################################################################# 00004 ##\file 00005 # 00006 # \note 00007 # Copyright (c) 2012 \n 00008 # University of Bedfordshire \n\n 00009 # 00010 ################################################################# 00011 # 00012 # \note 00013 # Project name: care-o-bot 00014 # \note 00015 # ROS stack name: srs_public 00016 # \note 00017 # ROS package name: srs_symbolic_grounding 00018 # 00019 # \author 00020 # Author: Beisheng Liu, email:beisheng.liu@beds.ac.uk 00021 # \author 00022 # Supervised by: Dayou Li, email:dayou.li@beds.ac.uk 00023 # 00024 # \date Date of creation: April 2012 00025 # 00026 # \brief 00027 # Grounding grasp object commands 00028 # 00029 ################################################################# 00030 # 00031 # Redistribution and use in source and binary forms, with or without 00032 # modification, are permitted provided that the following conditions are met: 00033 # 00034 # - Redistributions of source code must retain the above copyright 00035 # notice, this list of conditions and the following disclaimer. \n 00036 # - Redistributions in binary form must reproduce the above copyright 00037 # notice, this list of conditions and the following disclaimer in the 00038 # documentation and/or other materials provided with the distribution. \n 00039 # - Neither the name of the University of Bedfordshire nor the names of its 00040 # contributors may be used to endorse or promote products derived from 00041 # this software without specific prior written permission. \n 00042 # 00043 # This program is free software: you can redistribute it and/or modify 00044 # it under the terms of the GNU Lesser General Public License LGPL as 00045 # published by the Free Software Foundation, either version 3 of the 00046 # License, or (at your option) any later version. 00047 # 00048 # This program is distributed in the hope that it will be useful, 00049 # but WITHOUT ANY WARRANTY; without even the implied warranty of 00050 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00051 # GNU Lesser General Public License LGPL for more details. 00052 # 00053 # You should have received a copy of the GNU Lesser General Public 00054 # License LGPL along with this program. 00055 # If not, see <http://www.gnu.org/licenses/>. 00056 # 00057 ################################################################# 00058 00059 00060 import roslib; roslib.load_manifest('srs_symbolic_grounding') 00061 00062 from srs_symbolic_grounding.srv import * 00063 #from srs_symbolic_grounding.msg import * 00064 from std_msgs.msg import * 00065 from geometry_msgs.msg import * 00066 from nav_msgs.msg import * 00067 from nav_msgs.srv import * 00068 #from srs_knowledge.msg import * 00069 import rospy 00070 import math 00071 import tf 00072 from tf.transformations import euler_from_quaternion 00073 #import csv 00074 00075 00076 00077 ''' 00078 #get furniture list from the knowledge base (part 1) 00079 def getWorkspaceOnMap(): 00080 print 'test get all workspace (furnitures basically here) from map' 00081 try: 00082 requestNewTask = rospy.ServiceProxy('get_workspace_on_map', GetWorkspaceOnMap) 00083 res = requestNewTask('ipa-kitchen-map', True) 00084 return res 00085 except rospy.ServiceException, e: 00086 print "Service call failed: %s"%e 00087 ''' 00088 00089 #get the occupancy grid map from the navigation services 00090 def getMapClient(): 00091 00092 try: 00093 reqMap = rospy.ServiceProxy('static_map', GetMap) 00094 res = reqMap() 00095 return res 00096 except rospy.ServiceException, e: 00097 print "Service call failed: %s"%e 00098 00099 #this function is used to calculate a list of canidate poses around the target obj. thre number of candidate poses is 360/step_angle 00100 def getRobotBasePoseList(angle, dist, rbp, obj_x, obj_y): 00101 grasp_base_pose_list = list() 00102 step_angle = angle 00103 dist_to_obj = dist 00104 rb_pose = rbp 00105 target_obj_x = obj_x 00106 target_obj_y = obj_y 00107 th = math.atan((rb_pose.y - target_obj_y) / (rb_pose.x - target_obj_x)) 00108 if rb_pose.x < target_obj_x and rb_pose.y > target_obj_y: 00109 th = math.pi + th 00110 if rb_pose.x < target_obj_x and rb_pose.y < target_obj_y: 00111 th = -math.pi + th 00112 #rospy.loginfo(th) 00113 for n in range (0, int(2 * math.pi / step_angle)): 00114 grasp_base_pose = Pose2D() 00115 grasp_base_pose.x = target_obj_x + dist_to_obj * math.cos(th) - 0.1 * math.sin(th) - 2 * math.sqrt(dist_to_obj ** 2 + 0.1 ** 2) * math.sin(0.5 * n * step_angle) * math.sin(0.5 * n * step_angle + math.atan(0.1 / dist_to_obj) + th) 00116 grasp_base_pose.y = target_obj_y + dist_to_obj * math.sin(th) + 0.1 * math.cos(th) + 2 * math.sqrt(dist_to_obj ** 2 + 0.1 ** 2) * math.sin(0.5 * n * step_angle) * math.cos(0.5 * n * step_angle + math.atan(0.1 / dist_to_obj) + th) 00117 grasp_base_pose.theta = th + n * step_angle 00118 #rospy.loginfo([th, n * step_angle, math.atan(0.1 / dist_to_obj)]) 00119 if grasp_base_pose.theta > math.pi: 00120 grasp_base_pose.theta -= 2 * math.pi 00121 grasp_base_pose_list.append(grasp_base_pose) 00122 #rospy.loginfo(grasp_base_pose_list) 00123 return grasp_base_pose_list 00124 00125 00126 00127 #function for checking if a pose is obstacle free. 00128 def obstacleCheck(gbpl, po_x, po_y, po_th, po_w, po_l, fgl): 00129 parent_obj_checked_grasp_base_pose_list = list() 00130 obstacle_checked_grasp_base_pose_list = list() 00131 wall_checked_grasp_base_pose_list = list() 00132 grasp_base_pose_list = gbpl 00133 parent_obj_x = po_x 00134 parent_obj_y = po_y 00135 parent_obj_th = po_th 00136 parent_obj_w = po_w 00137 parent_obj_l = po_l 00138 furniture_geometry_list = fgl 00139 00140 #to check if a pose is too close to the edge of the parent obj. the valid poses will be stored in the parent_obj_checked_grasp_base_pose_list. 00141 dist_to_table = 0.45 #minimum distance to the edge of the parent obj 00142 index_1 = 0 00143 while index_1 < len(grasp_base_pose_list): 00144 th = math.atan((grasp_base_pose_list[index_1].y - parent_obj_y) / (grasp_base_pose_list[index_1].x - parent_obj_x)) 00145 if grasp_base_pose_list[index_1].x < parent_obj_x and grasp_base_pose_list[index_1].y > parent_obj_y: 00146 th = math.pi + th 00147 if grasp_base_pose_list[index_1].x < parent_obj_x and grasp_base_pose_list[index_1].y < parent_obj_y: 00148 th = -math.pi + th 00149 delta_x = math.sqrt((grasp_base_pose_list[index_1].x - parent_obj_x) ** 2 + (grasp_base_pose_list[index_1].y - parent_obj_y) ** 2) * math.cos(th - parent_obj_th) 00150 delta_y = math.sqrt((grasp_base_pose_list[index_1].x - parent_obj_x) ** 2 + (grasp_base_pose_list[index_1].y - parent_obj_y) ** 2) * math.sin(th - parent_obj_th) 00151 #rospy.loginfo([delta_x, delta_y]) 00152 if (delta_x <= -(parent_obj_w / 2.0 + dist_to_table) or delta_x >= (parent_obj_w / 2.0 + dist_to_table)) or (delta_y <= -(parent_obj_l / 2.0 + dist_to_table) or delta_y >= (parent_obj_l / 2.0 + dist_to_table)): 00153 parent_obj_checked_grasp_base_pose_list.append(grasp_base_pose_list[index_1]) 00154 index_1 += 1 00155 #rospy.loginfo(parent_obj_checked_grasp_base_pose_list) 00156 00157 00158 if parent_obj_checked_grasp_base_pose_list: #if there is a parent obj free pose 00159 00160 #to check if a pose is too close to or blocked by the furnitures in the room 00161 dist_to_obstacles = 0.45 #minimum disatnce to the furnitures in the room 00162 index_2 = 0 00163 while index_2 < len(parent_obj_checked_grasp_base_pose_list): 00164 index_3 = 0 00165 while index_3 < len(furniture_geometry_list): 00166 th = math.atan((parent_obj_checked_grasp_base_pose_list[index_2].y - furniture_geometry_list[index_3].pose.y) / (parent_obj_checked_grasp_base_pose_list[index_2].x - furniture_geometry_list[index_3].pose.x)) 00167 if parent_obj_checked_grasp_base_pose_list[index_2].x < furniture_geometry_list[index_3].pose.x and parent_obj_checked_grasp_base_pose_list[index_2].y > furniture_geometry_list[index_3].pose.y: 00168 th = math.pi + th 00169 if parent_obj_checked_grasp_base_pose_list[index_2].x < furniture_geometry_list[index_3].pose.x and parent_obj_checked_grasp_base_pose_list[index_2].y < furniture_geometry_list[index_3].pose.y: 00170 th = -math.pi + th 00171 delta_x = math.sqrt((parent_obj_checked_grasp_base_pose_list[index_2].x - furniture_geometry_list[index_3].pose.x) ** 2 + (parent_obj_checked_grasp_base_pose_list[index_2].y - furniture_geometry_list[index_3].pose.y) ** 2) * math.cos(th - furniture_geometry_list[index_3].pose.theta) 00172 delta_y = math.sqrt((parent_obj_checked_grasp_base_pose_list[index_2].x - furniture_geometry_list[index_3].pose.x) ** 2 + (parent_obj_checked_grasp_base_pose_list[index_2].y - furniture_geometry_list[index_3].pose.y) ** 2) * math.sin(th - furniture_geometry_list[index_3].pose.theta) 00173 if (delta_x <= -(furniture_geometry_list[index_3].w / 2.0 + dist_to_obstacles) or delta_x >= (furniture_geometry_list[index_3].w / 2.0 + dist_to_obstacles)) or (delta_y <= -(furniture_geometry_list[index_3].l / 2.0 + dist_to_obstacles) or delta_y >= (furniture_geometry_list[index_3].l / 2.0 + dist_to_obstacles)): 00174 index_3 += 1 00175 else: 00176 break 00177 if index_3 == len(furniture_geometry_list): 00178 obstacle_checked_grasp_base_pose_list.append(parent_obj_checked_grasp_base_pose_list[index_2]) 00179 index_2 += 1 00180 #rospy.loginfo(obstacle_checked_grasp_base_pose_list) 00181 00182 if obstacle_checked_grasp_base_pose_list: 00183 00184 #to check if a pose is too near to or blocked by the wall 00185 data = getMapClient() #get occupancy grid map 00186 #rospy.loginfo(data.map.info) 00187 dist_to_walls = 0.5 #set the minimum distance to the walls 00188 threshold = 20 #>20:occupied 00189 step_angle = 5.0 #(360 / step_angle) points will be put around the robot for the wall check 00190 00191 index_4 = 0 00192 while index_4 < len(obstacle_checked_grasp_base_pose_list): 00193 map_index_list = list() 00194 n = 0 00195 while n < int(360.0 / step_angle): 00196 wall_check_point_x = obstacle_checked_grasp_base_pose_list[index_4].x + dist_to_walls * math.cos(n * step_angle / 180.0 * math.pi) 00197 wall_check_point_y = obstacle_checked_grasp_base_pose_list[index_4].y + dist_to_walls * math.sin(n * step_angle / 180.0 * math.pi) 00198 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) 00199 map_index_list.append(map_index) 00200 n += 1 00201 00202 map_index = int((obstacle_checked_grasp_base_pose_list[index_4].y - data.map.info.origin.position.y) / data.map.info.resolution) * data.map.info.width + int((obstacle_checked_grasp_base_pose_list[index_4].x - data.map.info.origin.position.x) / data.map.info.resolution) 00203 map_index_list.append(map_index) 00204 #rospy.loginfo(map_index_list) 00205 00206 index_5 = 0 00207 while index_5 < len(map_index_list): 00208 if -1 < data.map.data[map_index_list[index_5]] < threshold: 00209 index_5 += 1 00210 else: 00211 break 00212 if index_5 == len(map_index_list): 00213 wall_checked_grasp_base_pose_list.append(obstacle_checked_grasp_base_pose_list[index_4]) 00214 index_4 += 1 00215 00216 return wall_checked_grasp_base_pose_list 00217 #rospy.loginfo(wall_checked_grasp_base_pose_list) 00218 00219 00220 def handle_symbol_grounding_grasp_base_region(req): 00221 00222 ''' 00223 #get the robot's current pose from tf 00224 rb_pose = Pose2D() 00225 listener = tf.TransformListener() 00226 listener.waitForTransform("/map", "/base_link", rospy.Time(0), rospy.Duration(4.0)) #wait for 4secs for the coordinate to be transformed 00227 (trans,rot) = listener.lookupTransform("/map", "/base_link", rospy.Time(0)) 00228 rb_pose.x = trans[0] 00229 rb_pose.y = trans[1] 00230 rb_pose_rpy = tf.transformations.euler_from_quaternion(rot) 00231 rb_pose.theta = rb_pose_rpy[2] 00232 rospy.sleep(0.5) 00233 #rospy.loginfo(rb_pose) 00234 ''' 00235 rb_pose = Pose2D() 00236 rb_pose.x = -1.06 00237 rb_pose.y = -1.08 00238 rb_pose.theta = 0.0 00239 00240 #predefined membership function 00241 mf1_x = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.75, 0.5, 0.25, 0] 00242 mf1_y = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.875, 0.75, 0.625, 0.5, 0.375, 0.25, 0.125, 0] 00243 mf1_th = [0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0] 00244 00245 ''' 00246 mf2_x = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.75, 0.5, 0.25, 0] 00247 mf2_y = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.84, 0.67, 0.49, 0.33, 0] 00248 mf2_th = [0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0] 00249 00250 mf3_x = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.75, 0.5, 0.25, 0] 00251 mf3_y = [0, 0.16, 0.33, 0.49, 0.67, 0.84, 1, 0.84, 0.67, 0.49, 0.33, 0] 00252 mf3_th = [0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0] 00253 ''' 00254 00255 00256 #transfrom the parent obj data from database 00257 target_obj_x = req.target_obj_pose.position.x 00258 target_obj_y = req.target_obj_pose.position.y 00259 target_obj_rpy = tf.transformations.euler_from_quaternion([req.target_obj_pose.orientation.x, req.target_obj_pose.orientation.y, req.target_obj_pose.orientation.z, req.target_obj_pose.orientation.w]) 00260 target_obj_th = target_obj_rpy[2] 00261 #rospy.loginfo(target_obj_th) 00262 00263 robot_base_pose_x = rb_pose.x 00264 robot_base_pose_y = rb_pose.y 00265 robot_base_pose_th = rb_pose.theta 00266 00267 parent_obj_x = req.parent_obj_geometry.pose.position.x 00268 parent_obj_y = req.parent_obj_geometry.pose.position.y 00269 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]) 00270 parent_obj_th = parent_obj_rpy[2] 00271 parent_obj_l = req.parent_obj_geometry.l 00272 parent_obj_w = req.parent_obj_geometry.w 00273 parent_obj_h = req.parent_obj_geometry.h 00274 00275 00276 ''' 00277 #get furniture list from the knowledge base (part 2) 00278 00279 workspace_info = getWorkspaceOnMap() 00280 furniture_geometry_list = list() 00281 furniture_geometry_list = workspace_info.objectsInfo 00282 00283 00284 #transfrom list 00285 index = 0 00286 furniture_geometry_list = list() 00287 while index < len(furniture_geometry_list): 00288 furniture_geometry = FurnitureGeometry() 00289 furniture_geometry.pose.x = furniture_geometry_list[index].pose.position.x 00290 furniture_geometry.pose.y = furniture_geometry_list[index].pose.position.y 00291 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]) 00292 furniture_geometry.pose.theta = furniture_pose_rpy[2] 00293 furniture_geometry.l = furniture_geometry_list[index].l 00294 furniture_geometry.w = furniture_geometry_list[index].w 00295 furniture_geometry.h = furniture_geometry_list[index].h 00296 furniture_geometry_list.append(furniture_geometry) 00297 index += 1 00298 ''' 00299 #transfrom furniture geometry data from database 00300 index = 0 00301 furniture_geometry_list = list() 00302 while index < len(req.furniture_geometry_list): 00303 furniture_geometry = FurnitureGeometry() 00304 furniture_geometry.pose.x = req.furniture_geometry_list[index].pose.position.x 00305 furniture_geometry.pose.y = req.furniture_geometry_list[index].pose.position.y 00306 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]) 00307 furniture_geometry.pose.theta = furniture_pose_rpy[2] 00308 furniture_geometry.l = req.furniture_geometry_list[index].l 00309 furniture_geometry.w = req.furniture_geometry_list[index].w 00310 furniture_geometry.h = req.furniture_geometry_list[index].h 00311 furniture_geometry_list.append(furniture_geometry) 00312 index += 1 00313 00314 00315 00316 00317 #calculate the reachability 00318 00319 #calculate the best grippier pose from the robot's current base pose 00320 best_gripper_pose_x = robot_base_pose_x - 0.8 * math.cos(robot_base_pose_th) + 0.1 * math.sin(robot_base_pose_th) 00321 best_gripper_pose_y = robot_base_pose_y - 0.8 * math.sin(robot_base_pose_th) - 0.1 * math.cos(robot_base_pose_th) 00322 00323 #calculate the difference between the best gripper pose and the target obj pose 00324 th = math.atan(target_obj_y - best_gripper_pose_y / target_obj_x - best_gripper_pose_x) 00325 if target_obj_x < best_gripper_pose_x and target_obj_y > best_gripper_pose_y: 00326 th += math.pi 00327 elif target_obj_x < best_gripper_pose_x and target_obj_y < best_gripper_pose_y: 00328 th -= math.pi 00329 delta_x = math.sqrt((target_obj_x - best_gripper_pose_x) ** 2 + (target_obj_y - best_gripper_pose_y) ** 2) * math.cos(th - robot_base_pose_th) 00330 delta_y = math.sqrt((target_obj_x - best_gripper_pose_x) ** 2 + (target_obj_y - best_gripper_pose_y) ** 2) * math.sin(th - robot_base_pose_th) 00331 th = math.atan((rb_pose.y - target_obj_y) / (rb_pose.x - target_obj_x)) 00332 if rb_pose.x < target_obj_x and rb_pose.y > target_obj_y: 00333 th += math.pi 00334 elif rb_pose.x < target_obj_x and rb_pose.y < target_obj_y: 00335 th -= math.pi 00336 delta_th = robot_base_pose_th - th 00337 if delta_th < 0: 00338 delta_th = -1.0 * delta_th 00339 00340 #if target obj pose is out side hdz, reach = 0 00341 if delta_x > 0.1 or delta_x < -0.15: 00342 reach = 0 00343 elif delta_y > 0.15 or delta_y < -0.1: 00344 reach = 0 00345 elif delta_th < -10.0 / 180.0 * math.pi or delta_th > 10.0 / 180 * math.pi: 00346 reach = 0 00347 else: 00348 #calculate membership value according to the membership functions 00349 index_x = int(round(delta_x / 0.025 + 6)) 00350 index_y = int(round(delta_y / 0.025 + 6)) 00351 index_th = int(round(delta_th / (1.0 / 180.0 * math.pi) + 10)) 00352 member_x = mf1_x[index_x] 00353 member_y = mf1_y[index_y] 00354 member_th = mf1_th[index_th] 00355 #Apply the fuzzy rule. 00356 reach = min(member_x, member_y, member_th) 00357 #rospy.loginfo(reach) 00358 00359 00360 00361 #calculate two lists of candidate base poses for grasping 00362 #the first candidate pose list 00363 step_angle_1 = 5.0 / 180.0 * math.pi #360 / step_angle of candidate poses will be put around the target obj 00364 dist_1 = 0.8 #distance to the target obj 00365 grasp_base_pose_list_1 = list() 00366 grasp_base_pose_list_1 = getRobotBasePoseList(step_angle_1, dist_1, rb_pose, target_obj_x, target_obj_y) 00367 #rospy.loginfo(grasp_base_pose_list) 00368 00369 #calculate another list of candidate grasping poses with lower reachability 00370 step_angle_2 = 5.0 / 180.0 * math.pi 00371 dist_2 = 0.9 00372 grasp_base_pose_list_2 = list() 00373 grasp_base_pose_list_2 = getRobotBasePoseList(step_angle_2, dist_2, rb_pose, target_obj_x, target_obj_y) 00374 #rospy.loginfo(grasp_base_pose_list) 00375 00376 00377 #optimisation 00378 obstacle_check = 1 00379 #obstacle check 00380 grasp_base_pose_list = obstacleCheck(grasp_base_pose_list_1, parent_obj_x, parent_obj_y, parent_obj_th, parent_obj_w, parent_obj_l, furniture_geometry_list) 00381 00382 grasp_base_pose = Pose2D() 00383 #rospy.loginfo(grasp_base_pose_list) 00384 if grasp_base_pose_list: 00385 obstacle_check = 0 00386 grasp_base_pose = grasp_base_pose_list[0] 00387 00388 else: 00389 #obstacle check for the grasping poses with lower reachability when no valid pose can be found from the first candidate pose list 00390 grasp_base_pose_list = obstacleCheck(grasp_base_pose_list_2, parent_obj_x, parent_obj_y, parent_obj_th, parent_obj_w, parent_obj_l, furniture_geometry_list) 00391 00392 #rospy.loginfo(grasp_base_pose_list) 00393 #check if there is valid pose 00394 if grasp_base_pose_list: 00395 00396 obstacle_check = 0 00397 grasp_base_pose = grasp_base_pose_list[0] 00398 00399 00400 #calculate R 00401 min_dist = 0.40 #set the minimum distance to obstacles 00402 hdz_size = 0.05 00403 dist_list = list() 00404 index = 0 00405 while index < len(furniture_geometry_list): 00406 th = math.atan((grasp_base_pose.y - furniture_geometry_list[index].pose.y) / (grasp_base_pose.x - furniture_geometry_list[index].pose.x)) 00407 if grasp_base_pose.x < furniture_geometry_list[index].pose.x and grasp_base_pose.y > furniture_geometry_list[index].pose.y: 00408 th = math.pi + th 00409 if grasp_base_pose.x < furniture_geometry_list[index].pose.x and grasp_base_pose.y < furniture_geometry_list[index].pose.y: 00410 th = -math.pi + th 00411 delta_x = math.sqrt((grasp_base_pose.x - furniture_geometry_list[index].pose.x) ** 2 + (grasp_base_pose.y - furniture_geometry_list[index].pose.y) ** 2) * math.cos(th - furniture_geometry_list[index].pose.theta) 00412 delta_y = math.sqrt((grasp_base_pose.x - furniture_geometry_list[index].pose.x) ** 2 + (grasp_base_pose.y - furniture_geometry_list[index].pose.y) ** 2) * math.sin(th - furniture_geometry_list[index].pose.theta) 00413 delta_x = abs(delta_x) 00414 delta_y = abs(delta_y) 00415 dist = max((delta_x - furniture_geometry_list[index].w / 2.0), (delta_y - furniture_geometry_list[index].l / 2.0)) 00416 dist_list.append(dist) 00417 index += 1 00418 R = min(min(dist_list) - min_dist, hdz_size) 00419 00420 00421 00422 #return value 00423 return obstacle_check, reach, grasp_base_pose, R 00424 00425 00426 00427 00428 00429 00430 00431 00432 00433 def symbol_grounding_grasp_base_region_server(): 00434 rospy.init_node('symbol_grounding_grasp_base_region_server') 00435 s = rospy.Service('symbol_grounding_grasp_base_region', SymbolGroundingGraspBaseRegion, handle_symbol_grounding_grasp_base_region) 00436 print "Ready to receive requests." 00437 rospy.spin() 00438 00439 00440 00441 00442 if __name__ == "__main__": 00443 symbol_grounding_grasp_base_region_server()