class QtPart(QtGui.QWidget):
accepted = QtCore.pyqtSignal(list)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
loadUi(os.path.join(path, 'resources', 'part.ui'), self)
self.pub_marker_array = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=1)
self.btnLoad.clicked.connect(self.btnLoadClicked)
self.btnProcessMesh.clicked.connect(self.btnProcessMeshClicked)
self.btnProcessLayer.clicked.connect(self.btnProcessLayerClicked)
self.btnAcceptPath.clicked.connect(self.btnAcceptPathClicked)
self.sbPositionX.valueChanged.connect(self.changePosition)
self.sbPositionY.valueChanged.connect(self.changePosition)
self.sbPositionZ.valueChanged.connect(self.changePosition)
self.sbSizeX.valueChanged.connect(self.changeSize)
self.sbSizeY.valueChanged.connect(self.changeSize)
self.sbSizeZ.valueChanged.connect(self.changeSize)
self.processing = False
self.timer = QtCore.QTimer(self)
self.timer.timeout.connect(self.updateProcess)
self.robpath = RobPath()
def btnLoadClicked(self):
self.blockSignals(True)
filename = QtGui.QFileDialog.getOpenFileName(
self, 'Open file', os.path.join(path, 'data'),
'Mesh Files (*.stl)')[0]
print 'Filename:', filename
self.setWindowTitle(filename)
self.robpath.load_mesh(filename)
self.mesh_size = self.robpath.mesh.bpoint2 - self.robpath.mesh.bpoint1
self.updatePosition(self.robpath.mesh.bpoint1) # Rename to position
self.updateSize(self.robpath.mesh.bpoint2 - self.robpath.mesh.bpoint1)
self.marker_array = MarkerArray()
#self.marker = MeshMarker(mesh_resource="file://"+filename, frame_id="/workobject")
self.mesh = TriangleListMarker()
self.mesh.set_frame('/workobject')
self.mesh.set_points(0.001 * np.vstack(self.robpath.mesh.triangles))
self.mesh.set_color((0.66, 0.66, 0.99, 0.66))
self.marker_array.markers.append(self.mesh.marker)
self.path = LinesMarker()
self.path.set_frame('/workobject')
self.path.set_color((1.0, 0.0, 0.0, 1.0))
self.marker_array.markers.append(self.path.marker)
for id, m in enumerate(self.marker_array.markers):
m.id = id
self.npoints = 0
# #rospy.loginfo()
self.pub_marker_array.publish(self.marker_array)
#TODO: Change bpoints.
#self.lblInfo.setText('Info:\n')
#self.plot.drawMesh(self.robpath.mesh)
#TODO: Add info from velocity estimation module.
self.blockSignals(False)
def updateParameters(self):
height = self.sbHeight.value()
width = self.sbWidth.value()
overlap = 0.01 * self.sbOverlap.value()
print height, width, overlap
self.robpath.set_track(height, width, overlap)
def updateProcess(self):
if self.robpath.k < len(self.robpath.levels):
self.robpath.update_process()
#self.plot.drawSlice(self.robpath.slices, self.robpath.path)
points = np.array(
[pose[0] for pose in self.robpath.path[self.npoints:-1]])
self.npoints = self.npoints + len(points)
self.path.set_points(0.001 * points)
print len(points)
self.pub_marker_array.publish(self.marker_array)
#self.plot.progress.setValue(100.0 * self.robpath.k / len(self.robpath.levels))
else:
self.processing = False
self.timer.stop()
def btnProcessMeshClicked(self):
if self.processing:
self.processing = False
self.timer.stop()
else:
self.updateParameters()
self.robpath.init_process()
self.processing = True
self.timer.start(100)
def btnProcessLayerClicked(self):
if self.processing:
self.updateParameters()
self.updateProcess()
else:
self.updateParameters()
self.robpath.init_process()
self.processing = True
def btnAcceptPathClicked(self):
self.accepted.emit(self.robpath.path)
def updatePosition(self, position):
x, y, z = position
self.sbPositionX.setValue(x)
self.sbPositionY.setValue(y)
self.sbPositionZ.setValue(z)
def changePosition(self):
x = self.sbPositionX.value()
y = self.sbPositionY.value()
z = self.sbPositionZ.value()
self.robpath.translate_mesh(np.float32([x, y, z]))
#self.marker.set_position((x, y, z))
self.mesh.set_points(0.001 * np.vstack(self.robpath.mesh.triangles))
self.pub_marker_array.publish(self.marker_array)
def updateSize(self, size):
sx, sy, sz = size
self.sbSizeX.setValue(sx)
self.sbSizeY.setValue(sy)
self.sbSizeZ.setValue(sz)
def changeSize(self):
sx = self.sbSizeX.value()
sy = self.sbSizeY.value()
sz = self.sbSizeZ.value()
self.robpath.resize_mesh(np.float32([sx, sy, sz]))
#scale = np.float32([sx, sy, sz]) / self.mesh_size
self.mesh.set_points(0.001 * np.vstack(self.robpath.mesh.triangles))
self.pub_marker_array.publish(self.marker_array)
def blockSignals(self, value):
self.sbPositionX.blockSignals(value)
self.sbPositionY.blockSignals(value)
self.sbPositionZ.blockSignals(value)
self.sbSizeX.blockSignals(value)
self.sbSizeY.blockSignals(value)
self.sbSizeZ.blockSignals(value)
class RobPathUI(QtGui.QMainWindow):
def __init__(self):
super(RobPathUI, self).__init__()
path = rospkg.RosPack().get_path('etna_planning')
loadUi(os.path.join(path, 'resources', 'robviz.ui'), self)
self.boxPlot.addWidget(MyViz())
self.btnLoad.clicked.connect(self.btnLoadClicked)
self.btnProcessMesh.clicked.connect(self.btnProcessMeshClicked)
self.btnSaveRapid.clicked.connect(self.btnSaveRapidClicked)
self.btnRecord.clicked.connect(self.btnRecordClicked)
self.sbSpeed.valueChanged.connect(self.changeSpeed)
self.sbPower.valueChanged.connect(self.changePower)
self.sbPositionX.valueChanged.connect(self.changePosition)
self.sbPositionY.valueChanged.connect(self.changePosition)
self.sbPositionZ.valueChanged.connect(self.changePosition)
self.sbSizeX.valueChanged.connect(self.changeSize)
self.sbSizeY.valueChanged.connect(self.changeSize)
self.sbSizeZ.valueChanged.connect(self.changeSize)
self.btnQuit.clicked.connect(self.btnQuitClicked)
self.publisher = rospy.Publisher('visualization_marker',
Marker,
queue_size=1)
self.recording = False
cloud_topic = rospy.get_param('~cloud',
'/camera/cloud') # cameara/points
rospy.Subscriber(cloud_topic,
PointCloud2,
self.callback_point_cloud,
queue_size=1)
self.listener = tf.TransformListener()
#rospy.spin()
self.processing = False
self.timer = QtCore.QTimer(self.boxPlot)
self.timer.timeout.connect(self.updateProcess)
self.robpath = RobPath()
def changeSpeed(self):
speed = self.Window.sbSpeed.value()
self.robpath.set_speed(speed)
def changePower(self):
power = self.Window.sbPower.value()
self.robpath.set_power(power)
def updatePosition(self, position):
x, y, z = position
self.sbPositionX.setValue(x)
self.sbPositionY.setValue(y)
self.sbPositionZ.setValue(z)
def updateSize(self, size):
sx, sy, sz = size
self.sbSizeX.setValue(sx)
self.sbSizeY.setValue(sy)
self.sbSizeZ.setValue(sz)
def updateProcess(self):
if self.robpath.k < len(self.robpath.levels):
self.robpath.update_process()
#self.plot.drawSlice(self.robpath.slices, self.robpath.path)
#self.plot.progress.setValue(100.0 * self.robpath.k / len(self.robpath.levels))
else:
self.processing = False
self.timer.stop()
def blockSignals(self, value):
self.sbPositionX.blockSignals(value)
self.sbPositionY.blockSignals(value)
self.sbPositionZ.blockSignals(value)
self.sbSizeX.blockSignals(value)
self.sbSizeY.blockSignals(value)
self.sbSizeZ.blockSignals(value)
def btnLoadClicked(self):
self.blockSignals(True)
filename = QtGui.QFileDialog.getOpenFileName(self, 'Open file', './',
'Mesh Files (*.stl)')[0]
print 'Filename:', filename
self.robpath.load_mesh(filename)
self.updatePosition(self.robpath.mesh.bpoint1) # Rename to position
self.updateSize(self.robpath.mesh.bpoint2 -
self.robpath.mesh.bpoint1) # Change by size
#self.marker = MeshMarker(mesh_resource="file://"+filename, frame_id="/workobject")
self.marker = TriangleListMarker(frame_id="/workobject")
self.marker.set_points(0.001 * np.vstack(self.robpath.mesh.triangles))
self.marker.set_color((0.75, 0.25, 0.25, 0.5))
# #rospy.loginfo()
# self.marker.set_position((0, 0, 0))
# self.marker.set_scale(scale=(0.001, 0.001, 0.001))
self.publisher.publish(self.marker.marker)
#self.Window.setWindowTitle('Mesh Viewer: %s' %filename)
#self.robpath.load_mesh(filename)
## -----
## TODO: Change bpoints.
#self.updatePosition(self.robpath.mesh.bpoint1) # Rename to position
#self.updateSize(self.robpath.mesh.bpoint2 - self.robpath.mesh.bpoint1) # Change by size
#self.Window.lblInfo.setText('Info:\n')
## -----
#self.plot.drawMesh(self.robpath.mesh)
self.blockSignals(False)
def btnProcessMeshClicked(self):
if self.processing:
self.processing = False
self.timer.stop()
else:
self.processing = True
self.timer.start(100)
def point_cloud_to_world(self, stamp, points3d):
"""Transforms the point cloud in camera coordinates to the world frame."""
self.listener.waitForTransform("/world", "/camera0", stamp,
rospy.Duration(1.0))
(position,
quaternion) = self.listener.lookupTransform("/world", "/camera0",
stamp)
matrix = tf.transformations.quaternion_matrix(quaternion)
matrix[:3, 3] = position
points = np.zeros((len(points3d), 3), dtype=np.float32)
for k, point3d in enumerate(points3d):
point = np.ones(4)
point[:3] = point3d
points[k] = np.dot(matrix, point)[:3]
return points
def callback_point_cloud(self, data):
if self.recording:
cloud_msg = data
stamp = data.header.stamp
points = pc2.read_points(cloud_msg, skip_nans=False)
points3d = []
for point in points:
points3d.append(point)
points3d = np.float32(points3d)
rospy.loginfo(points3d)
#TODO: Record only when the camera is moving.
points3d = self.point_cloud_to_world(stamp, points3d)
with open('test.xyz', 'a') as f:
np.savetxt(f, points3d, fmt='%.6f')
def btnRecordClicked(self):
if self.recording:
print 'Stop record'
self.recording = False
else:
print 'Recording...'
with open('test.xyz', 'w') as f:
pass
self.recording = True
def changePosition(self):
x = 0.001 * self.sbPositionX.value()
y = 0.001 * self.sbPositionY.value()
z = 0.001 * self.sbPositionZ.value()
#self.mesh.translate(position)
self.marker.set_position((x, y, z))
#TODO: Change mesh position. Include offset position.
self.publisher.publish(self.marker.marker)
def changeSize(self):
sx = self.sbSizeX.value()
sy = self.sbSizeY.value()
sz = self.sbSizeZ.value()
self.robpath.resize_mesh(np.float32([sx, sy, sz]))
#TODO: Scale the marker or change the mesh loaded in the mesh.
self.changePosition()
def btnProcessMeshClicked(self):
if self.processing:
self.processing = False
self.timer.stop()
else:
height = self.Window.sbHeight.value()
width = self.Window.sbWidth.value()
overlap = 0.01 * self.Window.sbOverlap.value()
self.robpath.set_track(height, width, overlap)
self.plot.drawWorkingArea()
self.robpath.init_process()
self.processing = True
self.timer.start(100)
def btnSaveRapidClicked(self):
#filename = QtGui.QFileDialog.getOpenFileName(self.plot, 'Save file', './',
# 'Rapid Modules (*.mod)')[0]
self.robpath.save_rapid()
def btnQuitClicked(self):
QtCore.QCoreApplication.instance().quit()
class QtPart(QtGui.QWidget):
accepted = QtCore.pyqtSignal(list)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
loadUi(os.path.join(path, 'resources', 'part.ui'), self)
self.pub_marker_array = rospy.Publisher(
'visualization_marker_array', MarkerArray, queue_size=1)
self.btnLoad.clicked.connect(self.btnLoadClicked)
self.btnProcessMesh.clicked.connect(self.btnProcessMeshClicked)
self.btnProcessLayer.clicked.connect(self.btnProcessLayerClicked)
self.btnAcceptPath.clicked.connect(self.btnAcceptPathClicked)
self.sbPositionX.valueChanged.connect(self.changePosition)
self.sbPositionY.valueChanged.connect(self.changePosition)
self.sbPositionZ.valueChanged.connect(self.changePosition)
self.sbSizeX.valueChanged.connect(self.changeSize)
self.sbSizeY.valueChanged.connect(self.changeSize)
self.sbSizeZ.valueChanged.connect(self.changeSize)
self.processing = False
self.timer = QtCore.QTimer(self)
self.timer.timeout.connect(self.updateProcess)
self.robpath = RobPath()
def btnLoadClicked(self):
self.blockSignals(True)
filename = QtGui.QFileDialog.getOpenFileName(
self, 'Open file', os.path.join(path, 'data'), 'Mesh Files (*.stl)')[0]
print 'Filename:', filename
self.setWindowTitle(filename)
self.robpath.load_mesh(filename)
self.mesh_size = self.robpath.mesh.bpoint2 - self.robpath.mesh.bpoint1
self.updatePosition(self.robpath.mesh.bpoint1) # Rename to position
self.updateSize(self.robpath.mesh.bpoint2 - self.robpath.mesh.bpoint1)
self.marker_array = MarkerArray()
#self.marker = MeshMarker(mesh_resource="file://"+filename, frame_id="/workobject")
self.mesh = TriangleListMarker()
self.mesh.set_frame('/workobject')
self.mesh.set_points(0.001 * np.vstack(self.robpath.mesh.triangles))
self.mesh.set_color((0.66, 0.66, 0.99, 0.66))
self.marker_array.markers.append(self.mesh.marker)
self.path = LinesMarker()
self.path.set_frame('/workobject')
self.path.set_color((1.0, 0.0, 0.0, 1.0))
self.marker_array.markers.append(self.path.marker)
for id, m in enumerate(self.marker_array.markers):
m.id = id
self.npoints = 0
# #rospy.loginfo()
self.pub_marker_array.publish(self.marker_array)
#TODO: Change bpoints.
#self.lblInfo.setText('Info:\n')
#self.plot.drawMesh(self.robpath.mesh)
#TODO: Add info from velocity estimation module.
self.blockSignals(False)
def updateParameters(self):
height = self.sbHeight.value()
width = self.sbWidth.value()
overlap = 0.01 * self.sbOverlap.value()
print height, width, overlap
self.robpath.set_track(height, width, overlap)
def updateProcess(self):
if self.robpath.k < len(self.robpath.levels):
self.robpath.update_process()
#self.plot.drawSlice(self.robpath.slices, self.robpath.path)
points = np.array([pose[0] for pose in self.robpath.path[self.npoints:-1]])
self.npoints = self.npoints + len(points)
self.path.set_points(0.001 * points)
print len(points)
self.pub_marker_array.publish(self.marker_array)
#self.plot.progress.setValue(100.0 * self.robpath.k / len(self.robpath.levels))
else:
self.processing = False
self.timer.stop()
def btnProcessMeshClicked(self):
if self.processing:
self.processing = False
self.timer.stop()
else:
self.updateParameters()
self.robpath.init_process()
self.processing = True
self.timer.start(100)
def btnProcessLayerClicked(self):
if self.processing:
self.updateParameters()
self.updateProcess()
else:
self.updateParameters()
self.robpath.init_process()
self.processing = True
def btnAcceptPathClicked(self):
self.accepted.emit(self.robpath.path)
def updatePosition(self, position):
x, y, z = position
self.sbPositionX.setValue(x)
self.sbPositionY.setValue(y)
self.sbPositionZ.setValue(z)
def changePosition(self):
x = self.sbPositionX.value()
y = self.sbPositionY.value()
z = self.sbPositionZ.value()
self.robpath.translate_mesh(np.float32([x, y, z]))
#self.marker.set_position((x, y, z))
self.mesh.set_points(0.001 * np.vstack(self.robpath.mesh.triangles))
self.pub_marker_array.publish(self.marker_array)
def updateSize(self, size):
sx, sy, sz = size
self.sbSizeX.setValue(sx)
self.sbSizeY.setValue(sy)
self.sbSizeZ.setValue(sz)
def changeSize(self):
sx = self.sbSizeX.value()
sy = self.sbSizeY.value()
sz = self.sbSizeZ.value()
self.robpath.resize_mesh(np.float32([sx, sy, sz]))
#scale = np.float32([sx, sy, sz]) / self.mesh_size
self.mesh.set_points(0.001 * np.vstack(self.robpath.mesh.triangles))
self.pub_marker_array.publish(self.marker_array)
def blockSignals(self, value):
self.sbPositionX.blockSignals(value)
self.sbPositionY.blockSignals(value)
self.sbPositionZ.blockSignals(value)
self.sbSizeX.blockSignals(value)
self.sbSizeY.blockSignals(value)
self.sbSizeZ.blockSignals(value)
class RobPathUI(QtGui.QMainWindow):
def __init__(self):
super(RobPathUI, self).__init__()
path = rospkg.RosPack().get_path('etna_planning')
loadUi(os.path.join(path, 'resources', 'robviz.ui'), self)
self.boxPlot.addWidget(MyViz())
self.btnLoad.clicked.connect(self.btnLoadClicked)
self.btnProcessMesh.clicked.connect(self.btnProcessMeshClicked)
self.btnSaveRapid.clicked.connect(self.btnSaveRapidClicked)
self.btnRecord.clicked.connect(self.btnRecordClicked)
self.sbSpeed.valueChanged.connect(self.changeSpeed)
self.sbPower.valueChanged.connect(self.changePower)
self.sbPositionX.valueChanged.connect(self.changePosition)
self.sbPositionY.valueChanged.connect(self.changePosition)
self.sbPositionZ.valueChanged.connect(self.changePosition)
self.sbSizeX.valueChanged.connect(self.changeSize)
self.sbSizeY.valueChanged.connect(self.changeSize)
self.sbSizeZ.valueChanged.connect(self.changeSize)
self.btnQuit.clicked.connect(self.btnQuitClicked)
self.publisher = rospy.Publisher('visualization_marker', Marker, queue_size=1)
self.recording = False
cloud_topic = rospy.get_param('~cloud', '/camera/cloud') # cameara/points
rospy.Subscriber(cloud_topic, PointCloud2, self.callback_point_cloud, queue_size=1)
self.listener = tf.TransformListener()
#rospy.spin()
self.processing = False
self.timer = QtCore.QTimer(self.boxPlot)
self.timer.timeout.connect(self.updateProcess)
self.robpath = RobPath()
def changeSpeed(self):
speed = self.Window.sbSpeed.value()
self.robpath.set_speed(speed)
def changePower(self):
power = self.Window.sbPower.value()
self.robpath.set_power(power)
def updatePosition(self, position):
x, y, z = position
self.sbPositionX.setValue(x)
self.sbPositionY.setValue(y)
self.sbPositionZ.setValue(z)
def updateSize(self, size):
sx, sy, sz = size
self.sbSizeX.setValue(sx)
self.sbSizeY.setValue(sy)
self.sbSizeZ.setValue(sz)
def updateProcess(self):
if self.robpath.k < len(self.robpath.levels):
self.robpath.update_process()
#self.plot.drawSlice(self.robpath.slices, self.robpath.path)
#self.plot.progress.setValue(100.0 * self.robpath.k / len(self.robpath.levels))
else:
self.processing = False
self.timer.stop()
def blockSignals(self, value):
self.sbPositionX.blockSignals(value)
self.sbPositionY.blockSignals(value)
self.sbPositionZ.blockSignals(value)
self.sbSizeX.blockSignals(value)
self.sbSizeY.blockSignals(value)
self.sbSizeZ.blockSignals(value)
def btnLoadClicked(self):
self.blockSignals(True)
filename = QtGui.QFileDialog.getOpenFileName(self, 'Open file', './',
'Mesh Files (*.stl)')[0]
print 'Filename:', filename
self.robpath.load_mesh(filename)
self.updatePosition(self.robpath.mesh.bpoint1) # Rename to position
self.updateSize(self.robpath.mesh.bpoint2 - self.robpath.mesh.bpoint1) # Change by size
#self.marker = MeshMarker(mesh_resource="file://"+filename, frame_id="/workobject")
self.marker = TriangleListMarker(frame_id="/workobject")
self.marker.set_points(0.001 * np.vstack(self.robpath.mesh.triangles))
self.marker.set_color((0.75,0.25,0.25,0.5))
# #rospy.loginfo()
# self.marker.set_position((0, 0, 0))
# self.marker.set_scale(scale=(0.001, 0.001, 0.001))
self.publisher.publish(self.marker.marker)
#self.Window.setWindowTitle('Mesh Viewer: %s' %filename)
#self.robpath.load_mesh(filename)
## -----
## TODO: Change bpoints.
#self.updatePosition(self.robpath.mesh.bpoint1) # Rename to position
#self.updateSize(self.robpath.mesh.bpoint2 - self.robpath.mesh.bpoint1) # Change by size
#self.Window.lblInfo.setText('Info:\n')
## -----
#self.plot.drawMesh(self.robpath.mesh)
self.blockSignals(False)
def btnProcessMeshClicked(self):
if self.processing:
self.processing = False
self.timer.stop()
else:
self.processing = True
self.timer.start(100)
def point_cloud_to_world(self, stamp, points3d):
"""Transforms the point cloud in camera coordinates to the world frame."""
self.listener.waitForTransform("/world", "/camera0", stamp, rospy.Duration(1.0))
(position, quaternion) = self.listener.lookupTransform("/world", "/camera0", stamp)
matrix = tf.transformations.quaternion_matrix(quaternion)
matrix[:3,3] = position
points = np.zeros((len(points3d), 3), dtype=np.float32)
for k, point3d in enumerate(points3d):
point = np.ones(4)
point[:3] = point3d
points[k] = np.dot(matrix, point)[:3]
return points
def callback_point_cloud(self, data):
if self.recording:
cloud_msg = data
stamp = data.header.stamp
points = pc2.read_points(cloud_msg, skip_nans=False)
points3d = []
for point in points:
points3d.append(point)
points3d = np.float32(points3d)
rospy.loginfo(points3d)
#TODO: Record only when the camera is moving.
points3d = self.point_cloud_to_world(stamp, points3d)
with open('test.xyz', 'a') as f:
np.savetxt(f, points3d, fmt='%.6f')
def btnRecordClicked(self):
if self.recording:
print 'Stop record'
self.recording = False
else:
print 'Recording...'
with open('test.xyz', 'w') as f: pass
self.recording = True
def changePosition(self):
x = 0.001 * self.sbPositionX.value()
y = 0.001 * self.sbPositionY.value()
z = 0.001 * self.sbPositionZ.value()
#self.mesh.translate(position)
self.marker.set_position((x, y, z))
#TODO: Change mesh position. Include offset position.
self.publisher.publish(self.marker.marker)
def changeSize(self):
sx = self.sbSizeX.value()
sy = self.sbSizeY.value()
sz = self.sbSizeZ.value()
self.robpath.resize_mesh(np.float32([sx, sy, sz]))
#TODO: Scale the marker or change the mesh loaded in the mesh.
self.changePosition()
def btnProcessMeshClicked(self):
if self.processing:
self.processing = False
self.timer.stop()
else:
height = self.Window.sbHeight.value()
width = self.Window.sbWidth.value()
overlap = 0.01 * self.Window.sbOverlap.value()
self.robpath.set_track(height, width, overlap)
self.plot.drawWorkingArea()
self.robpath.init_process()
self.processing = True
self.timer.start(100)
def btnSaveRapidClicked(self):
#filename = QtGui.QFileDialog.getOpenFileName(self.plot, 'Save file', './',
# 'Rapid Modules (*.mod)')[0]
self.robpath.save_rapid()
def btnQuitClicked(self):
QtCore.QCoreApplication.instance().quit()