$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 deliver 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 00058 import roslib; roslib.load_manifest('srs_symbolic_grounding') 00059 from srs_symbolic_grounding.srv import * 00060 from srs_symbolic_grounding.msg import * 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 #from srs_knowledge.msg import * 00066 import rospy 00067 import math 00068 import tf 00069 from tf.transformations import euler_from_quaternion 00070 00071 00072 00073 00074 def getMapClient(): #read map from navigation service 00075 00076 try: 00077 reqMap = rospy.ServiceProxy('static_map', GetMap) 00078 res = reqMap() 00079 return res 00080 except rospy.ServiceException, e: 00081 print "Service call failed: %s"%e 00082 00083 00084 00085 def obstacleCheck(dbp, fgl): 00086 deliver_base_pose = dbp 00087 furniture_geometry_list = fgl 00088 obstacle_checked_deliver_base_pose = Pose2D() 00089 wall_checked_deliver_base_pose = Pose2D() 00090 00091 #obstacle check 00092 dist_to_obstacles = 0.4 #set the minimum distance to the household furnitures 00093 index = 0 00094 while index < len(furniture_geometry_list): 00095 th = math.atan((deliver_base_pose.y - furniture_geometry_list[index].pose.y) / (deliver_base_pose.x - furniture_geometry_list[index].pose.x)) 00096 if deliver_base_pose.x < furniture_geometry_list[index].pose.x and deliver_base_pose.y > furniture_geometry_list[index].pose.y: 00097 th = math.pi + th 00098 if deliver_base_pose.x < furniture_geometry_list[index].pose.x and deliver_base_pose.y < furniture_geometry_list[index].pose.y: 00099 th = -math.pi + th 00100 dist = math.sqrt((deliver_base_pose.x - furniture_geometry_list[index].pose.x) ** 2 + (deliver_base_pose.y - furniture_geometry_list[index].pose.y) ** 2) 00101 delta_x = dist * math.cos(th - furniture_geometry_list[index].pose.theta) 00102 delta_y = dist * math.sin(th - furniture_geometry_list[index].pose.theta) 00103 if (delta_x <= -(furniture_geometry_list[index].w / 2.0 + dist_to_obstacles) or delta_x >= (furniture_geometry_list[index].w / 2.0 + dist_to_obstacles)) or (delta_y <= -(furniture_geometry_list[index].l / 2.0 + dist_to_obstacles) or delta_y >= (furniture_geometry_list[index_2].l / 2.0 + dist_to_obstacles)): 00104 index += 1 00105 else: 00106 break 00107 if index == len(furniture_geometry_list): 00108 obstacle_checked_deliver_base_pose = deliver_base_pose 00109 00110 print obstacle_checked_deliver_base_pose 00111 if obstacle_checked_deliver_base_pose: #check if there is a obstacle free pose. 00112 00113 #wall check 00114 data = getMapClient() #get occupancy grid map from the navigation serves 00115 dist_to_walls = 0.5 #set the minimum distance to the walls 00116 threshold = 10.0 #set the threshold to decide if a pose is occupaied. >threshold:occupied. 00117 step_angle = 10.0 #set the step angle for putting points around the robot for the wall check (360 / step_angle points will be used) 00118 map_index_list = list() 00119 00120 n = 0 00121 while n < int(360.0 / step_angle): 00122 wall_check_point_x = obstacle_checked_deliver_base_pose.x + dist_to_walls * math.cos(n * step_angle / 180.0 * math.pi) 00123 wall_check_point_y = obstacle_checked_deliver_base_pose.y + dist_to_walls * math.sin(n * step_angle / 180.0 * math.pi) 00124 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) 00125 map_index_list.append(map_index) 00126 n += 1 00127 00128 map_index = int((obstacle_checked_deliver_base_pose.y - data.map.info.origin.position.y) / data.map.info.resolution) * data.map.info.width + int((obstacle_checked_deliver_base_pose.x - data.map.info.origin.position.x) / data.map.info.resolution) 00129 map_index_list.append(map_index) 00130 00131 index = 0 00132 while index < len(map_index_list): 00133 print data.map.data[map_index_list[index]] 00134 if -1 < data.map.data[map_index_list[index]] < threshold: 00135 index += 1 00136 else: 00137 break 00138 print index 00139 if index == len(map_index_list): 00140 wall_checked_deliver_base_pose = obstacle_checked_deliver_base_pose 00141 00142 return wall_checked_deliver_base_pose 00143 00144 00145 00146 00147 #calculate deliver base region 00148 def handle_symbol_grounding_deliver_base_region(req): 00149 00150 00151 #transform from knowledge base data to function useable data 00152 parent_obj_x = req.parent_obj_geometry.pose.position.x 00153 parent_obj_y = req.parent_obj_geometry.pose.position.y 00154 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]) 00155 parent_obj_th = parent_obj_rpy[2] 00156 parent_obj_l = req.parent_obj_geometry.l 00157 parent_obj_w = req.parent_obj_geometry.w 00158 parent_obj_h = req.parent_obj_geometry.h 00159 00160 #transfrom furniture geometry list 00161 index = 0 00162 furniture_geometry_list = list() 00163 while index < len(req.furniture_geometry_list): 00164 furniture_geometry = FurnitureGeometry() 00165 furniture_geometry.pose.x = req.furniture_geometry_list[index].pose.position.x 00166 furniture_geometry.pose.y = req.furniture_geometry_list[index].pose.position.y 00167 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]) 00168 furniture_geometry.pose.theta = furniture_pose_rpy[2] 00169 furniture_geometry.l = req.furniture_geometry_list[index].l 00170 furniture_geometry.w = req.furniture_geometry_list[index].w 00171 furniture_geometry.h = req.furniture_geometry_list[index].h 00172 furniture_geometry_list.append(furniture_geometry) 00173 index += 1 00174 00175 #variable structure define 00176 deliver_base_pose = Pose2D() 00177 deliver_base_pose_1 = Pose2D() 00178 deliver_base_pose_2 = Pose2D() 00179 00180 #fix angle problem 00181 if parent_obj_th < 0: 00182 parent_obj_th += 2.0 * math.pi 00183 elif parent_obj_th > 2.0 * math.pi: 00184 parent_obj_th -= 2.0 * math.pi 00185 00186 #calculate deliver base poses 00187 rb_distance = 0.5 00188 deliver_base_pose_1.x = parent_obj_x + (parent_obj_w * 0.5 + rb_distance) * math.cos(parent_obj_th) + 0.3 * parent_obj_l * math.sin(parent_obj_th) 00189 deliver_base_pose_1.y = parent_obj_y + (parent_obj_w * 0.5 + rb_distance) * math.sin(parent_obj_th) - 0.3 * parent_obj_l * math.cos(parent_obj_th) 00190 deliver_base_pose_1.theta = parent_obj_th 00191 if deliver_base_pose_1.theta > math.pi: 00192 deliver_base_pose_1.theta -= 2.0 * math.pi 00193 elif deliver_base_pose_1.theta < -math.pi: 00194 deliver_base_pose_1.theta += 2.0 * math.pi 00195 00196 deliver_base_pose_2.x = parent_obj_x + (parent_obj_w * 0.5 + rb_distance) * math.cos(parent_obj_th) - 0.3 * parent_obj_l * math.sin(parent_obj_th) 00197 deliver_base_pose_2.y = parent_obj_y + (parent_obj_w * 0.5 + rb_distance) * math.sin(parent_obj_th) + 0.3 * parent_obj_l * math.cos(parent_obj_th) 00198 deliver_base_pose_2.theta = parent_obj_th 00199 if deliver_base_pose_2.theta > math.pi: 00200 deliver_base_pose_2.theta -= 2.0 * math.pi 00201 elif deliver_base_pose_2.theta < -math.pi: 00202 deliver_base_pose_2.theta += 2.0 * math.pi 00203 00204 #obstacle check 00205 obstacle_checked_deliver_base_pose_1 = obstacleCheck(deliver_base_pose_1, furniture_geometry_list) 00206 obstacle_checked_deliver_base_pose_2 = obstacleCheck(deliver_base_pose_2, furniture_geometry_list) 00207 00208 #choose deliver base pose 00209 if obstacle_checked_deliver_base_pose_1 and obstacle_checked_deliver_base_pose_2: 00210 00211 #get the robot's current pose from tf 00212 rb_pose = Pose2D() 00213 listener = tf.TransformListener() 00214 listener.waitForTransform("/map", "/base_link", rospy.Time(0), rospy.Duration(4.0)) #wait for 4secs for the coordinate to be transformed 00215 (trans,rot) = listener.lookupTransform("/map", "/base_link", rospy.Time(0)) 00216 rb_pose.x = trans[0] 00217 rb_pose.y = trans[1] 00218 rb_pose_rpy = tf.transformations.euler_from_quaternion(rot) 00219 rb_pose.theta = rb_pose_rpy[2] 00220 rospy.sleep(0.5) 00221 00222 dist_1 = (rb_pose.x - obstacle_checked_deliver_base_pose_1.x) ** 2 + (rb_pose.y - obstacle_checked_deliver_base_pose_1.y) ** 2 00223 dist_2 = (rb_pose.x - obstacle_checked_deliver_base_pose_2.x) ** 2 + (rb_pose.y - obstacle_checked_deliver_base_pose_2.y) ** 2 00224 if dist_1 > dist_2: 00225 deliver_base_pose = obstacle_checked_deliver_base_pose_2 00226 else: 00227 deliver_base_pose = obstacle_checked_deliver_base_pose_1 00228 00229 elif obstacle_checked_deliver_base_pose_1: 00230 deliver_base_pose = obstacle_checked_deliver_base_pose_1 00231 elif obstacle_checked_deliver_base_pose_2: 00232 deliver_base_pose = obstacle_checked_deliver_base_pose_2 00233 else: 00234 print "no valid deliver pose." 00235 00236 R = 0.0 00237 if deliver_base_pose: 00238 #calculate R 00239 min_dist = 0.40 #set the minimum distance to obstacles 00240 deliver_region_size = 0.1 00241 dist_list = list() 00242 index = 0 00243 while index < len(furniture_geometry_list): 00244 th = math.atan((deliver_base_pose.y - furniture_geometry_list[index].pose.y) / (deliver_base_pose.x - furniture_geometry_list[index].pose.x)) 00245 if deliver_base_pose.x < furniture_geometry_list[index].pose.x and deliver_base_pose.y > furniture_geometry_list[index].pose.y: 00246 th = math.pi + th 00247 if deliver_base_pose.x < furniture_geometry_list[index].pose.x and deliver_base_pose.y < furniture_geometry_list[index].pose.y: 00248 th = -math.pi + th 00249 d = math.sqrt((deliver_base_pose.x - furniture_geometry_list[index].pose.x) ** 2 + (deliver_base_pose.y - furniture_geometry_list[index].pose.y) ** 2) 00250 delta_x = d * math.cos(th - furniture_geometry_list[index].pose.theta) 00251 delta_y = d * math.sin(th - furniture_geometry_list[index].pose.theta) 00252 delta_x = abs(delta_x) 00253 delta_y = abs(delta_y) 00254 dist = max((delta_x - furniture_geometry_list[index].w / 2.0), (delta_y - furniture_geometry_list[index].l / 2.0)) 00255 dist_list.append(dist) 00256 index += 1 00257 R = min(min(dist_list) - min_dist, deliver_region_size) 00258 00259 return deliver_base_pose, R 00260 00261 00262 00263 def symbol_grounding_deliver_base_region_server(): 00264 rospy.init_node('symbol_grounding_deliver_base_region_server') 00265 s = rospy.Service('symbol_grounding_deliver_base_region', SymbolGroundingDeliverBaseRegion, handle_symbol_grounding_deliver_base_region) 00266 print "Ready to receive requests." 00267 rospy.spin() 00268 00269 00270 00271 if __name__ == "__main__": 00272 symbol_grounding_deliver_base_region_server() 00273 00274