Пример #1
0
class UInode(QtGui.QMainWindow):
    def __init__(self):
        super(UInode, self).__init__()

        self._ap_topic = rospy.Publisher('/apctrl', Empty, queue_size=5)
        self.webcam_shutdown_pub = rospy.Publisher('/webcam/shutdown',
                                                   Empty,
                                                   queue_size=5)

        self.swap_red_blue = rospy.get_param('~swap_red_blue', False)

        self.controller = DroneController(
            offline_timeout=rospy.get_param('~connection_check_period', 500))

        self.keymap = self.gen_keymap()
        # Without release action
        self.keymap2 = self.gen_keymap2()

        self.messages = Messages(
            rospy.get_param('~message_display_time', 5000), *grid)

        self.messages_named_template = re.compile(
            r'((?P<name>[a-zA-Z0-9_-]+)::)?(?P<message>.*)')

        self.setWindowTitle('ARdrone camera')
        self.image_box = QtGui.QLabel(self)
        self.setCentralWidget(self.image_box)

        self.image = None
        self.image_lock = Lock()

        fps = rospy.get_param('~fps', 50)

        self.redraw_timer = QtCore.QTimer(self)
        self.redraw_timer.timeout.connect(self.on_redraw)
        self.redraw_timer.start(1000 / fps)

        rospy.Subscriber('/ui/message', String, self.on_ui_request)
        rospy.Subscriber('/out/image', Image, self.on_video_update)

    def on_ui_request(self, message):
        """Process the message show request

        We have spetial `ui/message` topic where any node can send
        any message and that message will be displayed.

        By default, messages are displayed for a while and than hidden.

        Messages which match the mask `([a-zA-Z0-9_-])::(.*)` will be displayed
        permanently. Newer messages will overwrite older messages
        with the same name.

        """
        match = self.messages_named_template.match(message.data)
        self.messages.message_put(**match.groupdict())

    def on_video_update(self, data):
        """On each frame we save new picture for future rendering"""
        self.communication_since_timer = True

        image = QtGui.QImage(data.data, data.width, data.height,
                             QtGui.QImage.Format_RGB888)
        if self.swap_red_blue:
            image = QtGui.QImage.rgbSwapped(image)

        with self.image_lock:
            self.image = image

    def on_redraw(self):
        """Redraw interface"""
        image = None
        with self.image_lock:
            if self.image is not None:
                image = QtGui.QPixmap.fromImage(self.image)
            else:
                image = QtGui.QPixmap(640, 360)
                image.fill(QtGui.QColor(50, 50, 50))

        self.messages.messages_put((
            (self.controller.status.readable(), 'drone.status'),
            (self.controller.battery, 'drone.battery'),
        ))

        self.messages.render(image)

        self.resize(image.width(), image.height())
        self.image_box.setPixmap(image)

    def gen_keymap2(self):
        return {
            QtCore.Qt.Key.Key_M: lambda ax, e: self.controller.enable_cv(),
            QtCore.Qt.Key.Key_N:
            lambda ax, e: self.controller.enable_controller(),
        }

    def gen_keymap(self):
        return {
            QtCore.Qt.Key.Key_R:
            lambda ax, e: self.controller.reset(),
            QtCore.Qt.Key.Key_T:
            lambda ax, e: self.controller.takeoff(),
            QtCore.Qt.Key.Key_L:
            lambda ax, e: self.controller.land(),
            QtCore.Qt.Key.Key_C:
            lambda ax, e: self.controller.change_camera(),
            QtCore.Qt.Key.Key_M:
            lambda ax, e: self.controller.enable_cv(),
            QtCore.Qt.Key.Key_N:
            lambda ax, e: self.controller.enable_controller(),
            QtCore.Qt.Key.Key_F1:
            lambda ax, e: self.controller.increaseP(),
            QtCore.Qt.Key.Key_F2:
            lambda ax, e: self.controller.decreaseP(),
            QtCore.Qt.Key.Key_F3:
            lambda ax, e: self.controller.increaseI(),
            QtCore.Qt.Key.Key_F4:
            lambda ax, e: self.controller.decreaseI(),
            QtCore.Qt.Key.Key_F5:
            lambda ax, e: self.controller.increaseD(),
            QtCore.Qt.Key.Key_F6:
            lambda ax, e: self.controller.decreaseD(),
            QtCore.Qt.Key.Key_H:
            lambda ax, e: self.controller.hover(),
            QtCore.Qt.Key.Key_A:
            lambda ax, e: self.controller.send_vel(y=ax),
            QtCore.Qt.Key.Key_D:
            lambda ax, e: self.controller.send_vel(y=-ax),
            QtCore.Qt.Key.Key_W:
            lambda ax, e: self.controller.send_vel(x=ax),
            QtCore.Qt.Key.Key_S:
            lambda ax, e: self.controller.send_vel(x=-ax),
            QtCore.Qt.Key.Key_Q:
            lambda ax, e: self.controller.send_vel(yaw=ax),
            QtCore.Qt.Key.Key_E:
            lambda ax, e: self.controller.send_vel(yaw=-ax),
            QtCore.Qt.Key.Key_BracketRight:
            lambda ax, e: self.controller.send_vel(z=ax),
            QtCore.Qt.Key.Key_BracketLeft:
            lambda ax, e: self.controller.send_vel(z=-ax),
            QtCore.Qt.Key.Key_Y:
            lambda ax, e: self._ap_topic.publish(Empty()) if ax != 0 else None,
        }

    def keyPressEvent(self, event):
        key = event.key()

        if event.isAutoRepeat() or self.controller is None:
            return

        if key in self.keymap:
            self.keymap[key](1, event)

    def keyReleaseEvent(self, event):
        key = event.key()

        if event.isAutoRepeat() or self.controller is None:
            return

        if key in self.keymap and key not in self.keymap2:
            self.keymap[key](0, event)
Пример #2
0
class UInode(QtGui.QMainWindow):
    def __init__(self):
        super(UInode, self).__init__()

        self._ap_topic = rospy.Publisher('/apctrl', Empty,
                                         queue_size=5)

        self.swap_red_blue = rospy.get_param('~swap_red_blue', False)

        self.controller = DroneController(
            offline_timeout=rospy.get_param('~connection_check_period', 500))

        self.keymap = self.gen_keymap()

        self.messages = Messages(
            rospy.get_param('~message_display_time', 5000), *grid)

        self.messages_named_template = re.compile(
            r'((?P<name>[a-zA-Z0-9_-]+)::)?(?P<message>.*)')

        self.setWindowTitle('ARdrone camera')
        self.image_box = QtGui.QLabel(self)
        self.setCentralWidget(self.image_box)

        self.image = None
        self.image_lock = Lock()

        fps = rospy.get_param('~fps', 50)

        self.redraw_timer = QtCore.QTimer(self)
        self.redraw_timer.timeout.connect(self.on_redraw)
        self.redraw_timer.start(1000 / fps)

        rospy.Subscriber('/ui/message', String, self.on_ui_request)
        rospy.Subscriber('/in/image', Image, self.on_video_update)

    def on_ui_request(self, message):
        """Process the message show request

        We have spetial `ui/message` topic where any node can send
        any message and that message will be displayed.

        By default, messages are displayed for a while and than hidden.

        Messages which match the mask `([a-zA-Z0-9_-])::(.*)` will be displayed
        permanently. Newer messages will overwrite older messages
        with the same name.

        """
        match = self.messages_named_template.match(message.data)
        self.messages.message_put(**match.groupdict())

    def on_video_update(self, data):
        """On each frame we save new picture for future rendering"""
        self.communication_since_timer = True

        image = QtGui.QImage(data.data,
                             data.width,
                             data.height,
                             QtGui.QImage.Format_RGB888)
        if self.swap_red_blue:
            image = QtGui.QImage.rgbSwapped(image)

        with self.image_lock:
            self.image = image

    def on_redraw(self):
        """Redraw interface"""
        image = None
        with self.image_lock:
            if self.controller.is_online and self.image is not None:
                image = QtGui.QPixmap.fromImage(self.image)
            else:
                image = QtGui.QPixmap(640, 360)
                image.fill(QtGui.QColor(50, 50, 50))

        self.messages.messages_put((
            (self.controller.status.readable(), 'drone.status'),
            (self.controller.battery, 'drone.battery'),
        ))

        self.messages.render(image)

        self.resize(image.width(), image.height())
        self.image_box.setPixmap(image)

    def gen_keymap(self):
        return {
            QtCore.Qt.Key.Key_R: lambda ax, e: self.controller.reset(),
            QtCore.Qt.Key.Key_T: lambda ax, e: self.controller.takeoff(),
            QtCore.Qt.Key.Key_L: lambda ax, e: self.controller.land(),
            QtCore.Qt.Key.Key_H: lambda ax, e: self.controller.hover(),
            QtCore.Qt.Key.Key_A: lambda ax, e: self.controller.send_vel(y=ax),
            QtCore.Qt.Key.Key_D: lambda ax, e: self.controller.send_vel(y=-ax),
            QtCore.Qt.Key.Key_W: lambda ax, e: self.controller.send_vel(x=ax),
            QtCore.Qt.Key.Key_S: lambda ax, e: self.controller.send_vel(x=-ax),
            QtCore.Qt.Key.Key_Q: lambda ax, e: self.controller.send_vel(yaw=ax),
            QtCore.Qt.Key.Key_E: lambda ax, e: self.controller.send_vel(yaw=-ax),
            QtCore.Qt.Key.Key_BracketRight: lambda ax, e: self.controller.send_vel(z=ax),
            QtCore.Qt.Key.Key_BracketLeft: lambda ax, e: self.controller.send_vel(z=-ax),
            QtCore.Qt.Key.Key_Y: lambda ax, e: self._ap_topic.publish(Empty()) if ax != 0 else None,
        }

    def keyPressEvent(self, event):
        key = event.key()

        if event.isAutoRepeat() or self.controller is None:
            return

        if key in self.keymap:
            self.keymap[key](1, event)

    def keyReleaseEvent(self, event):
        key = event.key()

        if event.isAutoRepeat() or self.controller is None:
            return

        if key in self.keymap:
            self.keymap[key](0, event)
Пример #3
0
class UInode(QtGui.QMainWindow):
    keypressChecker = 0

    def __init__(self):
        super(UInode, self).__init__()

        self._ap_topic = rospy.Publisher('/apctrl', Empty, queue_size=5)
        self.webcam_shutdown_pub = rospy.Publisher('/webcam/shutdown',
                                                   Empty,
                                                   queue_size=5)

        self.swap_red_blue = rospy.get_param('~swap_red_blue', False)

        self.controller = DroneController(
            offline_timeout=rospy.get_param('~connection_check_period', 500))

        self.keymap = self.gen_keymap()
        # Without release action
        self.keymap2 = self.gen_keymap2()

        self.messages = Messages(
            rospy.get_param('~message_display_time', 5000), *grid)

        self.messages_named_template = re.compile(
            r'((?P<name>[a-zA-Z0-9_-]+)::)?(?P<message>.*)')

        self.setWindowTitle('ARdrone camera')
        self.image_box = QtGui.QLabel(self)
        self.setCentralWidget(self.image_box)

        self.image = None
        self.image_lock = Lock()

        fps = rospy.get_param('~fps', 50)

        self.redraw_timer = QtCore.QTimer(self)
        self.redraw_timer.timeout.connect(self.on_redraw)
        self.redraw_timer.start(1000 / fps)

        rospy.Subscriber('/ui/message', String, self.on_ui_request)
        rospy.Subscriber('/out/image', Image, self.on_video_update)

    def on_ui_request(self, message):
        """Process the message show request

        We have spetial `ui/message` topic where any node can send
        any message and that message will be displayed.

        By default, messages are displayed for a while and than hidden.

        Messages which match the mask `([a-zA-Z0-9_-])::(.*)` will be displayed
        permanently. Newer messages will overwrite older messages
        with the same name.

        """
        match = self.messages_named_template.match(message.data)
        self.messages.message_put(**match.groupdict())

    def on_video_update(self, data):
        """On each frame we save new picture for future rendering"""
        self.communication_since_timer = True

        image = QtGui.QImage(data.data, data.width, data.height,
                             QtGui.QImage.Format_RGB888)
        if self.swap_red_blue:
            image = QtGui.QImage.rgbSwapped(image)

        with self.image_lock:
            self.image = image

    def on_redraw(self):
        """Redraw interface"""
        image = None
        with self.image_lock:
            if self.image is not None:
                image = QtGui.QPixmap.fromImage(self.image)
            else:
                image = QtGui.QPixmap(640, 360)
                image.fill(QtGui.QColor(50, 50, 50))

        self.messages.messages_put((
            (self.controller.status.readable(), 'drone.status'),
            (self.controller.battery, 'drone.battery'),
        ))

        self.messages.render(image)

        self.resize(image.width(), image.height())
        self.image_box.setPixmap(image)

    """Homing function that checks continuously if the Euclidiean distance of marker to drone is greater or less than the setError. If greater, moves the drone closer to the marker position"""

    def Homing(self):
        """keypressChecker is used to assure that the function only happens once on a keypress. A keypress always yields two calls normally."""
        global keypressChecker
        keypressChecker = keypressChecker + 1
        if (keypressChecker == 2):
            global xdistance
            global ydistance
            """ setError is the maximum distance the drone can be away from the marker to be considered at the marker """
            setError = 0.05
            while (CheckHypo() > setError):
                if (xdistance > 0 and ydistance > 0):
                    time.sleep(.3)
                    self.controller.send_vel(-.0005, -.0005, 0, 0)
                    print("GO RIGHT FORWARD")
                if (xdistance > 0 and ydistance < 0):
                    time.sleep(.3)
                    self.controller.send_vel(-.0005, .0005, 0, 0)
                    print("GO RIGHT BACKWARD")
                if (xdistance < 0 and ydistance > 0):
                    time.sleep(.3)
                    self.controller.send_vel(.0005, -.0005, 0, 0)
                    print("GO LEFT FORWARD")
                if (xdistance < 0 and ydistance < 0):
                    time.sleep(.3)
                    self.controller.send_vel(.0005, .0005, 0, 0)
                    print("GO LEFT BACKWARD")
            keypressChecker = 0
            self.controller.send_vel(0, 0, 0, 0)
            self.controller.land()

    def gen_keymap2(self):
        return {
            QtCore.Qt.Key.Key_M: lambda ax, e: self.controller.enable_cv(),
            QtCore.Qt.Key.Key_N:
            lambda ax, e: self.controller.enable_controller(),
        }

    def gen_keymap(self):
        return {
            QtCore.Qt.Key.Key_R:
            lambda ax, e: self.controller.reset(),
            QtCore.Qt.Key.Key_T:
            lambda ax, e: self.controller.takeoff(),
            QtCore.Qt.Key.Key_L:
            lambda ax, e: self.controller.land(),
            QtCore.Qt.Key.Key_C:
            lambda ax, e: self.controller.change_camera(),
            QtCore.Qt.Key.Key_M:
            lambda ax, e: self.controller.enable_cv(),
            QtCore.Qt.Key.Key_N:
            lambda ax, e: self.controller.enable_controller(),
            QtCore.Qt.Key.Key_F1:
            lambda ax, e: self.controller.increaseP(),
            QtCore.Qt.Key.Key_F2:
            lambda ax, e: self.controller.decreaseP(),
            QtCore.Qt.Key.Key_F3:
            lambda ax, e: self.controller.increaseI(),
            QtCore.Qt.Key.Key_F4:
            lambda ax, e: self.controller.decreaseI(),
            QtCore.Qt.Key.Key_F5:
            lambda ax, e: self.controller.increaseD(),
            QtCore.Qt.Key.Key_F6:
            lambda ax, e: self.controller.decreaseD(),
            QtCore.Qt.Key.Key_H:
            lambda ax, e: self.controller.hover(),
            QtCore.Qt.Key.Key_A:
            lambda ax, e: self.controller.send_vel(y=ax),
            QtCore.Qt.Key.Key_D:
            lambda ax, e: self.controller.send_vel(y=-ax),
            QtCore.Qt.Key.Key_W:
            lambda ax, e: self.controller.send_vel(x=ax),
            QtCore.Qt.Key.Key_S:
            lambda ax, e: self.controller.send_vel(x=-ax),
            QtCore.Qt.Key.Key_Q:
            lambda ax, e: self.controller.send_vel(yaw=ax),
            QtCore.Qt.Key.Key_E:
            lambda ax, e: self.controller.send_vel(yaw=-ax),
            QtCore.Qt.Key.Key_BracketRight:
            lambda ax, e: self.controller.send_vel(z=ax),
            QtCore.Qt.Key.Key_BracketLeft:
            lambda ax, e: self.controller.send_vel(z=-ax),
            QtCore.Qt.Key.Key_Y:
            lambda ax, e: self._ap_topic.publish(Empty()) if ax != 0 else None,
            QtCore.Qt.Key.Key_J:
            lambda ax, e: self.Homing(),
        }

    def markerListener(self):
        print("This is going on:")
        rospy.Subscriber("visualization_marker", Marker, markerCallback)

    def droneListener(self):
        print("This is going on too!:")
        rospy.Subscriber("/orb/pose_scaled", PoseStamped, droneCallback)

    def keyPressEvent(self, event):
        key = event.key()

        if event.isAutoRepeat() or self.controller is None:
            return

        if key in self.keymap:
            self.keymap[key](1, event)

    def keyReleaseEvent(self, event):
        key = event.key()

        if event.isAutoRepeat() or self.controller is None:
            return

        if key in self.keymap and key not in self.keymap2:
            self.keymap[key](0, event)