Exemplo n.º 1
0
    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)
Exemplo n.º 2
0
    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)
Exemplo n.º 3
0
    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)
Exemplo n.º 4
0
    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)
Exemplo n.º 5
0
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)
Exemplo n.º 6
0
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)
Exemplo n.º 7
0
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()
Exemplo n.º 8
0
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()