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