def test_scale_path(self): discrete_traj = [(0,0), (2, 2), (7, 1)] discrete_test_traj = [(0,0), (4, 1), (14,0.5)] scaled_traj = Trajectory.scale(discrete_traj, 2, 0.5) for (x, y) , (test_x, test_y) in zip(scaled_traj, discrete_test_traj): self.assertAlmostEqual(x, test_x, delta=0.01) self.assertAlmostEqual(y, test_y, delta=0.01)
def test_scale_path(self): discrete_traj = [(0, 0), (2, 2), (7, 1)] discrete_test_traj = [(0, 0), (4, 1), (14, 0.5)] scaled_traj = Trajectory.scale(discrete_traj, 2, 0.5) for (x, y), (test_x, test_y) in zip(scaled_traj, discrete_test_traj): self.assertAlmostEqual(x, test_x, delta=0.01) self.assertAlmostEqual(y, test_y, delta=0.01)
def test_get_normalized_path(self): traj = Trajectory() test_path = Path(Line(start=(0+0j), end=(2+2j)), Line(start=(2+2j), end=(7+1j)), Line(start=(7+1j), end=(0+0j))) expected_path = Trajectory.scale(Trajectory.discretize(Path(Line(start=(0+0j), end=(2+2j)), Line(start=(2+2j), end=(7+1j)), Line(start=(7+1j), end=(0+0j))), 1), 1/7.0, 1/2.0) traj.paths.append(test_path) normalized_trajectory = traj.get_normalized_path(0) self.assertGreater(len(normalized_trajectory), 3) for (x1, y1), (x2, y2) in zip(normalized_trajectory, expected_path): self.assertAlmostEqual(x1, x2, delta=0.01) self.assertAlmostEqual(y1, y2, delta=0.01)
def test_get_normalized_path(self): traj = Trajectory() test_path = Path(Line(start=(0 + 0j), end=(2 + 2j)), Line(start=(2 + 2j), end=(7 + 1j)), Line(start=(7 + 1j), end=(0 + 0j))) expected_path = Trajectory.scale( Trajectory.discretize( Path(Line(start=(0 + 0j), end=(2 + 2j)), Line(start=(2 + 2j), end=(7 + 1j)), Line(start=(7 + 1j), end=(0 + 0j))), 1), 1 / 7.0, 1 / 2.0) traj.paths.append(test_path) normalized_trajectory = traj.get_normalized_path(0) self.assertGreater(len(normalized_trajectory), 3) for (x1, y1), (x2, y2) in zip(normalized_trajectory, expected_path): self.assertAlmostEqual(x1, x2, delta=0.01) self.assertAlmostEqual(y1, y2, delta=0.01)
class TrajectoryWidget(WorkspaceWidget, CoreXYEventListener): def __init__(self, parent, machine): super(TrajectoryWidget, self).__init__(parent, machine) self.name = "TrajectoryWidget" self.icon = os.path.join(os.path.realpath(os.path.dirname(__file__)), 'resources', 'icons', 'format-text-direction-ltr.png') self.tooltip = "Trajectory workspace" self.machine.add_listener(self) self.trajectory = Trajectory() self.trajectory_controller = TrajectoryControllerThread() self.limits = None # References to widgets self.calibrationFileButton = None self.calibrationFileLineEdit = None self.pointsFileButton = None self.pointsFileLineEdit = None self.loadButton = None self.graphicsView = None self.trajectoryComboBox = None self.indexComboBox = None self.stepSpinBox = None self.xScaleSpinBox = None self.yScaleSpinBox = None self.xOffsetSpinBox = None self.yOffsetSpinBox = None self.stopButton = None self.runButton = None # Image-related self.pixmap = QtGui.QPixmap() self.original_rect = None self.loadUI() self.update() def loadUI(self): # Load UI file main_widget = load_ui(os.path.join(os.path.realpath(os.path.dirname(__file__)), 'TrajectoryWidget.ui'), self) layout = QtGui.QVBoxLayout() layout.addWidget(main_widget) self.setLayout(layout) # Find references to controls self.calibrationFileButton = self.findChild(QtGui.QToolButton, "calibrationFileButton") self.pointsFileButton = self.findChild(QtGui.QToolButton, "pointsFileButton") self.loadButton = self.findChild(QtGui.QPushButton, "loadButton") self.stopButton = self.findChild(QtGui.QPushButton, "stopButton") self.runButton = self.findChild(QtGui.QPushButton, "runButton") self.calibrationFileLineEdit = self.findChild(QtGui.QLineEdit, "calibrationFileLineEdit") self.pointsFileLineEdit = self.findChild(QtGui.QLineEdit, "pointsFileLineEdit") self.graphicsView = self.findChild(QtGui.QGraphicsView, "graphicsView") self.trajectoryComboBox = self.findChild(QtGui.QComboBox, "trajectoryComboBox") self.indexComboBox = self.findChild(QtGui.QComboBox, "indexComboBox") self.stepSpinBox = self.findChild(QtGui.QDoubleSpinBox, "stepSpinBox") self.xScaleSpinBox = self.findChild(QtGui.QDoubleSpinBox, "xScaleSpinBox") self.yScaleSpinBox = self.findChild(QtGui.QDoubleSpinBox, "yScaleSpinBox") self.xOffsetSpinBox = self.findChild(QtGui.QDoubleSpinBox, "xOffsetSpinBox") self.yOffsetSpinBox = self.findChild(QtGui.QDoubleSpinBox, "yOffsetSpinBox") # Set spinBox limits: float_max = sys.float_info.max self.stepSpinBox.setMaximum(float_max) self.xScaleSpinBox.setMaximum(float_max) self.yScaleSpinBox.setMaximum(float_max) self.xOffsetSpinBox.setMaximum(float_max) self.yOffsetSpinBox.setMaximum(float_max) self.stepSpinBox.setMinimum(0) self.xScaleSpinBox.setMinimum(0) self.yScaleSpinBox.setMinimum(0) self.xOffsetSpinBox.setMinimum(-float_max) self.yOffsetSpinBox.setMinimum(-float_max) # Connect signals self.calibrationFileButton.clicked.connect(self.onCalibrationButtonClicked) self.pointsFileButton.clicked.connect(self.onPointsButtonClicked) self.loadButton.clicked.connect(self.onLoadButtonClicked) self.stopButton.clicked.connect(self.onStopButtonClicked) self.runButton.clicked.connect(self.onRunButtonClicked) self.trajectoryComboBox.currentIndexChanged.connect(self.onTrajectorySelectedChanged) self.indexComboBox.currentIndexChanged.connect(self.updateImage) self.stepSpinBox.valueChanged.connect(self.updateImage) self.xOffsetSpinBox.valueChanged.connect(self.updateImage) self.yOffsetSpinBox.valueChanged.connect(self.updateImage) self.xScaleSpinBox.valueChanged.connect(self.updateImage) self.yScaleSpinBox.valueChanged.connect(self.updateImage) self.runButton.setEnabled(False) self.stopButton.setEnabled(False) self.setTrajectoryControlsEnabled(False) self.trajectory_controller.finished.connect(self.onTrajectoryFinish) def update(self): self.updateImage() # def resetInputValues(self): # self.stepSpinBox.setValue(1) # self.xOffsetSpinBox.setValue(0) # self.yOffsetSpinBox.setValue(0) # self.xScaleSpinBox.setValue(1) # self.yScaleSpinBox.setValue(1) def resetInputValues(self): self.stepSpinBox.setValue(20) self.xOffsetSpinBox.setValue(0) self.yOffsetSpinBox.setValue(-10) self.xScaleSpinBox.setValue(0.8) self.yScaleSpinBox.setValue(0.8) @staticmethod def _currentComboBoxIndex(combobox): try: return combobox.findText(combobox.currentText()) except (IndexError, AttributeError) as e: print str(e) return None def loadImage(self, calibration): if calibration.image_name: self.pixmap.loadFromData(calibration.image, os.path.splitext(calibration.image_name)[1]) self.original_rect = self.pixmap.rect() self.pixmap = self.pixmap.scaled(480, 339) return True else: return False def calculateTrajectoryFromParams(self): # Get data from form current_trajectory = self._currentComboBoxIndex(self.trajectoryComboBox) current_starting_point = self._currentComboBoxIndex(self.indexComboBox) step = self.stepSpinBox.value() x_scale, y_scale = self.xScaleSpinBox.value(), self.yScaleSpinBox.value() x_offset, y_offset = self.xOffsetSpinBox.value(), self.yOffsetSpinBox.value() # Transform trajectory try: discrete_trajectory = self.trajectory.get_normalized_path(current_trajectory, current_starting_point, step) print "Path length: %d points" % len(discrete_trajectory) discrete_trajectory = self.trajectory.scale(discrete_trajectory, 480*x_scale, 339*y_scale) discrete_trajectory = self.trajectory.translate(discrete_trajectory, x_offset, y_offset) except ArithmeticError, e: print e return False return discrete_trajectory
class TrajectoryWidget(WorkspaceWidget, CoreXYEventListener): def __init__(self, parent, machine): super(TrajectoryWidget, self).__init__(parent, machine) self.name = "TrajectoryWidget" self.icon = os.path.join(os.path.realpath(os.path.dirname(__file__)), 'resources', 'icons', 'format-text-direction-ltr.png') self.tooltip = "Trajectory workspace" self.machine.add_listener(self) self.trajectory = Trajectory() self.trajectory_controller = TrajectoryControllerThread() self.limits = None # References to widgets self.calibrationFileButton = None self.calibrationFileLineEdit = None self.pointsFileButton = None self.pointsFileLineEdit = None self.loadButton = None self.graphicsView = None self.trajectoryComboBox = None self.indexComboBox = None self.stepSpinBox = None self.xScaleSpinBox = None self.yScaleSpinBox = None self.xOffsetSpinBox = None self.yOffsetSpinBox = None self.stopButton = None self.runButton = None # Image-related self.pixmap = QtGui.QPixmap() self.original_rect = None self.loadUI() self.update() def loadUI(self): # Load UI file main_widget = load_ui( os.path.join(os.path.realpath(os.path.dirname(__file__)), 'TrajectoryWidget.ui'), self) layout = QtGui.QVBoxLayout() layout.addWidget(main_widget) self.setLayout(layout) # Find references to controls self.calibrationFileButton = self.findChild(QtGui.QToolButton, "calibrationFileButton") self.pointsFileButton = self.findChild(QtGui.QToolButton, "pointsFileButton") self.loadButton = self.findChild(QtGui.QPushButton, "loadButton") self.stopButton = self.findChild(QtGui.QPushButton, "stopButton") self.runButton = self.findChild(QtGui.QPushButton, "runButton") self.calibrationFileLineEdit = self.findChild( QtGui.QLineEdit, "calibrationFileLineEdit") self.pointsFileLineEdit = self.findChild(QtGui.QLineEdit, "pointsFileLineEdit") self.graphicsView = self.findChild(QtGui.QGraphicsView, "graphicsView") self.trajectoryComboBox = self.findChild(QtGui.QComboBox, "trajectoryComboBox") self.indexComboBox = self.findChild(QtGui.QComboBox, "indexComboBox") self.stepSpinBox = self.findChild(QtGui.QDoubleSpinBox, "stepSpinBox") self.xScaleSpinBox = self.findChild(QtGui.QDoubleSpinBox, "xScaleSpinBox") self.yScaleSpinBox = self.findChild(QtGui.QDoubleSpinBox, "yScaleSpinBox") self.xOffsetSpinBox = self.findChild(QtGui.QDoubleSpinBox, "xOffsetSpinBox") self.yOffsetSpinBox = self.findChild(QtGui.QDoubleSpinBox, "yOffsetSpinBox") # Set spinBox limits: float_max = sys.float_info.max self.stepSpinBox.setMaximum(float_max) self.xScaleSpinBox.setMaximum(float_max) self.yScaleSpinBox.setMaximum(float_max) self.xOffsetSpinBox.setMaximum(float_max) self.yOffsetSpinBox.setMaximum(float_max) self.stepSpinBox.setMinimum(0) self.xScaleSpinBox.setMinimum(0) self.yScaleSpinBox.setMinimum(0) self.xOffsetSpinBox.setMinimum(-float_max) self.yOffsetSpinBox.setMinimum(-float_max) # Connect signals self.calibrationFileButton.clicked.connect( self.onCalibrationButtonClicked) self.pointsFileButton.clicked.connect(self.onPointsButtonClicked) self.loadButton.clicked.connect(self.onLoadButtonClicked) self.stopButton.clicked.connect(self.onStopButtonClicked) self.runButton.clicked.connect(self.onRunButtonClicked) self.trajectoryComboBox.currentIndexChanged.connect( self.onTrajectorySelectedChanged) self.indexComboBox.currentIndexChanged.connect(self.updateImage) self.stepSpinBox.valueChanged.connect(self.updateImage) self.xOffsetSpinBox.valueChanged.connect(self.updateImage) self.yOffsetSpinBox.valueChanged.connect(self.updateImage) self.xScaleSpinBox.valueChanged.connect(self.updateImage) self.yScaleSpinBox.valueChanged.connect(self.updateImage) self.runButton.setEnabled(False) self.stopButton.setEnabled(False) self.setTrajectoryControlsEnabled(False) self.trajectory_controller.finished.connect(self.onTrajectoryFinish) def update(self): self.updateImage() # def resetInputValues(self): # self.stepSpinBox.setValue(1) # self.xOffsetSpinBox.setValue(0) # self.yOffsetSpinBox.setValue(0) # self.xScaleSpinBox.setValue(1) # self.yScaleSpinBox.setValue(1) def resetInputValues(self): self.stepSpinBox.setValue(20) self.xOffsetSpinBox.setValue(0) self.yOffsetSpinBox.setValue(-10) self.xScaleSpinBox.setValue(0.8) self.yScaleSpinBox.setValue(0.8) @staticmethod def _currentComboBoxIndex(combobox): try: return combobox.findText(combobox.currentText()) except (IndexError, AttributeError) as e: print str(e) return None def loadImage(self, calibration): if calibration.image_name: self.pixmap.loadFromData( calibration.image, os.path.splitext(calibration.image_name)[1]) self.original_rect = self.pixmap.rect() self.pixmap = self.pixmap.scaled(480, 339) return True else: return False def calculateTrajectoryFromParams(self): # Get data from form current_trajectory = self._currentComboBoxIndex( self.trajectoryComboBox) current_starting_point = self._currentComboBoxIndex(self.indexComboBox) step = self.stepSpinBox.value() x_scale, y_scale = self.xScaleSpinBox.value( ), self.yScaleSpinBox.value() x_offset, y_offset = self.xOffsetSpinBox.value( ), self.yOffsetSpinBox.value() # Transform trajectory try: discrete_trajectory = self.trajectory.get_normalized_path( current_trajectory, current_starting_point, step) print "Path length: %d points" % len(discrete_trajectory) discrete_trajectory = self.trajectory.scale( discrete_trajectory, 480 * x_scale, 339 * y_scale) discrete_trajectory = self.trajectory.translate( discrete_trajectory, x_offset, y_offset) except ArithmeticError, e: print e return False return discrete_trajectory