class Map(QtGui.QFrame):
msg2Statusbar = QtCore.pyqtSignal(str)
changedStatus = QtCore.pyqtSignal(bool)
MapWidth = 1024
MapHeight = 640
Speed = 100
NoAction = 0
PlaceBlockAction = 1
PlaceRobotAction = 2
PlaceTargetAction = 3
def __init__(self, parent):
super(Map, self).__init__(parent)
self.parent = parent
self.initMap()
def initMap(self):
QtCore.qDebug('sim_map.Map.initMap')
self.timer = QtCore.QBasicTimer()
self.setFrameStyle(QtGui.QFrame.Panel | QtGui.QFrame.Raised)
self.objects = []
self.setFocusPolicy(QtCore.Qt.StrongFocus)
self.setFixedSize(Map.MapWidth, Map.MapHeight)
self.isStarted = False
self.isPaused = False
self.clearMap()
self.dragXstart = -1
self.dragYstart = -1
self.dragXend = -1
self.dragYend = -1
self.dragObject = None
self.mouseActionType = Map.NoAction
self.saveToImage = True
self.mapChanged = False
self.robot = None
self.target = None
self.vbox = QtGui.QVBoxLayout()
self.setLayout(self.vbox)
self.simStats = SimStats(self)
def load(self, fname):
print("[sim_map.Map.load]")
with open(fname) as fp:
fp.readline()
line = fp.readline().strip().split()
Map.MapWidth = int(line[0])
Map.MapHeight = int(line[1])
Map.Speed = int(line[2])
fp.readline()
line = fp.readline().strip()
nObjects = int(line)
fp.readline()
for i in range(nObjects):
line = fp.readline().strip().split()
x = int(line[0])
y = int(line[1])
w = int(line[2])
h = int(line[3])
print(line)
self.objects.append(Rectangle(x, y, w, h))
self.setFixedSize(Map.MapWidth, Map.MapHeight)
self.repaint()
def save(self, fname):
print("[sim_map.Map.save]")
self.setChanged(False)
with open(fname, 'w') as fp:
fp.write('# MapWidth MapHeight Speed\n')
fp.write(str(Map.MapWidth) + ' ' + str(Map.MapHeight) + ' ' + str(Map.Speed) + '\n')
fp.write('# Number of objects\n')
fp.write(str(len(self.objects)) + '\n')
fp.write('# x y width height\n')
for obj in self.objects:
fp.write(str(obj) + '\n')
def start(self):
QtCore.qDebug('sim_map.Map.start')
if self.isPaused:
return
self.isStarted = True
self.clearMap()
self.msg2Statusbar.emit(str(0))
self.timer.start(Map.Speed, self)
def pause(self):
QtCore.qDebug('sim_map.Map.pause')
if not self.isStarted:
return
self.isPaused = not self.isPaused
if self.isPaused:
self.timer.stop()
self.msg2Statusbar.emit("paused")
else:
self.timer.start(Map.Speed, self)
self.msg2Statusbar.emit(str(0))
self.update()
def mousePressEvent(self, QMouseEvent):
x = QMouseEvent.x()
y = QMouseEvent.y()
if self.mouseActionType == Map.NoAction:
self.dragXstart = x
self.dragYstart = y
self.dragObject = Rectangle()
self.dragObject.updateDrag(self.dragXstart,
self.dragYstart, self.dragXstart, self.dragYstart)
self.mouseActionType = Map.PlaceBlockAction
#TODO cleanup
#print(QMouseEvent.pos())
elif self.mouseActionType == Map.PlaceRobotAction:
self.robot = Robot(x, y, 0)
self.setStatsWidget()
def mouseMoveEvent(self, QMouseEvent):
x = QMouseEvent.x()
y = QMouseEvent.y()
if self.mouseActionType == Map.PlaceBlockAction:
self.dragXend = x
self.dragYend = y
self.dragObject.updateDrag(self.dragXstart,
self.dragYstart, self.dragXend, self.dragYend)
#TODO cleanup
#print(QMouseEvent.pos())
self.repaint()
elif self.mouseActionType == Map.PlaceRobotAction:
ab = x - self.robot.posX
mb = math.sqrt(math.pow(x - self.robot.posX, 2) + math.pow(y - self.robot.posY, 2))
if mb == 0:
newTheta = 0
else:
newTheta = math.acos(ab / mb)
if y < self.robot.posY:
newTheta = math.pi - newTheta + math.pi
print('theta: %f' % newTheta)
self.robot.setOrientation(newTheta)
if self.target is not None:
# I am computing the angle relative to Ox ax.
x = self.target.x - self.robot.posX
y = self.target.y - self.robot.posY
ab = x
mb = math.sqrt(x * x + y * y)
if mb == 0:
theta = 0
else:
theta = math.acos(ab / mb)
if self.target.y < self.robot.posY:
theta = math.pi - theta + math.pi
theta = theta - newTheta
if theta < 0:
theta = theta + 2 * math.pi
self.robot.setTargetDirection(theta)
self.repaint()
def mouseReleaseEvent(self, QMouseEvent):
x = QMouseEvent.x()
y = QMouseEvent.y()
if self.mouseActionType == Map.PlaceRobotAction:
self.mouseActionType = Map.NoAction
self.simStats.btPlaceRobot.setEnabled(False)
ab = x - self.robot.posX
mb = math.sqrt(math.pow(x - self.robot.posX, 2) + math.pow(y - self.robot.posY, 2))
if mb == 0:
newTheta = 0
else:
newTheta = math.acos(ab / mb)
if y < self.robot.posY:
newTheta = math.pi - newTheta + math.pi
self.robot.setOrientation(newTheta)
if self.target is not None:
# I am computing the angle relative to Ox ax.
x = self.target.x - self.robot.posX
y = self.target.y - self.robot.posY
ab = x
mb = math.sqrt(x * x + y * y)
if mb == 0:
theta = 0
else:
theta = math.acos(ab / mb)
if self.target.y < self.robot.posY:
theta = math.pi - theta + math.pi
theta = theta - newTheta
if theta < 0:
theta = theta + 2 * math.pi
self.robot.setTargetDirection(theta)
self.repaint()
elif self.mouseActionType == Map.PlaceTargetAction:
self.target = Target(x, y)
self.mouseActionType = Map.NoAction
self.simStats.btPlaceTarget.setEnabled(False)
self.simStats.btPlaceRobot.setEnabled(True)
self.repaint()
elif self.mouseActionType == Map.PlaceBlockAction:
self.dragXend = x
self.dragYend = y
self.dragObject.updateDrag(self.dragXstart, self.dragYstart,
self.dragXend, self.dragYend)
self.objects.append(self.dragObject)
self.dragObject = None
self.saveToImage = True
self.setChanged(True)
self.mouseActionType = Map.NoAction
#TODO
#print(QMouseEvent.pos())
self.repaint()
def paintEvent(self, event):
rect = self.contentsRect()
if self.saveToImage == True:
ImageMap.image = QtGui.QImage(rect.right(), rect.bottom(), QtGui.QImage.Format_RGB32)
imagePainter = QtGui.QPainter(ImageMap.image)
self.draw(imagePainter)
ImageMap.image.save('image.jpg')
self.saveToImage = False
painter = QtGui.QPainter(self)
self.draw(painter)
def draw(self, painter):
rect = self.contentsRect()
painter.setPen(QtGui.QColor(0xff0000))
#TODO
#QtCore.qDebug('[sim_map.Map.paintEvent] %d %d %d %d' % (rect.top(), rect.left(), rect.bottom(), rect.right()))
painter.fillRect(0, 0, rect.right(), rect.bottom(), QtGui.QColor(0xffffff))
for obj in self.objects:
obj.draw(painter)
if not self.dragObject is None:
self.dragObject.draw(painter)
if self.robot is not None and self.saveToImage != True:
self.robot.draw(painter)
if self.target is not None and self.saveToImage != True:
self.target.draw(painter)
def keyPressEvent(self, event):
key = event.key()
if key == QtCore.Qt.Key_S:
self.start()
return
if not self.isStarted:
super(Map, self).keyPressEvent(event)
return
if key == QtCore.Qt.Key_P:
self.pause()
return
if self.isPaused:
return
elif key == QtCore.Qt.Key_Q:
self.robot.increaseLeftMotorSpeed(10)
elif key == QtCore.Qt.Key_A:
self.robot.increaseLeftMotorSpeed(-10)
elif key == QtCore.Qt.Key_E:
self.robot.increaseRightMotorSpeed(10)
elif key == QtCore.Qt.Key_D:
self.robot.increaseRightMotorSpeed(-10)
else:
super(Map, self).keyPressEvent(event)
def timerEvent(self, event):
if event.timerId() == self.timer.timerId():
if self.robot is not None:
self.robot.move()
self.repaint()
else:
super(Map, self).timerEvent(event)
def clearMap(self):
self.objects = []
def changed(self):
return self.mapChanged
def setChanged(self, mapChanged):
self.mapChanged = mapChanged
self.changedStatus.emit(bool(self.mapChanged))
def getStatsWidget(self):
widgets = []
widgets.append(self.simStats)
if self.robot is not None:
widgets.append(self.robot.getStatsWidget())
return widgets
def setStatsWidget(self):
widgets = []
widgets.append(self.simStats)
if self.robot is not None:
widgets.append(self.robot.getStatsWidget())
self.parent.setStatsWidgets(widgets)
def placeRobot(self):
self.mouseActionType = Map.PlaceRobotAction
def placeTarget(self):
self.mouseActionType = Map.PlaceTargetAction