def make_icon(self, image_list, mode=QIcon.Normal, state=QIcon.On):
"""
Helper function to create QIcons from lists of image files
Warning: svg files interleaved with other files will not render correctly
:param image_list: list of image paths to layer into an icon.
:type image: list of str
:param mode: The mode of the QIcon to be created.
:type mode: int
:param state: the state of the QIcon to be created.
:type state: int
"""
if type(image_list) is not list:
image_list = [image_list]
if len(image_list) <= 0:
raise TypeError('The list of images is empty.')
num_svg = 0
for item in image_list:
if item[-4:].lower() == '.svg':
num_svg = num_svg + 1
if num_svg != len(image_list):
# Legacy support for non-svg images
icon_pixmap = QPixmap()
icon_pixmap.load(image_list[0])
painter = QPainter(icon_pixmap)
for item in image_list[1:]:
painter.drawPixmap(0, 0, QPixmap(item))
icon = QIcon()
icon.addPixmap(icon_pixmap, mode, state)
painter.end()
return icon
else:
# rendering SVG files into a QImage
renderer = QSvgRenderer(image_list[0])
icon_image = QImage(renderer.defaultSize(), QImage.Format_ARGB32)
icon_image.fill(0)
painter = QPainter(icon_image)
renderer.render(painter)
if len(image_list) > 1:
for item in image_list[1:]:
renderer.load(item)
renderer.render(painter)
painter.end()
# Convert QImage into a pixmap to create the icon
icon_pixmap = QPixmap()
icon_pixmap.convertFromImage(icon_image)
icon = QIcon(icon_pixmap)
return icon
def make_icon(self, image_list, mode=QIcon.Normal, state=QIcon.On):
"""
Helper function to create QIcons from lists of image files
Warning: svg files interleaved with other files will not render correctly
:param image_list: list of image paths to layer into an icon.
:type image: list of str
:param mode: The mode of the QIcon to be created.
:type mode: int
:param state: the state of the QIcon to be created.
:type state: int
"""
if type(image_list) is not list:
image_list = [image_list]
if len(image_list) <= 0:
raise TypeError('The list of images is empty.')
num_svg = 0
for item in image_list:
if item[-4:].lower() == '.svg':
num_svg = num_svg + 1
if num_svg != len(image_list):
# Legacy support for non-svg images
icon_pixmap = QPixmap()
icon_pixmap.load(image_list[0])
painter = QPainter(icon_pixmap)
for item in image_list[1:]:
painter.drawPixmap(0, 0, QPixmap(item))
icon = QIcon()
icon.addPixmap(icon_pixmap, mode, state)
painter.end()
return icon
else:
# rendering SVG files into a QImage
renderer = QSvgRenderer(image_list[0])
icon_image = QImage(renderer.defaultSize(), QImage.Format_ARGB32)
icon_image.fill(0)
painter = QPainter(icon_image)
renderer.render(painter)
if len(image_list) > 1:
for item in image_list[1:]:
renderer.load(item)
renderer.render(painter)
painter.end()
# Convert QImage into a pixmap to create the icon
icon_pixmap = QPixmap()
icon_pixmap.convertFromImage(icon_image)
icon = QIcon(icon_pixmap)
return icon
class DroneTeleop(Plugin):
def __init__(self, context):
super(DroneTeleop, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('DroneTeleop')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
# args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'DroneTeleop.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('DroneTeleopUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add logo
pixmap = QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'jderobot.png'))
self._widget.img_logo.setPixmap(pixmap.scaled(121, 121))
# Set Variables
self.play_code_flag = False
self.takeoff = False
self.linear_velocity_scaling_factor = 1
self.vertical_velocity_scaling_factor = 0.8
self.angular_velocity_scaling_factor = 0.5
self._widget.term_out.setReadOnly(True)
self._widget.term_out.setLineWrapMode(self._widget.term_out.NoWrap)
# Set functions for each GUI Item
self._widget.takeoffButton.clicked.connect(self.call_takeoff_land)
self._widget.playButton.clicked.connect(self.call_play)
self._widget.stopButton.clicked.connect(self.stop_drone)
# Add Publishers
self.takeoff_pub = rospy.Publisher('gui/takeoff_land',
Bool,
queue_size=1)
self.play_stop_pub = rospy.Publisher('gui/play_stop',
Bool,
queue_size=1)
self.twist_pub = rospy.Publisher('gui/twist', Twist, queue_size=1)
#Add global variables
self.shared_twist_msg = Twist()
self.current_pose = Pose()
self.current_twist = Twist()
self.stop_icon = QIcon()
self.stop_icon.addPixmap(
QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'stop.png')), QIcon.Normal, QIcon.Off)
self.play_icon = QIcon()
self.play_icon.addPixmap(
QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'play.png')), QIcon.Normal, QIcon.Off)
self.bridge = CvBridge()
self.teleop_stick_1 = TeleopWidget(self, 'set_linear_xy', 151)
self._widget.tlLayout.addWidget(self.teleop_stick_1)
self.teleop_stick_1.setVisible(True)
self.teleop_stick_2 = TeleopWidget(self, 'set_alt_yawrate', 151)
self._widget.tlLayout_2.addWidget(self.teleop_stick_2)
self.teleop_stick_2.setVisible(True)
self.sensors_widget = SensorsWidget(self)
self._widget.sensorsCheck.stateChanged.connect(
self.show_sensors_widget)
# Add widget to the user interface
context.add_widget(self._widget)
# Add Subscibers
cam_frontal_topic = rospy.get_param('cam_frontal_topic',
'/iris/cam_frontal/image_raw')
cam_ventral_topic = rospy.get_param('cam_ventral_topic',
'/iris/cam_ventral/image_raw')
rospy.Subscriber(cam_frontal_topic, Image, self.cam_frontal_cb)
rospy.Subscriber(cam_ventral_topic, Image, self.cam_ventral_cb)
rospy.Subscriber('interface/filtered_img', Image, self.filtered_img_cb)
rospy.Subscriber('interface/threshed_img', Image, self.threshed_img_cb)
rospy.Subscriber('mavros/local_position/pose', PoseStamped,
self.pose_stamped_cb)
rospy.Subscriber('mavros/local_position/velocity_body', TwistStamped,
self.twist_stamped_cb)
def show_sensors_widget(self, state):
if state == Qt.Checked:
self.sensors_widget.show()
else:
self.sensors_widget.hide()
def msg_to_pixmap(self, msg):
cv_img = self.bridge.imgmsg_to_cv2(msg)
h, w, _ = cv_img.shape
bytesPerLine = 3 * w
q_img = QImage(cv_img.data, w, h, bytesPerLine, QImage.Format_RGB888)
return QPixmap.fromImage(q_img).scaled(320, 240)
def cam_frontal_cb(self, msg):
self._widget.img_frontal.setPixmap(self.msg_to_pixmap(msg))
self.sensors_widget.sensorsUpdate.emit()
def cam_ventral_cb(self, msg):
self._widget.img_ventral.setPixmap(self.msg_to_pixmap(msg))
def threshed_img_cb(self, msg):
self._widget.img_threshed.setPixmap(self.msg_to_pixmap(msg))
def filtered_img_cb(self, msg):
self._widget.img_filtered.setPixmap(self.msg_to_pixmap(msg))
def pose_stamped_cb(self, msg):
self.current_pose = msg.pose
def twist_stamped_cb(self, msg):
self.current_twist = msg.twist
def call_takeoff_land(self):
if self.takeoff == True:
self._widget.takeoffButton.setText("Take Off")
rospy.loginfo('Landing')
self._widget.term_out.append('Landing')
self.takeoff_pub.publish(Bool(False))
self.takeoff = False
else:
self._widget.takeoffButton.setText("Land")
rospy.loginfo('Taking off')
self._widget.term_out.append('Taking off')
self.takeoff_pub.publish(Bool(True))
self.takeoff = True
def call_play(self):
if not self.play_code_flag:
self._widget.playButton.setText('Stop Code')
self._widget.playButton.setStyleSheet("background-color: #ec7063")
self._widget.playButton.setIcon(self.stop_icon)
rospy.loginfo('Executing student code')
self._widget.term_out.append('Executing student code')
self.play_stop_pub.publish(Bool(True))
self.play_code_flag = True
else:
self._widget.playButton.setText('Play Code')
self._widget.playButton.setStyleSheet("background-color: #7dcea0")
self._widget.playButton.setIcon(self.play_icon)
rospy.loginfo('Stopping student code')
self._widget.term_out.append('Stopping student code')
self.play_stop_pub.publish(Bool(False))
self.play_code_flag = False
def stop_drone(self):
self._widget.term_out.append('Stopping Drone')
rospy.loginfo('Stopping Drone')
self.teleop_stick_1.stop()
self.teleop_stick_2.stop()
if self.play_code_flag:
self.call_play()
for i in range(5):
self.shared_twist_msg = Twist()
self.twist_pub.publish(self.shared_twist_msg)
rospy.sleep(0.05)
def set_linear_xy(self, u, v):
x = -self.linear_velocity_scaling_factor * v
y = -self.linear_velocity_scaling_factor * u
self._widget.XValue.setText('%.2f' % x)
self._widget.YValue.setText('%.2f' % y)
rospy.logdebug('Stick 2 value changed to - x: %.2f y: %.2f', x, y)
self.shared_twist_msg.linear.x = x
self.shared_twist_msg.linear.y = y
self.twist_pub.publish(self.shared_twist_msg)
def set_alt_yawrate(self, u, v):
az = -self.vertical_velocity_scaling_factor * u
z = -self.angular_velocity_scaling_factor * v
self._widget.rotValue.setText('%.2f' % az)
self._widget.altdValue.setText('%.2f' % z)
rospy.logdebug('Stick 1 value changed to - az: %.2f z: %.2f', az, z)
self.shared_twist_msg.linear.z = z
self.shared_twist_msg.angular.z = az
self.twist_pub.publish(self.shared_twist_msg)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class GroundRobotTeleop(Plugin):
def __init__(self, context):
super(GroundRobotTeleop, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('GroundRobotTeleop')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
# args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path(
'rqt_ground_robot_teleop'), 'resource', 'GroundRobotTeleop.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('GroundRobotTeleopUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(
self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add logo
pixmap = QPixmap(os.path.join(rospkg.RosPack().get_path(
'rqt_ground_robot_teleop'), 'resource', 'jderobot.png'))
self._widget.img_logo.setPixmap(pixmap.scaled(121, 121))
# Set Variables
self.linear_velocity_scaling_factor = 1
self.angular_velocity_scaling_factor = 0.5
# Set functions for each GUI Item
self._widget.stop_button.clicked.connect(self.stop_robot)
# Add Publishers
self.twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
#Add global variables
self.twist_msg = Twist()
self.stop_icon = QIcon()
self.stop_icon.addPixmap(QPixmap(os.path.join(rospkg.RosPack().get_path(
'rqt_ground_robot_teleop'), 'resource', 'stop.png')), QIcon.Normal, QIcon.Off)
self.play_icon = QIcon()
self.play_icon.addPixmap(QPixmap(os.path.join(rospkg.RosPack().get_path(
'rqt_ground_robot_teleop'), 'resource', 'play.png')), QIcon.Normal, QIcon.Off)
self.bridge = CvBridge()
self.teleop = TeleopWidget(self, 'set_twist', 311)
self._widget.teleop_layout.addWidget(self.teleop)
self.teleop.setVisible(True)
# Add widget to the user interface
context.add_widget(self._widget)
# Add Subscibers
rospy.Subscriber('camera/rgb/image_raw', Image, self.cam_cb)
def msg_to_pixmap(self, msg):
cv_img = self.bridge.imgmsg_to_cv2(msg)
h, w, _ = cv_img.shape
bytesPerLine = 3 * w
q_img = QImage(cv_img.data, w, h, bytesPerLine, QImage.Format_RGB888)
return QPixmap.fromImage(q_img).scaled(320, 240)
def cam_cb(self, msg):
self._widget.img_camera.setPixmap(self.msg_to_pixmap(msg))
def stop_robot(self):
rospy.loginfo('Stopping Robot')
self.teleop.stop()
for i in range(5):
self.twist_msg = Twist()
self.twist_pub.publish(self.twist_msg)
rospy.sleep(0.05)
def set_twist(self, u, v):
az = -self.linear_velocity_scaling_factor * u
x = -self.angular_velocity_scaling_factor * v
self._widget.rot_value.setText('%.2f' % az)
self._widget.x_value.setText('%.2f' % x)
self.twist_msg.linear.x = x
self.twist_msg.angular.z = az
self.twist_pub.publish(self.twist_msg)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class PosTeleop(Plugin):
def __init__(self, context):
super(PosTeleop, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('PosTeleop')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'PositionTeleop.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('PosTeleopUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add logo
pixmap = QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'jderobot.png'))
self._widget.img_logo.setPixmap(pixmap.scaled(121, 121))
# Set Variables
self.play_code_flag = False
self.takeoff = False
self._widget.term_out.setReadOnly(True)
self._widget.term_out.setLineWrapMode(self._widget.term_out.NoWrap)
# Set functions for each GUI Item
self._widget.takeoffButton.clicked.connect(self.call_takeoff_land)
self._widget.stopButton.clicked.connect(self.stop_drone)
self._widget.playButton.clicked.connect(self.call_play)
self._widget.posControlButton.clicked.connect(self.set_pos)
# Add Publishers
self.play_stop_pub = rospy.Publisher('gui/play_stop',
Bool,
queue_size=1)
# Add global variables
self.extended_state = ExtendedState()
self.shared_pose_msg = Pose()
self.shared_twist_msg = Twist()
self.current_pose = Pose()
self.pose_frame = ''
self.current_twist = Twist()
self.twist_frame = ''
self.is_running = True
self.stop_icon = QIcon()
self.stop_icon.addPixmap(
QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'stop.png')), QIcon.Normal, QIcon.Off)
self.play_icon = QIcon()
self.play_icon.addPixmap(
QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'play.png')), QIcon.Normal, QIcon.Off)
self.sensors_widget = SensorsWidget(self)
self._widget.sensorsCheck.stateChanged.connect(
self.show_sensors_widget)
# Add widget to the user interface
context.add_widget(self._widget)
# Add Subscribers
rospy.Subscriber('drone_wrapper/local_position/pose', PoseStamped,
self.pose_stamped_cb)
rospy.Subscriber('drone_wrapper/local_position/velocity_body',
TwistStamped, self.twist_stamped_cb)
rospy.Subscriber('drone_wrapper/extended_state', ExtendedState,
self.extended_state_cb)
# Add Timer
self.update_status_info()
def show_sensors_widget(self, state):
if state == Qt.Checked:
self.sensors_widget.show()
else:
self.sensors_widget.hide()
def pose_stamped_cb(self, msg):
self.current_pose = msg.pose
self.pose_frame = msg.header.frame_id
self.sensors_widget.sensorsUpdate.emit()
def twist_stamped_cb(self, msg):
self.current_twist = msg.twist
self.twist_frame = msg.header.frame_id
def update_status_info(self):
threading.Timer(0.5, self.update_status_info).start()
if self.is_running:
self.set_info_pos(self.current_pose, self.pose_frame)
self.set_info_vel(self.current_twist, self.twist_frame)
def set_info_pos(self, pose, frame):
self._widget.posX.setText(str(round(pose.position.x, 2)))
self._widget.posY.setText(str(round(pose.position.y, 2)))
self._widget.posZ.setText(str(round(pose.position.z, 2)))
self._widget.posFrame.setText(str(frame))
def set_info_vel(self, twist, frame):
self._widget.velX.setText(str(round(twist.linear.x, 2)))
self._widget.velY.setText(str(round(twist.linear.y, 2)))
self._widget.velZ.setText(str(round(twist.linear.z, 2)))
self._widget.velYaw.setText(str(round(twist.angular.z, 2)))
self._widget.velFrame.setText(str(frame))
def extended_state_cb(self, msg):
if self.extended_state.landed_state != msg.landed_state:
self.extended_state = msg
if self.extended_state.landed_state == 1: # ON GROUND
self._widget.takeoffButton.setText("Take Off")
elif self.extended_state.landed_state == 2: # IN AIR
self._widget.takeoffButton.setText("Land")
def takeoff_drone(self):
try:
global drone
drone.takeoff()
self.takeoff = True
finally:
rospy.loginfo('Takeoff finished')
def call_takeoff_land(self):
if self.extended_state.landed_state == 0: # UNDEFINED --> not ready
self._widget.term_out.append('Drone not ready')
return
if self.takeoff == True:
rospy.loginfo('Landing')
self._widget.term_out.append('Landing')
global drone
drone.land()
self.takeoff = False
else:
rospy.loginfo('Taking off')
self._widget.term_out.append('Taking off')
x = threading.Thread(target=self.takeoff_drone)
x.daemon = True
x.start()
def stop_drone(self):
self._widget.term_out.append('Stopping Drone')
rospy.loginfo('Stopping Drone')
if self.play_code_flag:
self.call_play()
for i in range(5):
self.shared_twist_msg = Twist()
global drone
drone.set_cmd_vel(self.shared_twist_msg.linear.x,
self.shared_twist_msg.linear.y,
self.shared_twist_msg.linear.z,
self.shared_twist_msg.angular.z)
rospy.sleep(0.05)
def call_play(self):
if not self.play_code_flag:
self._widget.playButton.setText('Stop Code')
self._widget.playButton.setStyleSheet("background-color: #ec7063")
self._widget.playButton.setIcon(self.stop_icon)
rospy.loginfo('Executing student code')
self._widget.term_out.append('Executing student code')
self.play_stop_pub.publish(Bool(True))
self.play_code_flag = True
else:
self._widget.playButton.setText('Play Code')
self._widget.playButton.setStyleSheet("background-color: #7dcea0")
self._widget.playButton.setIcon(self.play_icon)
rospy.loginfo('Stopping student code')
self._widget.term_out.append('Stopping student code')
self.play_stop_pub.publish(Bool(False))
self.play_code_flag = False
def set_pos(self):
x = float(self._widget.x_control.text())
y = float(self._widget.y_control.text())
z = float(self._widget.z_control.text())
rospy.loginfo('Going to {0} {1} {2}'.format(x, y, z))
self._widget.term_out.append('Going to {0} {1} {2}'.format(x, y, z))
self.shared_pose_msg.position.x = x
self.shared_pose_msg.position.y = y
self.shared_pose_msg.position.z = z
global drone
drone.set_cmd_pos(self.shared_pose_msg.position.x,
self.shared_pose_msg.position.y,
self.shared_pose_msg.position.z)
def shutdown_plugin(self):
# TODO unregister all publishers here
self.is_running = False
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
