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)
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)