00001
00002
00003 import sys
00004 import roslib; roslib.load_manifest('ar_kinect')
00005 import rospy
00006
00007 import tf
00008 from geometry_msgs.msg import Pose2D, PoseStamped, Pose, PoseWithCovarianceStamped
00009 from nav_msgs.msg import OccupancyGrid, MapMetaData
00010 from ar_pose.msg import ARMarkers
00011 from math import sin, cos, pi
00012 import numpy
00013 import array
00014 from std_msgs.msg import Float64
00015 from std_srvs.srv import Empty, EmptyRequest, EmptyResponse
00016 from tf import TransformListener
00017 import copy
00018 import shlex
00019
00020
00021
00022 class Obstacles:
00023 def __init__(self):
00024 liste = []
00025 self.markers = []
00026
00027 fobj = open("/ros_workspace/robin-jku-linz-ros-pkg/bipedRobin_navigation/data/objects")
00028 self.numOfMarker = int(fobj.readlines()[0])
00029 fobj.close()
00030
00031 obj = open("/ros_workspace/robin-jku-linz-ros-pkg/bipedRobin_navigation/data/objects")
00032
00033 i = 4
00034 line = obj.readlines()[i:i+self.numOfMarker]
00035 liste.append(line)
00036
00037 j = 0
00038 while(j < self.numOfMarker):
00039 ls = shlex.split(liste[0][j])
00040 self.markers.append(ls)
00041 j = j + 1
00042
00043 k = 0
00044 self.numOfMarker = int(self.numOfMarker)
00045 while(k < self.numOfMarker):
00046 l = 1
00047 while(l < 9):
00048 if(l == 5):
00049
00050 self.markers[k][l] = Pose2D()
00051 else:
00052 self.markers[k][l] = float(self.markers[k][l])
00053 l = l + 1
00054 k = k + 1
00055
00056
00057 obj.close()
00058
00059
00060
00061 rospy.Subscriber('ar_pose_markers', ARMarkers, self.localize)
00062
00063 def localize(self, data):
00064
00065 global counter_markers
00066 counter_markers = 0
00067 for marker in data.markers:
00068 if marker.id < len(data.markers):
00069 if ls.frameExists(self.markers[marker.id][0]) and ls.frameExists("/map"):
00070 t = ls.getLatestCommonTime("/map", self.markers[marker.id][0])
00071 position, quaternion = ls.lookupTransform("/map", self.markers[marker.id][0], t)
00072
00073 x = position[0]
00074 y = 8-position[1]
00075
00076 counter_markers = counter_markers + 1
00077
00078 self.markers[marker.id][6] = 1
00079 self.markers[marker.id][5].x = x
00080 self.markers[marker.id][5].y = y
00081
00082 orientation = quaternion
00083
00084 (phi, psi, theta) = tf.transformations.euler_from_quaternion([orientation[0],orientation[1],orientation[2],orientation[3]], axes = 'rxyz')
00085
00086
00087
00088 if phi > pi:
00089 phi = phi - 2*pi
00090 elif phi < - pi:
00091 phi = phi + 2*pi
00092
00093 if (pi/4 <= phi <= 3*pi/4) or (-pi/4 >= phi >= -3*pi/4):
00094
00095
00096
00097 if(-pi/4 <= theta <= pi/4) or (-pi <= theta <= -3*pi/4) or (pi >= theta >= 3*pi/4):
00098
00099
00100 self.markers[marker.id][8] = 1
00101 else:
00102
00103 self.markers[marker.id][8] = 2
00104
00105 self.markers[marker.id][5].theta = -psi
00106
00107 else:
00108
00109
00110
00111 self.markers[marker.id][5].theta = -theta
00112
00113 class Rectangle:
00114 def __init__(self):
00115 self.corners = { 0: Pose2D(), 1: Pose2D(), 2: Pose2D(), 3: Pose2D() }
00116 self.width = 0
00117 self.height = 0
00118 self.rotation = 0
00119
00120 def __init__(self, pose, width, height):
00121
00122
00123
00124 if pose.theta!=0:
00125 corner_lu = Pose2D(pose.x, pose.y, 0)
00126 corner_ld = Pose2D(pose.x+height*sin(pose.theta), pose.y-height*cos(pose.theta), 0)
00127 corner_rd = Pose2D(corner_ld.x+width*cos(pose.theta), corner_ld.y+width*sin(pose.theta), 0)
00128 corner_ru = Pose2D(pose.x+width*cos(pose.theta), pose.y+width*sin(pose.theta), 0)
00129 else:
00130 corner_lu = Pose2D(pose.x, pose.y, 0)
00131 corner_ld = Pose2D(pose.x, pose.y-height, 0)
00132 corner_rd = Pose2D(corner_ld.x+width, corner_ld.y, 0)
00133 corner_ru = Pose2D(pose.x+width, pose.y, 0)
00134
00135 self.corners = { 0: corner_lu, 1: corner_ld, 2: corner_rd, 3: corner_ru }
00136 self.width = width
00137 self.height = height
00138 self.rotation = pose.theta
00139
00140 def isPointInside(self, point):
00141
00142 if self.rotation != 0:
00143 point_rot = Pose2D(point.x*cos(self.rotation)+point.y*sin(self.rotation),-point.x*sin(self.rotation)+point.y*cos(self.rotation), 0)
00144 corner_lu_rot = Pose2D(self.corners[0].x*cos(self.rotation)+self.corners[0].y*sin(self.rotation), -self.corners[0].x*sin(self.rotation)+self.corners[0].y*cos(self.rotation), 0)
00145 else:
00146 point_rot = point;
00147 corner_lu_rot = self.corners[0]
00148
00149
00150 return (corner_lu_rot.x <= point_rot.x <= corner_lu_rot.x+self.width) and (corner_lu_rot.y-self.height <= point_rot.y <= corner_lu_rot.y)
00151
00152 def crossWith(self, anotherRect):
00153
00154
00155 diagonal_1_x = anotherRect.corners[2].x - anotherRect.corners[0].x
00156 diagonal_1_y = anotherRect.corners[2].y - anotherRect.corners[0].y
00157
00158 diagonal_2_x = self.corners[2].x - self.corners[0].x
00159 diagonal_2_y = self.corners[2].y - self.corners[0].y
00160
00161
00162 diagonal_1 = (diagonal_1_x**2+diagonal_1_y**2)**0.5
00163 diagonal_2 = (diagonal_2_x**2+diagonal_2_y**2)**0.5
00164
00165 r_m1 = Pose2D(anotherRect.corners[0].x + diagonal_1_x/2,anotherRect.corners[0].y + diagonal_1_y/2,0)
00166 r_m2 = Pose2D(self.corners[0].x + diagonal_2_x/2,self.corners[0].y + diagonal_2_y/2,0)
00167
00168
00169 if (r_m1.x-r_m2.x)**2 + (r_m1.y-r_m2.y)**2 > (diagonal_1+diagonal_2)**2:
00170 return False
00171 else:
00172
00173 selfInRect = self.isPointInside(anotherRect.corners[0]) or self.isPointInside(anotherRect.corners[1]) or self.isPointInside(anotherRect.corners[2]) or self.isPointInside(anotherRect.corners[3])
00174 rectInSelf = anotherRect.isPointInside(self.corners[0]) or anotherRect.isPointInside(self.corners[1]) or anotherRect.isPointInside(self.corners[2]) or anotherRect.isPointInside(self.corners[3])
00175
00176 return rectInSelf or selfInRect
00177
00178 def fillobstacles(obstacles, occupancy):
00179
00180
00181 rows = xrange(len(occupancy.data))
00182 cols = xrange(len(occupancy.data[1]))
00183
00184 newMap = 0
00185 for marker_id in xrange(obstacles.numOfMarker):
00186 marker = obstacles.markers[marker_id]
00187
00188 if marker[6]==1:
00189 newMap = 1
00190
00191 if newMap == 1:
00192
00193 for row in rows:
00194 for col in cols:
00195 occupancy.data[row][col] = 0
00196
00197 for marker_id in xrange(obstacles.numOfMarker):
00198 marker = obstacles.markers[marker_id]
00199 if (marker[6] == 1):
00200
00201 corner_lu = Pose2D(marker[5].x-marker[3]*cos(marker[5].theta)-marker[4]*sin(marker[5].theta), marker[5].y-marker[3]*sin(marker[5].theta)+marker[4]*cos(marker[5].theta),marker[5].theta)
00202
00203
00204
00205
00206
00207
00208 if (marker[8] == 0):
00209 obstacle = Rectangle(corner_lu, marker[1], marker[2])
00210 elif (marker[8] == 1):
00211 obstacle = Rectangle(corner_lu, marker[1], marker[7])
00212 else:
00213 obstacle = Rectangle(corner_lu, marker[2], marker[7])
00214
00215 marker[8] = 0
00216
00217
00218 for row in rows:
00219 for col in cols:
00220
00221
00222 corner_lu = Pose2D((col)*occupancy.info.resolution, (occupancy.info.height-row)*occupancy.info.resolution, 0)
00223
00224 raster_element = Rectangle(corner_lu, occupancy.info.resolution, occupancy.info.resolution)
00225 if obstacle.crossWith(raster_element):
00226
00227 occupancy.data[row][col] = 100
00228 marker[6] = 0
00229
00230 return occupancy
00231
00232
00233 ls = TransformListener()
00234
00235
00236 def handle_createMap(req):
00237
00238
00239 info = MapMetaData()
00240
00241 info.width = 160
00242 info.height = 160
00243
00244 info.resolution = 0.05
00245
00246
00247 occMat = [[0 for j in xrange(info.width)] for i in xrange(info.height)]
00248 occupancy = OccupancyGrid()
00249 temp_occupancy = OccupancyGrid()
00250 occupancy.info = info
00251 occupancy.data = occMat[:][:]
00252 occupancy.header.frame_id = "/map"
00253
00254 occupancy = fillobstacles(obstacles, occupancy)
00255
00256 temp_occupancy.info = info
00257 temp_occupancy.header.frame_id = "/map"
00258
00259 global counter_markers
00260
00261
00262 if(counter_markers == 0):
00263
00264 occMat = [[0 for j in xrange(info.width)] for i in xrange(info.height)]
00265 occupancy.data = occMat
00266 occMat_numpy = numpy.matrix(occupancy.data)
00267 occMat_list = list(numpy.array(occMat_numpy).reshape(-1,))
00268 occMat_int8list = array.array('i',occMat_list)
00269 temp_occupancy.data = occMat_int8list
00270 pub1.publish(temp_occupancy)
00271
00272
00273
00274 occMat_numpy = numpy.matrix(occupancy.data)
00275 occMat_list = list(numpy.array(occMat_numpy).reshape(-1,))
00276 occMat_int8list = array.array('i',occMat_list)
00277 temp_occupancy.data = occMat_int8list
00278
00279
00280
00281 pub1.publish(temp_occupancy)
00282 return EmptyResponse();
00283
00284
00285 if __name__ == '__main__':
00286 rospy.init_node('calcmap')
00287
00288
00289 fobj = open("/ros_workspace/robin-jku-linz-ros-pkg/bipedRobin_navigation/data/objects")
00290 numMarker = int(fobj.readlines()[0])
00291 fobj.close()
00292
00293 global pub1
00294 pub1 = rospy.Publisher('drawmap_coord', OccupancyGrid)
00295
00296 global coordinates
00297
00298 coordinates = [[0.0000 for j in xrange(5)] for i in xrange(numMarker)]
00299
00300
00301 obstacles = Obstacles()
00302
00303
00304 info = MapMetaData()
00305
00306 info.width = 80
00307 info.height = 80
00308
00309 info.resolution = 0.05
00310
00311
00312 occMat = [[0 for j in xrange(info.width)] for i in xrange(info.height)]
00313 occupancy = OccupancyGrid()
00314 temp_occupancy = OccupancyGrid()
00315 occupancy.info = info
00316 occupancy.data = occMat[:][:]
00317 occupancy.header.frame_id = "/map"
00318
00319
00320
00321 createMap_srv = rospy.Service("createMap_srv", Empty, handle_createMap)
00322
00323 rospy.spin()
00324
00325