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 tf import TransformListener
00016 import copy
00017 import shlex
00018
00019 global change
00020
00021 class Obstacles:
00022 def __init__(self):
00023 global change
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 print "TEST"
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 global coordinates
00068
00069
00070 global change
00071 change = 0
00072 for marker in data.markers:
00073 if marker.id < len(data.markers):
00074 if ls.frameExists(self.markers[marker.id][0]) and ls.frameExists("/map"):
00075 t = ls.getLatestCommonTime("/map", self.markers[marker.id][0])
00076 position, quaternion = ls.lookupTransform("/map", self.markers[marker.id][0], t)
00077
00078 x = position[0]
00079 y = 4-position[1]
00080
00081 counter_markers = counter_markers + 1
00082
00083 self.markers[marker.id][6] = 1
00084 self.markers[marker.id][5].x = x
00085 self.markers[marker.id][5].y = y
00086
00087 orientation = quaternion
00088
00089 (phi, psi, theta) = tf.transformations.euler_from_quaternion([orientation[0],orientation[1],orientation[2],orientation[3]], axes = 'rxyz')
00090
00091
00092
00093
00094
00095
00096
00097
00098 if(abs(coordinates[marker.id][0]-x) >= 0.005 or abs(coordinates[marker.id][1]-y) >= 0.005 or abs(coordinates[marker.id][2]-phi) >= 0.1 or abs(coordinates[marker.id][3]-psi) >= 0.1 or abs(coordinates[marker.id][4]-theta) >= 0.1):
00099 coordinates[marker.id][0] = x
00100 coordinates[marker.id][1] = y
00101 coordinates[marker.id][2] = phi
00102 coordinates[marker.id][3] = psi
00103 coordinates[marker.id][4] = theta
00104
00105 rospy.loginfo("test");
00106 global change
00107 change = 1
00108
00109 if phi > pi:
00110 phi = phi - 2*pi
00111 elif phi < - pi:
00112 phi = phi + 2*pi
00113
00114 if (pi/4 <= phi <= 3*pi/4) or (-pi/4 >= phi >= -3*pi/4):
00115
00116
00117
00118 if(-pi/4 <= theta <= pi/4) or (-pi <= theta <= -3*pi/4) or (pi >= theta >= 3*pi/4):
00119
00120
00121 self.markers[marker.id][8] = 1
00122 else:
00123
00124 self.markers[marker.id][8] = 2
00125
00126 self.markers[marker.id][5].theta = -psi
00127
00128 else:
00129
00130
00131
00132 self.markers[marker.id][5].theta = -theta
00133
00134 class Rectangle:
00135 def __init__(self):
00136 self.corners = { 0: Pose2D(), 1: Pose2D(), 2: Pose2D(), 3: Pose2D() }
00137 self.width = 0
00138 self.height = 0
00139 self.rotation = 0
00140
00141 def __init__(self, pose, width, height):
00142
00143
00144
00145 if pose.theta!=0:
00146 corner_lu = Pose2D(pose.x, pose.y, 0)
00147
00148
00149
00150
00151 corner_ld = Pose2D(pose.x+height*sin(pose.theta), pose.y-height*cos(pose.theta), 0)
00152 corner_rd = Pose2D(corner_ld.x+width*cos(pose.theta), corner_ld.y+width*sin(pose.theta), 0)
00153 corner_ru = Pose2D(pose.x+width*cos(pose.theta), pose.y+width*sin(pose.theta), 0)
00154 else:
00155 corner_lu = Pose2D(pose.x, pose.y, 0)
00156 corner_ld = Pose2D(pose.x, pose.y-height, 0)
00157 corner_rd = Pose2D(corner_ld.x+width, corner_ld.y, 0)
00158 corner_ru = Pose2D(pose.x+width, pose.y, 0)
00159
00160 self.corners = { 0: corner_lu, 1: corner_ld, 2: corner_rd, 3: corner_ru }
00161 self.width = width
00162 self.height = height
00163 self.rotation = pose.theta
00164
00165 def isPointInside(self, point):
00166
00167 if self.rotation != 0:
00168 point_rot = Pose2D(point.x*cos(self.rotation)+point.y*sin(self.rotation),-point.x*sin(self.rotation)+point.y*cos(self.rotation), 0)
00169 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)
00170 else:
00171 point_rot = point;
00172 corner_lu_rot = self.corners[0]
00173
00174
00175 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)
00176
00177 def crossWith(self, anotherRect):
00178
00179
00180 diagonal_1_x = anotherRect.corners[2].x - anotherRect.corners[0].x
00181 diagonal_1_y = anotherRect.corners[2].y - anotherRect.corners[0].y
00182
00183 diagonal_2_x = self.corners[2].x - self.corners[0].x
00184 diagonal_2_y = self.corners[2].y - self.corners[0].y
00185
00186
00187 diagonal_1 = (diagonal_1_x**2+diagonal_1_y**2)**0.5
00188 diagonal_2 = (diagonal_2_x**2+diagonal_2_y**2)**0.5
00189
00190 r_m1 = Pose2D(anotherRect.corners[0].x + diagonal_1_x/2,anotherRect.corners[0].y + diagonal_1_y/2,0)
00191 r_m2 = Pose2D(self.corners[0].x + diagonal_2_x/2,self.corners[0].y + diagonal_2_y/2,0)
00192
00193
00194 if (r_m1.x-r_m2.x)**2 + (r_m1.y-r_m2.y)**2 > (diagonal_1+diagonal_2)**2:
00195 return False
00196 else:
00197
00198 selfInRect = self.isPointInside(anotherRect.corners[0]) or self.isPointInside(anotherRect.corners[1]) or self.isPointInside(anotherRect.corners[2]) or self.isPointInside(anotherRect.corners[3])
00199 rectInSelf = anotherRect.isPointInside(self.corners[0]) or anotherRect.isPointInside(self.corners[1]) or anotherRect.isPointInside(self.corners[2]) or anotherRect.isPointInside(self.corners[3])
00200
00201 return rectInSelf or selfInRect
00202
00203 def fillobstacles(obstacles, occupancy):
00204
00205
00206 rows = xrange(len(occupancy.data))
00207 cols = xrange(len(occupancy.data[1]))
00208
00209 newMap = 0
00210 for marker_id in xrange(obstacles.numOfMarker):
00211 marker = obstacles.markers[marker_id]
00212
00213 if marker[6]==1:
00214 newMap = 1
00215
00216 if newMap == 1:
00217
00218 for row in rows:
00219 for col in cols:
00220 occupancy.data[row][col] = 0
00221
00222 for marker_id in xrange(obstacles.numOfMarker):
00223 marker = obstacles.markers[marker_id]
00224 if (marker[6] == 1):
00225
00226 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)
00227
00228
00229
00230
00231
00232
00233 if (marker[8] == 0):
00234 obstacle = Rectangle(corner_lu, marker[1], marker[2])
00235 elif (marker[8] == 1):
00236 obstacle = Rectangle(corner_lu, marker[1], marker[7])
00237 else:
00238 obstacle = Rectangle(corner_lu, marker[2], marker[7])
00239
00240 marker[8] = 0
00241
00242
00243 for row in rows:
00244 for col in cols:
00245
00246
00247 corner_lu = Pose2D((col)*occupancy.info.resolution, (occupancy.info.height-row)*occupancy.info.resolution, 0)
00248
00249 raster_element = Rectangle(corner_lu, occupancy.info.resolution, occupancy.info.resolution)
00250 if obstacle.crossWith(raster_element):
00251
00252 occupancy.data[row][col] = 100
00253 marker[6] = 0
00254
00255 return occupancy
00256
00257
00258 ls = TransformListener()
00259
00260 counter_markers = 0
00261
00262 if __name__ == '__main__':
00263 rospy.init_node('calcmap')
00264
00265
00266 fobj = open("/ros_workspace/robin-jku-linz-ros-pkg/bipedRobin_navigation/data/objects")
00267 numMarker = int(fobj.readlines()[0])
00268 fobj.close()
00269
00270 global coordinates
00271
00272 coordinates = [[0.0000 for j in xrange(5)] for i in xrange(numMarker)]
00273
00274
00275
00276
00277 obstacles = Obstacles()
00278
00279
00280 info = MapMetaData()
00281
00282 info.width = 80
00283 info.height = 80
00284
00285 info.resolution = 0.05
00286
00287
00288 occMat = [[0 for j in xrange(info.width)] for i in xrange(info.height)]
00289 occupancy = OccupancyGrid()
00290 old_occupancy = OccupancyGrid()
00291 old_occupancy.data = occMat[:][:]
00292 temp_occupancy = OccupancyGrid()
00293 occupancy.info = info
00294 occupancy.data = occMat[:][:]
00295 occupancy.header.frame_id = "/map"
00296 old_occupancy.data = occMat[:][:]
00297 pub1 = rospy.Publisher('drawmap_coord', OccupancyGrid)
00298 pub2 = rospy.Publisher('drawmap_pixel', OccupancyGrid)
00299
00300
00301 marker = obstacles.markers[2]
00302
00303 r = rospy.Rate(rospy.get_param('rate', 10.0))
00304 global change
00305 change = 1
00306
00307
00308 while not rospy.is_shutdown():
00309 global change
00310
00311 rows = xrange(len(occupancy.data))
00312 cols = xrange(len(occupancy.data[1]))
00313
00314 occupancy = fillobstacles(obstacles, occupancy)
00315
00316
00317 counter = 0
00318
00319 for row in rows:
00320 for col in cols:
00321 if(old_occupancy.data[row][col] != occupancy.data[row][col]):
00322 counter = counter + 1
00323
00324 temp_occupancy.info = info
00325 temp_occupancy.header.frame_id = "/map"
00326
00327
00328 if(counter_markers == 0 or change == 1):
00329
00330 occMat = [[0 for j in xrange(info.width)] for i in xrange(info.height)]
00331 occupancy.data = occMat
00332 occMat_numpy = numpy.matrix(occupancy.data)
00333 occMat_list = list(numpy.array(occMat_numpy).reshape(-1,))
00334 occMat_int8list = array.array('i',occMat_list)
00335 temp_occupancy.data = occMat_int8list
00336 change = 0
00337 pub1.publish(temp_occupancy)
00338 pub2.publish(temp_occupancy)
00339
00340
00341
00342 occMat_numpy = numpy.matrix(occupancy.data)
00343 occMat_list = list(numpy.array(occMat_numpy).reshape(-1,))
00344 occMat_int8list = array.array('i',occMat_list)
00345
00346 temp_occupancy.data = occMat_int8list
00347
00348
00349 if(counter > counter_markers*10):
00350 old_occupancy.data = copy.deepcopy(occupancy.data)
00351 pub2.publish(temp_occupancy)
00352
00353
00354 if(change == 1):
00355 pub1.publish(temp_occupancy)
00356 change = 0
00357 r.sleep()