Go to the documentation of this file.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 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
00066 import rospy
00067 import math
00068 import tf
00069 from tf.transformations import euler_from_quaternion
00070
00071
00072
00073
00074 def getMapClient():
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
00092 dist_to_obstacles = 0.4
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:
00112
00113
00114 data = getMapClient()
00115 dist_to_walls = 0.5
00116 threshold = 10.0
00117 step_angle = 10.0
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
00148 def handle_symbol_grounding_deliver_base_region(req):
00149
00150
00151
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
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
00176 deliver_base_pose = Pose2D()
00177 deliver_base_pose_1 = Pose2D()
00178 deliver_base_pose_2 = Pose2D()
00179
00180
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
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
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
00209 if obstacle_checked_deliver_base_pose_1 and obstacle_checked_deliver_base_pose_2:
00210
00211
00212 rb_pose = Pose2D()
00213 listener = tf.TransformListener()
00214 listener.waitForTransform("/map", "/base_link", rospy.Time(0), rospy.Duration(4.0))
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
00239 min_dist = 0.40
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