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