| 331 | } |
| 332 | |
| 333 | def plotSelf(self, vtag: str, widget: QWidget, ex: float, ey: float, ctf: CoordTF): |
| 334 | if not self.xQ: |
| 335 | raise TypeError('Please call Model.updateVeh() at first.') |
| 336 | rotateMat = np.array( |
| 337 | [ |
| 338 | [cos(self.yaw), -sin(self.yaw)], |
| 339 | [sin(self.yaw), cos(self.yaw)] |
| 340 | ] |
| 341 | ) |
| 342 | vertexes = [ |
| 343 | np.array([[self.length/2], [self.width/2]]), |
| 344 | np.array([[self.length/2], [-self.width/2]]), |
| 345 | np.array([[-self.length/2], [-self.width/2]]), |
| 346 | np.array([[-self.length/2], [self.width/2]]) |
| 347 | ] |
| 348 | rotVertexes = [np.dot(rotateMat, vex) for vex in vertexes] |
| 349 | relativeVex = [[self.x+rv[0]-ex, self.y+rv[1]-ey] |
| 350 | for rv in rotVertexes] |
| 351 | drawVex = [ |
| 352 | QPointF( |
| 353 | ctf.zoomScale*(ctf.drawCenter+rev[0]+ctf.offset[0]), |
| 354 | ctf.zoomScale*(ctf.drawCenter-rev[1]+ctf.offset[1]) |
| 355 | ) for rev in relativeVex |
| 356 | ] |
| 357 | if vtag == 'ego': |
| 358 | vcolor = QColor(211, 84, 0) |
| 359 | elif vtag == 'AoI': |
| 360 | vcolor = QColor(41, 128, 185) |
| 361 | else: |
| 362 | vcolor = QColor(99, 110, 114) |
| 363 | widget.drawPolygon(drawVex, vcolor, vcolor) |
| 364 | # graphics_items.append(graphicsItem('polygon', drawVex, vcolor, vcolor)) |
| 365 | # signal.emit('polygon', drawVex, vcolor, vcolor, 1, False, "") |
| 366 | |
| 367 | def plotTrajectory(self, widget: QWidget, ex: float, ey: float, ctf: CoordTF): |
| 368 | if self.plannedTrajectory and self.plannedTrajectory.xQueue: |