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