class PropStatus(Plugin):
    def __init__(self, context):
        super(PropStatus, self).__init__(context)
        self.setObjectName('Status')

        self._widget = QWidget()
        loadUi(os.path.join(uipath, 'propagatorstatus.ui'), self._widget)
        context.add_widget(self._widget)
        
        rospy.Subscriber('/thrusters/info',ThrusterInfo,motordriver_callback)
        rospy.Subscriber('/skytraq_serial',SerialPacket,gps_callback)
        rospy.Subscriber('/imu/data_raw',Imu,imu_callback)
        rospy.Subscriber('/scan',LaserScan,lidar_callback)
        rospy.Subscriber('/mv_bluefox_camera_node/image_raw',Image,camera_callback)
        rospy.Timer(rospy.Duration(2),kill)

        self._update_timer = QTimer(self._widget)
        self._update_timer.timeout.connect(self._on_update)
        self._update_timer.start(100)

    def _on_update(self):
        self._widget.findChild(QListWidget, 'list').clear()
        
        for i in ['FR','FL','BR','BL','GPS','IMU','LIDAR','CAMERA']:
                pItem = QListWidgetItem(i);
                if i in active: 
                        pItem.setBackground(Qt.green); 
                else:
                        pItem.setBackground(Qt.red); 
                self._widget.findChild(QListWidget, 'list').addItem(pItem)
 def __init__(self, parent=None):
     super(XTermWidget, self).__init__(parent)
     self.setObjectName('XTermWidget')
     self._process = QProcess(self)
     self._process.finished.connect(self.close_signal)
     # let the widget finish init before embedding xterm
     QTimer.singleShot(100, self._embed_xterm)
class MicoButtonsWidget(QWidget):
    def __init__(self, widget):
        super(MicoButtonsWidget, self).__init__()
        rospkg_pack = rospkg.RosPack()
        ui_file = os.path.join(rospkg_pack.get_path('mico_controller'), 'resource', 'MicoButtons.ui')
        loadUi(ui_file, self)

        self.start_arm_srv = rospy.ServiceProxy('mico_arm_driver/in/start', MicoStart)
        self.stop_arm_srv = rospy.ServiceProxy('mico_arm_driver/in/stop', MicoStop)
        self.home_arm_srv = rospy.ServiceProxy('mico_arm_driver/in/home_arm', MicoHome)

        self._updateTimer = QTimer(self)
        self._updateTimer.timeout.connect(self.timeout_callback)

    def start(self):
        self._updateTimer.start(1000)  # loop rate[ms]

    def stop(self):
        self._updateTimer.stop()

    def timeout_callback(self):
        pass

    # rqt override
    def save_settings(self, plugin_settings, instance_settings):
        pass
        # instance_settings.set_value('topic_name', self._topic_name)

    def restore_settings(self, plugin_settings, instance_settings):
        pass
        # topic_name = instance_settings.value('topic_name')
        # try:
        # self._topic_name = eval(topic_name)
        # except Exception:
        # self._topic_name = self.TOPIC_NAME

    def shutdown_plugin(self):
        self.stop()

    @Slot()
    def on_qt_start_btn_clicked(self):
        rospy.loginfo(self.start_arm_srv())

    @Slot()
    def on_qt_stop_btn_clicked(self):
        rospy.loginfo(self.stop_arm_srv())

    @Slot()
    def on_qt_home_btn_clicked(self):
        rospy.loginfo(self.home_arm_srv())
 def __init__(self):
     super(StatusLightWidget, self).__init__()
     self.lock = Lock()
     self.status_sub = None
     self.status = 0
     self._status_topics = []
     self._update_topic_timer = QTimer(self)
     self._update_topic_timer.timeout.connect(self.updateTopics)
     self._update_topic_timer.start(1000)
     self._active_topic = None
     self._dialog = ComboBoxDialog()
     self._update_plot_timer = QTimer(self)
     self._update_plot_timer.timeout.connect(self.redraw)
     self._update_plot_timer.start(1000 / 15)
Beispiel #5
0
    def __init__(self, context):
        super(Control, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('Control')

        # 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('robosub'),
                               'src/rqt/rqt_control/resource', 'Control.ui')
        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)

        self.control_timer = QTimer(self)
        self.control_timer.timeout.connect(self.control_missed)
        self.control_timer.start(1000)

        self.control_status_timer = QTimer(self)
        self.control_status_timer.timeout.connect(self.control_status_missed)
        self.control_status_timer.start(1000)

        # Give QObjects reasonable names
        self._widget.setObjectName('Control')

        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))

        # Add widget to the user interface
        context.add_widget(self._widget)

        self._widget.statusActive.hide()
        self._widget.controlActive.hide()

        self.con_sub = rospy.Subscriber('control', control,
                                        self.control_callback, queue_size=1)
        self.cs_sub = rospy.Subscriber('control_status', control_status,
                                       self.control_status_callback,
                                       queue_size=1)
        img_file = os.path.join(rospkg.RosPack().get_path('robosub'),
                                'src/rqt/resource/robosub_logo.png')

        self._widget.setStyleSheet(".QWidget {background-image: url(" +
                                   img_file +
                                   "); background-repeat: no-repeat;" +
                                   "background-position:bottom right}")
Beispiel #6
0
class RosPlot(FigureCanvas):
    """Ultimately, this is a QWidget (as well as a FigureCanvasAgg, etc.)."""
    def __init__(self, parent, label_name,topic_name,topic_type,topic_field,buffer_size):
        
        self.label_name= label_name
        self.topic_type = topic_type
        self.topic_name = topic_name
        self.topic_field = topic_field
        self.buffer_size = buffer_size
        
        self.timer = QTimer()
        self.timer.setInterval(100)
        self.timer.timeout.connect(self.update_figure)
        
        fig = Figure(figsize=(5, 4), dpi=100)
        self.axes = fig.add_subplot(111)
        # We want the axes cleared every time plot() is called
        self.axes.hold(False)

        self.compute_initial_figure()
        self.buffer = collections.deque(maxlen=self.buffer_size)

        
        FigureCanvas.__init__(self, fig)
        self.setParent(parent)

        FigureCanvas.setSizePolicy(self,
                                   QtGui.QSizePolicy.Expanding,
                                   QtGui.QSizePolicy.Expanding)
        FigureCanvas.updateGeometry(self)
        
        self.subscriber = RosHelper.create_subscriber_from_type(self.topic_name,self.topic_type,self.on_message)
        self.timer.start()

    def compute_initial_figure(self):
        pass
    
    def on_message(self,msg):
        r = msg
        for subfields in self.topic_field.split(".")[1:]:
            r = getattr(r,subfields) 
        self.buffer.append(r)
        
    def update_figure(self):
        x = np.array(range(0,len(self.buffer)))
        y = np.array(self.buffer)
        self.axes.plot(x, y.T, 'r')
        self.draw()
Beispiel #7
0
    def __init__(self, context):
        super(Battery_state, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('Battery_state')

        # 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

        self._widget = QWidget()
        ui_file = os.path.join(rospkg.RosPack().get_path('rqt_battery_state'), 'resource', 'qt_gui3.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('qt_gui3')
        if context.serial_number() > 0:
            self._widget.setWindowTitle('Battery State')
        # Add widget to the user interface
        context.add_widget(self._widget)
        self._update_timer = QTimer(self)
        self._update_timer.timeout.connect(self._handle_update)
        self._update_timer.start(1000)#time step
        
        self.subscriber = rospy.Subscriber('/my_roomba/battery_state', BatteryState, self.subscriber_callback)
    def __init__(self, plugin):
        super(PoseViewWidget, self).__init__()
        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('rqt_pose_view'), 'resource', 'PoseViewWidget.ui')
        loadUi(ui_file, self)
        self._plugin = plugin

        self._position = (0.0, 0.0, 0.0)
        self._orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
        self._topic_name = None
        self._subscriber = None

        # create GL view
        self._gl_view = GLWidget()
        self._gl_view.setAcceptDrops(True)

        # backup and replace original paint method
        self._gl_view.paintGL_original = self._gl_view.paintGL
        self._gl_view.paintGL = self._gl_view_paintGL

        # backup and replace original mouse release method
        self._gl_view.mouseReleaseEvent_original = self._gl_view.mouseReleaseEvent
        self._gl_view.mouseReleaseEvent = self._gl_view_mouseReleaseEvent

        # add GL view to widget layout
        self.layout().addWidget(self._gl_view)

        # init and start update timer with 40ms (25fps)
        self._update_timer = QTimer(self)
        self._update_timer.timeout.connect(self.update_timeout)
        self._update_timer.start(40)
Beispiel #9
0
    def __init__(self, parent, label_name,topic_name,topic_type,topic_field,buffer_size):
        
        self.label_name= label_name
        self.topic_type = topic_type
        self.topic_name = topic_name
        self.topic_field = topic_field
        self.buffer_size = buffer_size
        
        self.timer = QTimer()
        self.timer.setInterval(100)
        self.timer.timeout.connect(self.update_figure)
        
        fig = Figure(figsize=(5, 4), dpi=100)
        self.axes = fig.add_subplot(111)
        # We want the axes cleared every time plot() is called
        self.axes.hold(False)

        self.compute_initial_figure()
        self.buffer = collections.deque(maxlen=self.buffer_size)

        
        FigureCanvas.__init__(self, fig)
        self.setParent(parent)

        FigureCanvas.setSizePolicy(self,
                                   QtGui.QSizePolicy.Expanding,
                                   QtGui.QSizePolicy.Expanding)
        FigureCanvas.updateGeometry(self)
        
        self.subscriber = RosHelper.create_subscriber_from_type(self.topic_name,self.topic_type,self.on_message)
        self.timer.start()
    def __init__(self, mainWidget):
        self.mainWidget = mainWidget
        # create GL view
        self._gl_view = mainWidget._gl_view
        self._gl_view.setAcceptDrops(True)
        self.resolution_meters = 50
        self._cross = None

        # backup and replace original paint method
        self._gl_view.paintGL_original = self._gl_view.paintGL
        self._gl_view.paintGL = self._gl_view_paintGL


        self._grid_layer = GridLayer(self.resolution_meters,self._gl_view)
        self._submarine_layer = SubmarineLayer(self.resolution_meters,self._gl_view)
        self._coor_layer = CoorSystemLayer(self._gl_view)
        self._target_layer = TargetLayer(self._gl_view)
        self._path_layer = PathLayer(self.resolution_meters,self._gl_view)

        self._layers = []
        self._layers.append(self._grid_layer)
        self._layers.append(self._submarine_layer)
        self._layers.append(self._coor_layer)
        self._layers.append(self._target_layer)
        self._layers.append(self._path_layer)

        # backup and replace original mouse release method
        self._gl_view.mouseReleaseEvent_original = self._gl_view.mouseReleaseEvent
        self._gl_view.mouseReleaseEvent = self.mainWidget._gl_view_mouseReleaseEvent

        # init and start update timer with 40ms (25fps)
        self._update_timer = QTimer(mainWidget)
        self._update_timer.timeout.connect(self.update_timeout)
        self._update_timer.start(100)
Beispiel #11
0
 def __init__(self, topics):
     super(HistogramPlotWidget, self).__init__()
     self.setObjectName('HistogramPlotWidget')
     rp = rospkg.RosPack()
     ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 
                            'resource', 'plot_histogram.ui')
     loadUi(ui_file, self)
     self.cv_bridge = CvBridge()
     self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
     self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
     self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
     self.data_plot = MatHistogramPlot(self)
     self.data_plot_layout.addWidget(self.data_plot)
     self._topic_completer = TopicCompleter(self.topic_edit)
     self._topic_completer.update_topics()
     self.topic_edit.setCompleter(self._topic_completer)
     self.data_plot.dropEvent = self.dropEvent
     self.data_plot.dragEnterEvent = self.dragEnterEvent
     self._start_time = rospy.get_time()
     self._rosdata = None
     if len(topics) != 0:
         self.subscribe_topic(topics)
     self._update_plot_timer = QTimer(self)
     self._update_plot_timer.timeout.connect(self.update_plot)
     self._update_plot_timer.start(self._redraw_interval)
    def __init__(self, context):
        """
        :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext''
        """
        super(Console, self).__init__(context)
        self.setObjectName('Console')

        self._rospack = rospkg.RosPack()

        self._model = MessageDataModel()
        self._proxy_model = MessageProxyModel()
        self._proxy_model.setSourceModel(self._model)

        self._widget = ConsoleWidget(self._proxy_model, self._rospack)
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
        context.add_widget(self._widget)

        # queue to store incoming data which get flushed periodically to the model
        # required since QSortProxyModel can not handle a high insert rate
        self._message_queue = []
        self._mutex = QMutex()
        self._timer = QTimer()
        self._timer.timeout.connect(self.insert_messages)
        self._timer.start(100)

        self._subscriber = None
        self._topic = '/rosout_agg'
        self._subscribe(self._topic)
Beispiel #13
0
  def __init__(self):
    print('constructor')
    # Init QWidget
    super(CalibWidget, self).__init__()
    self.setObjectName('CalibWidget')

    # load UI
    ui_file = os.path.join(rospkg.RosPack().get_path('dvs_calibration_gui'), 'resource', 'widget.ui')
    loadUi(ui_file, self)

    # init and start update timer for data, the timer calls the function update_info all 40ms    
    self._update_info_timer = QTimer(self)
    self._update_info_timer.timeout.connect(self.update_info)
    self._update_info_timer.start(40)

    self.button_reset.pressed.connect(self.on_button_reset_pressed)
    self.button_start.pressed.connect(self.on_button_start_calibration_pressed)
    self.button_save.pressed.connect(self.on_button_save_calibration_pressed)
    
    self._sub_pattern_detections = rospy.Subscriber('dvs_calibration/pattern_detections', Int32, self.pattern_detections_cb)    
    self._sub_calibration_output = rospy.Subscriber('dvs_calibration/output', String, self.calibration_output_cb)

    print('reset')

    self.on_button_reset_pressed()
    
    print('reset done')
    def __init__(self):
        super(BaseFilter, self).__init__()
        self._enabled = True

        self._timer = QTimer(self)
        self._timer.setSingleShot(True)
        self._timer.timeout.connect(self.filter_changed_signal.emit)
    def __init__(self, initial_topics=None, start_paused=False):
        super(PlotWidget, self).__init__()
        self.setObjectName('PlotWidget')

        self._initial_topics = initial_topics

        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('rqt_plot'), 'resource', 'plot.ui')
        loadUi(ui_file, self)
        self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add'))
        self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove'))
        self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
        self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
        self.data_plot = None

        self.subscribe_topic_button.setEnabled(False)
        if start_paused:
            self.pause_button.setChecked(True)

        self._topic_completer = TopicCompleter(self.topic_edit)
        self._topic_completer.update_topics()
        self.topic_edit.setCompleter(self._topic_completer)

        self._start_time = rospy.get_time()
        self._rosdata = {}
        self._remove_topic_menu = QMenu()

        # init and start update timer for plot
        self._update_plot_timer = QTimer(self)
        self._update_plot_timer.timeout.connect(self.update_plot)
    def __init__(self, plugin):
        super(Theta360ViewWidget, self).__init__()
        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('theta_viewer'), 'resource', 'Theta360ViewWidget.ui')
        loadUi(ui_file, self)
        self.plugin = plugin

        self.position = (0.0, 0.0, 0.0)
        self.orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
        self.topicName = None
        self.subscriber = None

        # create GL view
        self._glview = MyGLWidget()
        self._glview.setAcceptDrops(True)

        # backup and replace original paint method
        # self.glView.paintGL is callbacked from QGLWidget
        self._glview.paintGL_original = self._glview.paintGL
        self._glview.paintGL = self.glview_paintGL

        # backup and replace original mouse release method
        self._glview.mouseReleaseEvent_original = self._glview.mouseReleaseEvent
        self._glview.mouseReleaseEvent = self.glview_mouseReleaseEvent

        # add GL view to widget layout
        self.layout().addWidget(self._glview)

        # init and start update timer with 40ms (25fps)
        # updateTimeout is called with interval time
        self.update_timer = QTimer(self)
        self.update_timer.timeout.connect(self.update_timeout)
        self.update_timer.start(40)
        self.cnt = 0
 def __init__(self, mcptam_namespace='mcptam'):
   
   # Init QWidget
   super(SysmonWidget, self).__init__()
   self.setObjectName('SysmonWidget')
   
   # load UI
   ui_file = os.path.join(rospkg.RosPack().get_path('artemis_sysmon'), 'resource', 'widget.ui')
   loadUi(ui_file, self)
           
   # Subscribe to ROS topics and register callbacks
   self._slam_info_sub = rospy.Subscriber(mcptam_namespace+'/system_info', SystemInfo, self.slam_info_cb)
   self._slam_tracker_sub = rospy.Subscriber(mcptam_namespace+'/tracker_state', TrackerState, self.slam_tracker_cb)
   
   # Initialize service call
   print('Waiting for MAV services')
   rospy.wait_for_service(mcptam_namespace+'/init')
   rospy.wait_for_service(mcptam_namespace+'/reset')
   print('Connected to SLAM system')
   self._slam_init_srv = rospy.ServiceProxy(mcptam_namespace+'/init', Empty)
   self._slam_reset_srv = rospy.ServiceProxy(mcptam_namespace+'/reset', Reset)
  
   # init and start update timer for data, the timer calls the function update_info all 40ms    
   self._update_info_timer = QTimer(self)
   self._update_info_timer.timeout.connect(self.update_info)
   self._update_info_timer.start(40)
   
   # set the functions that are called when a button is pressed
   self.button_start.pressed.connect(self.on_start_button_pressed)
   self.button_reset.pressed.connect(self.on_reset_button_pressed)
   self.button_quit.pressed.connect(self.on_quit_button_pressed)
    def __init__(self, topic="diagnostics"):
        super(RuntimeMonitorWidget, self).__init__()
        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('rqt_runtime_monitor'), 'resource', 'runtime_monitor_widget.ui')
        loadUi(ui_file, self)
        self.setObjectName('RuntimeMonitorWidget')

        self._mutex = threading.Lock()

        self._error_icon = QIcon.fromTheme('dialog-error')
        self._warning_icon = QIcon.fromTheme('dialog-warning')
        self._ok_icon = QIcon.fromTheme('dialog-information')

        self._stale_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Stale (0)'])
        self._stale_node.setIcon(0, self._error_icon)
        self.tree_widget.addTopLevelItem(self._stale_node)

        self._error_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Errors (0)'])
        self._error_node.setIcon(0, self._error_icon)
        self.tree_widget.addTopLevelItem(self._error_node)

        self._warning_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Warnings (0)'])
        self._warning_node.setIcon(0, self._warning_icon)
        self.tree_widget.addTopLevelItem(self._warning_node)

        self._ok_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Ok (0)'])
        self._ok_node.setIcon(0, self._ok_icon)
        self.tree_widget.addTopLevelItem(self._ok_node)
        self.tree_widget.itemSelectionChanged.connect(self._refresh_selection)
        self.keyPressEvent = self._on_key_press

        self._name_to_item = {}
        self._new_errors_callback = None

        self._subscriber = rospy.Subscriber(topic, DiagnosticArray, self._diagnostics_callback)

        self._previous_ros_time = rospy.Time.now()
        self._timer = QTimer()
        self._timer.timeout.connect(self._on_timer)
        self._timer.start(1000)

        self._msg_timer = QTimer()
        self._msg_timer.timeout.connect(self._update_messages)
        self._msg_timer.start(100)

        self._messages = []
        self._used_items = 0
    def __init__(self, context, topic=None):
        """
        :param context: plugin context hook to enable adding widgets as a
                        ROS_GUI pane, 'PluginContext'
        :param topic: Diagnostic topic to subscribe to 'str'
        """

        super(RobotMonitorWidget, self).__init__()
        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('rqt_robot_monitor'), 'resource',
                               'robotmonitor_mainwidget.ui')
        loadUi(ui_file, self)

        obj_name = 'Robot Monitor'
        self.setObjectName(obj_name)
        self.setWindowTitle(obj_name)

        self.message_updated.connect(self.message_cb)

        # if we're given a topic, create a timeline. otherwise, don't
        #  this can be used later when writing an rqt_bag plugin
        if topic:
            # create timeline data structure
            self._timeline = Timeline(topic, DiagnosticArray)
            self._timeline.message_updated.connect(self.message_updated)

            # create timeline pane widget
            self.timeline_pane = TimelinePane(self)

            self.timeline_pane.set_timeline(self._timeline)

            self.vlayout_top.addWidget(self.timeline_pane)
            self.timeline_pane.show()
        else:
            self._timeline = None
            self.timeline_pane = None

        self._inspectors = {}
        # keep a copy of the current message for opening new inspectors
        self._current_msg = None

        self.tree_all_devices.itemDoubleClicked.connect(self._tree_clicked)
        self.warn_flattree.itemDoubleClicked.connect(self._tree_clicked)
        self.err_flattree.itemDoubleClicked.connect(self._tree_clicked)
        # TODO: resize on itemCollapsed and itemExpanded

        self._is_stale = False

        self._timer = QTimer()
        self._timer.timeout.connect(self._update_message_state)
        self._timer.start(1000)

        palette = self.tree_all_devices.palette()
        self._original_base_color = palette.base().color()
        self._original_alt_base_color = palette.alternateBase().color()

        self._tree = StatusItem(self.tree_all_devices.invisibleRootItem())
        self._warn_tree = StatusItem(self.warn_flattree.invisibleRootItem())
        self._err_tree = StatusItem(self.err_flattree.invisibleRootItem())
Beispiel #20
0
  def __init__(self, context):
        super(MyPlugin, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('MyPlugin')

        #final Inputの取得 TODO:ちゃんと表示させるようにする
        rospy.Subscriber('initialpose',PoseWithCovarianceStamped,self.InitialPoseCallback)

        # 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 is a sibling of this file
        # in this example the .ui and .py file are in the same folder
        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'RvizRuler.ui')

        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)
        # Give QObjects reasonable names
        self._widget.setObjectName('MyPluginUi')
        # 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 
        
        #Topicデータ表示用タイマ
        self._timer_refresh_topics = QTimer(self)
        self._timer_refresh_topics.timeout.connect(self.printdata)
        #このスロットが一定周期で呼ばれる
        self._timer_refresh_topics.start(500)#スレッドのループ周期 msec
        
        # 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 widget to the user interface
        context.add_widget(self._widget)

        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
Beispiel #21
0
class ProblemViewerWidget(QWidget):

    _problem_text = ""

    def __init__(self, plugin=None):
        super(ProblemViewerWidget, self).__init__()

        # Create QWidget
        ui_file = os.path.join(rospkg.RosPack().get_path('rosplan_rqt'), 'resource', 'problem_viewer.ui')
        loadUi(ui_file, self)
        self.setObjectName('ROSPlanProblemViewer')

        self._plugin = plugin
        rospy.Subscriber("/kcl_rosplan/problem", String, self.refresh_problem)

        # init and start update timers
        self._timer_set_problem = QTimer(self)
        self._timer_set_problem.timeout.connect(self.set_problem)

        self.start()

    def start(self):
        self._timer_set_problem.start(1000)

    """
    updating problem view
    """
    def refresh_problem(self, msg):
        self._problem_text = msg.data

    def set_problem(self):
        self.textEdit.setPlainText(self._problem_text)

    """
    Qt methods
    """ 
    def shutdown_plugin(self):
        pass

    def save_settings(self, plugin_settings, instance_settings):
        pass

    def restore_settings(self, plugin_settings, instance_settings):
        pass
class TopicDataWidget(DataDialog):

	def __init__(self, plugin):
		super(TopicDataWidget, self).__init__()
		rospkgPack = rospkg.RosPack()
		self._plugin = plugin
		self._topicName = TOPIC_NAME
		self._subscriber = None

		self._updateTimer = QTimer(self)
		self._updateTimer.timeout.connect(self.timeoutCallback)
		self._updateTimer.start(40)
		self.subscribeTopic(self._topicName)


	# rqt override
	def save_settings(self, plugin_settings, instance_settings):
		instance_settings.set_value('topic_name', self._topicName)

	def restore_settings(self, plugin_settings, instance_settings):
		topicName = instance_settings.value('topic_name')
		try:
			self._topicName = eval(topicName)
		except Exception:
			topicName = None

	def shutdown_plugin(self):
		self.unregister_topic()

	# subscribe topic
	def subscribeTopic(self, topicName):
#		msgClass, self._topicName, _ = get_topic_class(topicName)
		self._subscriber = rospy.Subscriber(TOPIC_NAME, MSG_CLASS, self.messageCallback)

	def messageCallback(self, msg):
		self.setDisplayedData(msg.data, TOPIC_NAME)

	def unregisterTopic(self):
		if self._subscriber:
			self._subscriber.unregister()

	def timeoutCallback(self):
		pass
Beispiel #23
0
    def __init__(self, parent, signal_tobe_emitted, nodes_monitored):
        """
        @type nodes_monitored: str[]
        """
        super(NodeMonitorThread, self).__init__()
        self._parent = parent
        self._signal = signal_tobe_emitted
        self._nodes_monitored = nodes_monitored

        self._timer = QTimer()
        self._timer.timeout.connect(self._monitor_nodes)
Beispiel #24
0
    def __init__(self):

        # Create QWidget
        self.setObjectName('ROSPlanGraphUpdater')

        # init and start update timers
        self._timer_refresh_plan = QTimer(self)
        self._timer_refresh_plan.timeout.connect(self.refresh_plan)

        rospy.Subscriber("/kcl_rosplan/plan", CompletePlan, self.plan_callback)
        rospy.Subscriber("/kcl_rosplan/action_feedback", ActionFeedback, self.action_feedback_callback)
Beispiel #25
0
class ParamCheckThread(threading.Thread):
    def __init__(self, parent, signal_tobe_emitted, params_monitored):
        """
        @type params_monitored: str[]
        """
        super(ParamCheckThread, self).__init__()
        self._parent = parent
        self._signal = signal_tobe_emitted
        self._params_monitored = params_monitored

        self._timer = QTimer()
        self._timer.timeout.connect(self._monitor_param)

    def run(self):
        self._timer.start(300)

    def _monitor_param(self):
        for param in self._params_monitored:
            has_param = rospy.has_param(param)
            self._signal.emit(has_param, param)
Beispiel #26
0
    def __init__(self, parent, signal_tobe_emitted, params_monitored):
        """
        @type params_monitored: str[]
        """
        super(ParamCheckThread, self).__init__()
        self._parent = parent
        self._signal = signal_tobe_emitted
        self._params_monitored = params_monitored

        self._timer = QTimer()
        self._timer.timeout.connect(self._monitor_param)
	def __init__(self, plugin):
		super(TopicDataWidget, self).__init__()
		rospkgPack = rospkg.RosPack()
		self._plugin = plugin
		self._topicName = TOPIC_NAME
		self._subscriber = None

		self._updateTimer = QTimer(self)
		self._updateTimer.timeout.connect(self.timeoutCallback)
		self._updateTimer.start(40)
		self.subscribeTopic(self._topicName)
 def __init__(self, parent=None):
     super(LifeFrame, self).__init__(parent)
     self._ui = Ui_life_frame()
     self._motion = Rotate('/mobile_base/commands/velocity')
     self._motion_thread = None
     self._timer = QTimer()
     #self._timer.setInterval(60000) #60s
     self._timer.setInterval(250) #60s
     QObject.connect(self._timer, SIGNAL('timeout()'), self, SLOT('update_progress_callback()'))
     self._state = LifeFrame.STATE_STOPPED
     self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
    def __init__(self, widget):
        super(MicoButtonsWidget, self).__init__()
        rospkg_pack = rospkg.RosPack()
        ui_file = os.path.join(rospkg_pack.get_path('mico_controller'), 'resource', 'MicoButtons.ui')
        loadUi(ui_file, self)

        self.start_arm_srv = rospy.ServiceProxy('mico_arm_driver/in/start', MicoStart)
        self.stop_arm_srv = rospy.ServiceProxy('mico_arm_driver/in/stop', MicoStop)
        self.home_arm_srv = rospy.ServiceProxy('mico_arm_driver/in/home_arm', MicoHome)

        self._updateTimer = QTimer(self)
        self._updateTimer.timeout.connect(self.timeout_callback)
Beispiel #30
0
    def __init__(self, parent=None):
        super(View, self).__init__()
        self.unit = 'deg'

        self.timer = QTimer(self)
        self.timer.timeout.connect(lambda: self.publishRequested.emit())

        pkg_path = rospkg.RosPack().get_path('rqt_tf_rot')
        ui_file_path = os.path.join(pkg_path, 'res', 'PluginUI.ui')

        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file_path, self)
        self.initUI()
Beispiel #31
0
    def __init__(self):
        super(BagPlayerWidget, self).__init__()
        rospkg_pack = rospkg.RosPack()
        ui_file = os.path.join(rospkg_pack.get_path('bag_browser'), 'resource',
                               'BagPlayer.ui')
        loadUi(ui_file, self)

        self._play_icon = QIcon.fromTheme('media-playback-start')
        self._pause_icon = QIcon.fromTheme('media-playback-pause')
        self.qt_play_btn.setIcon(self._play_icon)

        self._playing = False
        self._current_msec = 0
        self._max_msec = 1000000
        self.qt_seekbar_slider.setTracking(False)

        self._play_timer = QTimer()
        self._play_timer.timeout.connect(self.on_image_update)
        self._play_timer.setInterval(1)  # [ms]

        self._gui_timer = QTimer()
        self._gui_timer.timeout.connect(self.on_gui_update)
        self._gui_timer.setInterval(100)  # [ms]
        self._gui_timer.start()
Beispiel #32
0
    def __init__(self, context):
        """
        :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext''
        """
        super(Console, self).__init__(context)
        self.setObjectName('Console')

        self._datamodel = MessageDataModel()
        self._proxymodel = MessageProxyModel()
        self._proxymodel.setSourceModel(self._datamodel)

        self._mainwindow = ConsoleWidget(self._proxymodel)
        if context.serial_number() > 1:
            self._mainwindow.setWindowTitle(self._mainwindow.windowTitle() + (' (%d)' % context.serial_number()))
        context.add_widget(self._mainwindow)

        self._consolesubscriber = ConsoleSubscriber(self.message_callback)

        # Timer and Mutex for flushing recieved messages to datamodel.
        # Required since QSortProxyModel can not handle a high insert rate
        self._mutex = QMutex()
        self._timer = QTimer()
        self._timer.timeout.connect(self.insert_messages)
        self._timer.start(100)
Beispiel #33
0
class GroundStationWidget(QWidget):
    def __init__(self):
        super(GroundStationWidget, self).__init__()

        self._principle_layout = QBoxLayout(0)  # main layout is horizontal (0)
        self._map_layout = QVBoxLayout()
        self._principle_layout.addLayout(self._map_layout, 4)
        self._control_layout = QVBoxLayout()
        self._principle_layout.addLayout(self._control_layout, 3)

        self.setAcceptDrops(False)  # Dragging and Dropping not permitted
        self.setWindowTitle('ROSplane Ground Station')

        #=============================
        self._mw = MapWindow()
        self._map_layout.addWidget(self._mw)
        self._tv = PlotWidget()
        self._data_plot = DataPlot(self._tv)
        self._data_plot.set_autoscale(x=False)
        self._data_plot.set_autoscale(y=DataPlot.SCALE_EXTEND
                                      | DataPlot.SCALE_VISIBLE)
        self._data_plot.set_xlim([0, 10.0])
        self._tv.switch_data_plot_widget(self._data_plot)

        self._control_layout.addWidget(
            self._tv,
            1)  # ratio of these numbers determines window proportions
        self._ah = ArtificialHorizon()
        self._control_layout.addWidget(self._ah, 1)
        #=============================

        print('fake init')
        self.setLayout(self._principle_layout)

        # Global timer for marble_map and artificial_horizon
        self.interval = 100  # in milliseconds, period of regular update
        self.timer = QTimer(self)
        self.timer.setInterval(self.interval)
        self.timer.timeout.connect(self._mw._marble_map.update)
        self.timer.timeout.connect(self._ah.update)
        self.timer.start()

    def closeEvent(self, event):
        self.timer.stop()

    def save_settings(self, plugin_settings,
                      instance_settings):  # have a file to read and write from
        print('fake save')  # < prints to terminal

    def restore_settings(self, plugin_settings, instance_settings):
        print('fake restore')
Beispiel #34
0
    def _add_publisher(self, publisher_info):
        publisher_info['publisher_id'] = self._id_counter
        self._id_counter += 1
        publisher_info['counter'] = 0
        publisher_info['enabled'] = publisher_info.get('enabled', False)
        publisher_info['expressions'] = publisher_info.get('expressions', {})

        publisher_info['message_instance'] = self._create_message_instance(
            publisher_info['type_name'])
        if publisher_info['message_instance'] is None:
            return

        msg_module = get_message_class(publisher_info['type_name'])
        if not msg_module:
            raise RuntimeError(
                'The passed message type "{}" is invalid'.format(
                    publisher_info['type_name']))

        # Topic name provided was relative, remap to node namespace (if it was set)
        if not publisher_info['topic_name'].startswith('/'):
            publisher_info['topic_name'] = \
                self._node.get_namespace() + publisher_info['topic_name']

        # create publisher and timer
        publisher_info['publisher'] = self._node.create_publisher(
            msg_module,
            publisher_info['topic_name'],
            qos_profile=QoSProfile(depth=10))
        publisher_info['timer'] = QTimer(self)

        # add publisher info to _publishers dict and create signal mapping
        self._publishers[publisher_info['publisher_id']] = publisher_info
        self._timeout_mapper.setMapping(publisher_info['timer'],
                                        publisher_info['publisher_id'])
        publisher_info['timer'].timeout.connect(self._timeout_mapper.map)
        if publisher_info['enabled'] and publisher_info['rate'] > 0:
            publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
        self._widget.publisher_tree_widget.model().add_publisher(
            publisher_info)
Beispiel #35
0
    def _add_publisher(self, publisher_info):
        publisher_info['publisher_id'] = self._id_counter
        self._id_counter += 1
        publisher_info['counter'] = 0
        publisher_info['enabled'] = publisher_info.get('enabled', False)
        publisher_info['expressions'] = publisher_info.get('expressions', {})

        publisher_info['message_instance'] = self._create_message_instance(publisher_info['type_name'])
        if publisher_info['message_instance'] is None:
            return

        # create publisher and timer
        publisher_info['publisher'] = rospy.Publisher(publisher_info['topic_name'], type(publisher_info['message_instance']))
        publisher_info['timer'] = QTimer(self)

        # add publisher info to _publishers dict and create signal mapping
        self._publishers[publisher_info['publisher_id']] = publisher_info
        self._timeout_mapper.setMapping(publisher_info['timer'], publisher_info['publisher_id'])
        publisher_info['timer'].timeout.connect(self._timeout_mapper.map)
        if publisher_info['enabled'] and publisher_info['rate'] > 0:
            publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))

        self._widget.publisher_tree_widget.model().add_publisher(publisher_info)
Beispiel #36
0
class BaseFilter(QObject):
    """
    Contains basic functions common to all filters.
    Handles enabled logic and change notification.
    """
    filter_changed_signal = Signal()

    def __init__(self):
        super(BaseFilter, self).__init__()
        self._enabled = True

        self._timer = QTimer(self)
        self._timer.setSingleShot(True)
        self._timer.timeout.connect(self.filter_changed_signal.emit)

    def start_emit_timer(self, msec=None):
        """
        Starts a timer to emit a signal to refresh the filters after the filter is changed
        :param msec: number of msecs to wait before emitting the signal to change the filter ''int''
        """
        if msec is None:
            self._timer.start(10)
        else:
            self._timer.start(msec)

    def is_enabled(self):
        return self._enabled

    def set_enabled(self, checked):
        """
        Setter for _enabled
        :param checked: boolean flag to set ''bool''
        :emits filter_changed_signal: Always
        """
        self._enabled = checked
        self.start_emit_timer(200)

    def has_filter(self):
        raise NotImplementedError()

    def test_message(self, message):
        raise NotImplementedError()
Beispiel #37
0
class TopicWidget(QWidget):
    _column_names = ['topic', 'type', 'bandwidth', 'rate', 'value']

    def __init__(self, plugin):
        super(TopicWidget, self).__init__()
        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
                               'TopicWidget.ui')
        loadUi(ui_file, self)
        self._plugin = plugin
        self.topics_tree_widget.sortByColumn(0, Qt.AscendingOrder)
        header = self.topics_tree_widget.header()
        header.setResizeMode(QHeaderView.ResizeToContents)
        header.customContextMenuRequested.connect(
            self.handle_header_view_customContextMenuRequested)
        header.setContextMenuPolicy(Qt.CustomContextMenu)

        self._current_topic_list = []
        self._topics = {}
        self._tree_items = {}
        self._column_index = {}
        for column_name in self._column_names:
            self._column_index[column_name] = len(self._column_index)

        self.refresh_topics()
        # init and start update timer
        self._timer_refresh_topics = QTimer(self)
        self._timer_refresh_topics.timeout.connect(self.refresh_topics)
        self._timer_refresh_topics.start(1000)

    @Slot()
    def refresh_topics(self):
        # refresh tree view items
        topic_list = rospy.get_published_topics()
        if self._current_topic_list != topic_list:
            self._current_topic_list = topic_list

            # start new topic dict
            new_topics = {}

            for topic_name, topic_type in topic_list:
                # if topic is new or has changed its type
                if topic_name not in self._topics or self._topics[topic_name][
                        'type'] != topic_type:
                    # create new TopicInfo
                    topic_info = TopicInfo(topic_name)
                    # if successful, add it to the dict and tree view
                    if topic_info._topic_name:
                        topic_item = self._recursive_create_widget_items(
                            self.topics_tree_widget, topic_name, topic_type,
                            topic_info.message_class())
                        new_topics[topic_name] = {
                            'item': topic_item,
                            'info': topic_info,
                            'type': topic_type,
                        }
                else:
                    # if topic has been seen before, copy it to new dict and remove it from the old one
                    new_topics[topic_name] = self._topics[topic_name]
                    del self._topics[topic_name]

            # clean up old topics
            for topic_name in self._topics.keys():
                self._topics[topic_name]['info'].stop_monitoring()
                index = self.topics_tree_widget.indexOfTopLevelItem(
                    self._topics[topic_name]['item'])
                self.topics_tree_widget.takeTopLevelItem(index)
                del self._topics[topic_name]

            # switch to new topic dict
            self._topics = new_topics

        self._update_topics_data()

    def _update_topics_data(self):
        for topic in self._topics.values():
            topic_info = topic['info']
            if topic_info.monitoring:
                # update rate
                rate, _, _, _ = topic_info.get_hz()
                rate_text = '%1.2f' % rate if rate != None else 'unknown'

                # update bandwidth
                bytes_per_s, _, _, _ = topic_info.get_bw()
                if bytes_per_s is None:
                    bandwidth_text = 'unknown'
                elif bytes_per_s < 1000:
                    bandwidth_text = '%.2fB/s' % bytes_per_s
                elif bytes_per_s < 1000000:
                    bandwidth_text = '%.2fKB/s' % (bytes_per_s / 1000.)
                else:
                    bandwidth_text = '%.2fMB/s' % (bytes_per_s / 1000000.)

                # update values
                value_text = ''
                self.update_value(topic_info._topic_name,
                                  topic_info.last_message)

            else:
                rate_text = ''
                bandwidth_text = ''
                value_text = 'not monitored'

            self._tree_items[topic_info._topic_name].setText(
                self._column_index['rate'], rate_text)
            self._tree_items[topic_info._topic_name].setText(
                self._column_index['bandwidth'], bandwidth_text)
            self._tree_items[topic_info._topic_name].setText(
                self._column_index['value'], value_text)

    def update_value(self, topic_name, message):
        if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
            for slot_name in message.__slots__:
                self.update_value(topic_name + '/' + slot_name,
                                  getattr(message, slot_name))

        elif type(message) in (list, tuple) and (len(message) > 0) and hasattr(
                message[0], '__slots__'):
            for index, slot in enumerate(message):
                if topic_name + '[%d]' % index in self._tree_items:
                    self.update_value(topic_name + '[%d]' % index, slot)
                else:
                    base_type_str, _ = self._extract_array_info(
                        self._tree_items[topic_name].text(
                            self._column_index['type']))
                    self._recursive_create_widget_items(
                        self._tree_items[topic_name],
                        topic_name + '[%d]' % index, base_type_str, slot)

        else:
            if topic_name in self._tree_items:
                self._tree_items[topic_name].setText(
                    self._column_index['value'], repr(message))

    def _extract_array_info(self, type_str):
        array_size = None
        if '[' in type_str and type_str[-1] == ']':
            type_str, array_size_str = type_str.split('[', 1)
            array_size_str = array_size_str[:-1]
            if len(array_size_str) > 0:
                array_size = int(array_size_str)
            else:
                array_size = 0

        return type_str, array_size

    def _recursive_create_widget_items(self, parent, topic_name, type_name,
                                       message):
        if parent is self.topics_tree_widget:
            # show full topic name with preceding namespace on toplevel item
            topic_text = topic_name
        else:
            topic_text = topic_name.split('/')[-1]
            if '[' in topic_text:
                topic_text = topic_text[topic_text.index('['):]
        item = QTreeWidgetItem(parent)
        item.setText(self._column_index['topic'], topic_text)
        item.setText(self._column_index['type'], type_name)
        item.setData(0, Qt.UserRole, topic_name)
        self._tree_items[topic_name] = item
        if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
            for slot_name, type_name in zip(message.__slots__,
                                            message._slot_types):
                self._recursive_create_widget_items(
                    item, topic_name + '/' + slot_name, type_name,
                    getattr(message, slot_name))

        else:
            base_type_str, array_size = self._extract_array_info(type_name)
            try:
                base_instance = roslib.message.get_message_class(
                    base_type_str)()
            except ValueError:
                base_instance = None
            if array_size is not None and hasattr(base_instance, '__slots__'):
                for index in range(array_size):
                    self._recursive_create_widget_items(
                        item, topic_name + '[%d]' % index, base_type_str,
                        base_instance)
        return item

    @Slot('QPoint')
    def handle_header_view_customContextMenuRequested(self, pos):
        header = self.topics_tree_widget.header()

        # show context menu
        menu = QMenu(self)
        action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize')
        action = menu.exec_(header.mapToGlobal(pos))

        # evaluate user action
        if action is action_toggle_auto_resize:
            if header.resizeMode(0) == QHeaderView.ResizeToContents:
                header.setResizeMode(QHeaderView.Interactive)
            else:
                header.setResizeMode(QHeaderView.ResizeToContents)

    @Slot('QPoint')
    def on_topics_tree_widget_customContextMenuRequested(self, pos):
        item = self.topics_tree_widget.itemAt(pos)
        if item is None:
            return

        # show context menu
        menu = QMenu(self)
        action_moggle_monitoring = menu.addAction(QIcon.fromTheme('search'),
                                                  'Toggle Monitoring')
        action_item_expand = menu.addAction(QIcon.fromTheme('zoom-in'),
                                            'Expand All Children')
        action_item_collapse = menu.addAction(QIcon.fromTheme('zoom-out'),
                                              'Collapse All Children')
        action = menu.exec_(self.topics_tree_widget.mapToGlobal(pos))

        # evaluate user action
        if action is action_moggle_monitoring:
            root_item = item
            while root_item.parent() is not None:
                root_item = root_item.parent()
            root_topic_name = root_item.data(0, Qt.UserRole)
            self._topics[root_topic_name]['info'].toggle_monitoring()

        elif action in (action_item_expand, action_item_collapse):
            expanded = (action is action_item_expand)

            def recursive_set_expanded(item):
                item.setExpanded(expanded)
                for index in range(item.childCount()):
                    recursive_set_expanded(item.child(index))

            recursive_set_expanded(item)

    def shutdown_plugin(self):
        for topic in self._topics.values():
            topic['info'].stop_monitoring()
        self._timer_refresh_topics.stop()
Beispiel #38
0
    def main(self,
             argv=None,
             standalone=None,
             plugin_argument_provider=None,
             plugin_manager_settings_prefix=''):
        if argv is None:
            argv = sys.argv

        # extract --args and everything behind manually since argparse can not handle that
        arguments = argv[1:]

        # extract plugin specific args when not being invoked in standalone mode programmatically
        if not standalone:
            plugin_args = []
            if '--args' in arguments:
                index = arguments.index('--args')
                plugin_args = arguments[index + 1:]
                arguments = arguments[0:index + 1]

        parser = ArgumentParser(os.path.basename(Main.main_filename),
                                add_help=False)
        self.add_arguments(parser,
                           standalone=bool(standalone),
                           plugin_argument_provider=plugin_argument_provider)
        self._options = parser.parse_args(arguments)
        self._options.multi_process = False  # not supported anymore

        if standalone:
            # rerun parsing to separate common arguments from plugin specific arguments
            parser = ArgumentParser(os.path.basename(Main.main_filename),
                                    add_help=False)
            self.add_arguments(parser, standalone=bool(standalone))
            self._options, plugin_args = parser.parse_known_args(arguments)
        self._options.plugin_args = plugin_args

        # set default values for options not available in standalone mode
        if standalone:
            self._options.freeze_layout = False
            self._options.lock_perspective = False
            self._options.hide_title = False
            self._options.perspective = None
            self._options.perspective_file = None
            self._options.standalone_plugin = standalone
            self._options.list_perspectives = False
            self._options.list_plugins = False
            self._options.command_pid = None
            self._options.command_start_plugin = None
            self._options.command_switch_perspective = None
            self._options.embed_plugin = None
            self._options.embed_plugin_serial = None
            self._options.embed_plugin_address = None

        # check option dependencies
        try:
            if self._options.plugin_args and not self._options.standalone_plugin and not self._options.command_start_plugin and not self._options.embed_plugin:
                raise RuntimeError(
                    'Option --args can only be used together with either --standalone, --command-start-plugin or --embed-plugin option'
                )

            if self._options.freeze_layout and not self._options.lock_perspective:
                raise RuntimeError(
                    'Option --freeze_layout can only be used together with the --lock_perspective option'
                )

            list_options = (self._options.list_perspectives,
                            self._options.list_plugins)
            list_options_set = [
                opt for opt in list_options if opt is not False
            ]
            if len(list_options_set) > 1:
                raise RuntimeError(
                    'Only one --list-* option can be used at a time')

            command_options = (self._options.command_start_plugin,
                               self._options.command_switch_perspective)
            command_options_set = [
                opt for opt in command_options if opt is not None
            ]
            if len(command_options_set) > 0 and not self._dbus_available:
                raise RuntimeError(
                    'Without DBus support the --command-* options are not available'
                )
            if len(command_options_set) > 1:
                raise RuntimeError(
                    'Only one --command-* option can be used at a time (except --command-pid which is optional)'
                )
            if len(command_options_set
                   ) == 0 and self._options.command_pid is not None:
                raise RuntimeError(
                    'Option --command_pid can only be used together with an other --command-* option'
                )

            embed_options = (self._options.embed_plugin,
                             self._options.embed_plugin_serial,
                             self._options.embed_plugin_address)
            embed_options_set = [
                opt for opt in embed_options if opt is not None
            ]
            if len(command_options_set) > 0 and not self._dbus_available:
                raise RuntimeError(
                    'Without DBus support the --embed-* options are not available'
                )
            if len(embed_options_set) > 0 and len(embed_options_set) < len(
                    embed_options):
                raise RuntimeError(
                    'Missing option(s) - all \'--embed-*\' options must be set'
                )

            if len(embed_options_set) > 0 and self._options.clear_config:
                raise RuntimeError(
                    'Option --clear-config can only be used without any --embed-* option'
                )

            groups = (list_options_set, command_options_set, embed_options_set)
            groups_set = [opt for opt in groups if len(opt) > 0]
            if len(groups_set) > 1:
                raise RuntimeError(
                    'Options from different groups (--list, --command, --embed) can not be used together'
                )

            perspective_options = (self._options.perspective,
                                   self._options.perspective_file)
            perspective_options_set = [
                opt for opt in perspective_options if opt is not None
            ]
            if len(perspective_options_set) > 1:
                raise RuntimeError(
                    'Only one --perspective-* option can be used at a time')

            if self._options.perspective_file is not None and not os.path.isfile(
                    self._options.perspective_file):
                raise RuntimeError(
                    'Option --perspective-file must reference existing file')

        except RuntimeError as e:
            print(str(e))
            #parser.parse_args(['--help'])
            # calling --help will exit
            return 1

        # set implicit option dependencies
        if self._options.standalone_plugin is not None:
            self._options.lock_perspective = True

        # create application context containing various relevant information
        from .application_context import ApplicationContext
        context = ApplicationContext()
        context.qtgui_path = self._qtgui_path
        context.options = self._options

        if self._dbus_available:
            from dbus import DBusException, Interface, SessionBus

        # non-special applications provide various dbus interfaces
        if self._dbus_available:
            context.provide_app_dbus_interfaces = len(groups_set) == 0
            context.dbus_base_bus_name = 'org.ros.qt_gui'
            if context.provide_app_dbus_interfaces:
                context.dbus_unique_bus_name = context.dbus_base_bus_name + '.pid%d' % os.getpid(
                )

                # provide pid of application via dbus
                from .application_dbus_interface import ApplicationDBusInterface
                _dbus_server = ApplicationDBusInterface(
                    context.dbus_base_bus_name)

        # determine host bus name, either based on pid given on command line or via dbus application interface if any other instance is available
        if len(command_options_set) > 0 or len(embed_options_set) > 0:
            host_pid = None
            if self._options.command_pid is not None:
                host_pid = self._options.command_pid
            else:
                try:
                    remote_object = SessionBus().get_object(
                        context.dbus_base_bus_name, '/Application')
                except DBusException:
                    pass
                else:
                    remote_interface = Interface(
                        remote_object,
                        context.dbus_base_bus_name + '.Application')
                    host_pid = remote_interface.get_pid()
            if host_pid is not None:
                context.dbus_host_bus_name = context.dbus_base_bus_name + '.pid%d' % host_pid

        # execute command on host application instance
        if len(command_options_set) > 0:
            if self._options.command_start_plugin is not None:
                try:
                    remote_object = SessionBus().get_object(
                        context.dbus_host_bus_name, '/PluginManager')
                except DBusException:
                    (rc,
                     msg) = (1,
                             'unable to communicate with GUI instance "%s"' %
                             context.dbus_host_bus_name)
                else:
                    remote_interface = Interface(
                        remote_object,
                        context.dbus_base_bus_name + '.PluginManager')
                    (rc, msg) = remote_interface.start_plugin(
                        self._options.command_start_plugin,
                        ' '.join(self._options.plugin_args))
                if rc == 0:
                    print('qt_gui_main() started plugin "%s" in GUI "%s"' %
                          (msg, context.dbus_host_bus_name))
                else:
                    print(
                        'qt_gui_main() could not start plugin "%s" in GUI "%s": %s'
                        % (self._options.command_start_plugin,
                           context.dbus_host_bus_name, msg))
                return rc
            elif self._options.command_switch_perspective is not None:
                remote_object = SessionBus().get_object(
                    context.dbus_host_bus_name, '/PerspectiveManager')
                remote_interface = Interface(
                    remote_object,
                    context.dbus_base_bus_name + '.PerspectiveManager')
                remote_interface.switch_perspective(
                    self._options.command_switch_perspective)
                print(
                    'qt_gui_main() switched to perspective "%s" in GUI "%s"' %
                    (self._options.command_switch_perspective,
                     context.dbus_host_bus_name))
                return 0
            raise RuntimeError('Unknown command not handled')

        # choose selected or default qt binding
        setattr(sys, 'SELECT_QT_BINDING', self._options.qt_binding)
        from python_qt_binding import QT_BINDING

        from python_qt_binding.QtCore import qDebug, qInstallMessageHandler, QSettings, Qt, QtCriticalMsg, QtDebugMsg, QtFatalMsg, QTimer, QtWarningMsg
        from python_qt_binding.QtGui import QIcon
        from python_qt_binding.QtWidgets import QAction, QMenuBar

        from .about_handler import AboutHandler
        from .composite_plugin_provider import CompositePluginProvider
        from .container_manager import ContainerManager
        from .help_provider import HelpProvider
        from .icon_loader import get_icon
        from .main_window import MainWindow
        from .minimized_dock_widgets_toolbar import MinimizedDockWidgetsToolbar
        from .perspective_manager import PerspectiveManager
        from .plugin_manager import PluginManager

        # TODO PySide2 segfaults when invoking this custom message handler atm
        if QT_BINDING != 'pyside':

            def message_handler(type_, context, msg):
                colored_output = 'TERM' in os.environ and 'ANSI_COLORS_DISABLED' not in os.environ
                cyan_color = '\033[36m' if colored_output else ''
                red_color = '\033[31m' if colored_output else ''
                reset_color = '\033[0m' if colored_output else ''
                if type_ == QtDebugMsg and self._options.verbose:
                    print(msg, file=sys.stderr)
                elif type_ == QtWarningMsg:
                    print(cyan_color + msg + reset_color, file=sys.stderr)
                elif type_ == QtCriticalMsg:
                    print(red_color + msg + reset_color, file=sys.stderr)
                elif type_ == QtFatalMsg:
                    print(red_color + msg + reset_color, file=sys.stderr)
                    sys.exit(1)

            qInstallMessageHandler(message_handler)

        app = self.create_application(argv)

        self._check_icon_theme_compliance()

        settings = QSettings(QSettings.IniFormat, QSettings.UserScope,
                             'ros.org', self._settings_filename)
        if len(embed_options_set) == 0:
            if self._options.clear_config:
                settings.clear()

            main_window = MainWindow()
            if self._options.on_top:
                main_window.setWindowFlags(Qt.WindowStaysOnTopHint)

            main_window.statusBar()

            def sigint_handler(*args):
                qDebug('\nsigint_handler()')
                main_window.close()

            signal.signal(signal.SIGINT, sigint_handler)
            # the timer enables triggering the sigint_handler
            timer = QTimer()
            timer.start(500)
            timer.timeout.connect(lambda: None)

            if not self._options.lock_perspective:
                menu_bar = main_window.menuBar()
                file_menu = menu_bar.addMenu(menu_bar.tr('&File'))
                action = QAction(file_menu.tr('&Quit'), file_menu)
                action.setIcon(QIcon.fromTheme('application-exit'))
                action.triggered.connect(main_window.close)
                file_menu.addAction(action)
            else:
                menu_bar = None

        else:
            app.setQuitOnLastWindowClosed(False)

            main_window = None
            menu_bar = None

        self._add_plugin_providers()

        # setup plugin manager
        plugin_provider = CompositePluginProvider(self.plugin_providers)
        plugin_manager = PluginManager(
            plugin_provider,
            settings,
            context,
            settings_prefix=plugin_manager_settings_prefix)

        if self._options.list_plugins:
            # output available plugins
            print('\n'.join(sorted(plugin_manager.get_plugins().values())))
            return 0

        help_provider = HelpProvider()
        plugin_manager.plugin_help_signal.connect(
            help_provider.plugin_help_request)

        # setup perspective manager
        if main_window is not None:
            perspective_manager = PerspectiveManager(settings, context)

            if self._options.list_perspectives:
                # output available perspectives
                print('\n'.join(sorted(perspective_manager.perspectives)))
                return 0
        else:
            perspective_manager = None

        if main_window is not None:
            container_manager = ContainerManager(main_window, plugin_manager)
            plugin_manager.set_main_window(main_window, menu_bar,
                                           container_manager)

            if not self._options.freeze_layout:
                minimized_dock_widgets_toolbar = MinimizedDockWidgetsToolbar(
                    container_manager, main_window)
                main_window.addToolBar(Qt.BottomToolBarArea,
                                       minimized_dock_widgets_toolbar)
                plugin_manager.set_minimized_dock_widgets_toolbar(
                    minimized_dock_widgets_toolbar)

        if menu_bar is not None:
            perspective_menu = menu_bar.addMenu(menu_bar.tr('P&erspectives'))
            perspective_manager.set_menu(perspective_menu)

        # connect various signals and slots
        if perspective_manager is not None and main_window is not None:
            # signal changed perspective to update window title
            perspective_manager.perspective_changed_signal.connect(
                main_window.perspective_changed)
            # signal new settings due to changed perspective
            perspective_manager.save_settings_signal.connect(
                main_window.save_settings)
            perspective_manager.restore_settings_signal.connect(
                main_window.restore_settings)
            perspective_manager.restore_settings_without_plugin_changes_signal.connect(
                main_window.restore_settings)

        if perspective_manager is not None and plugin_manager is not None:
            perspective_manager.save_settings_signal.connect(
                plugin_manager.save_settings)
            plugin_manager.save_settings_completed_signal.connect(
                perspective_manager.save_settings_completed)
            perspective_manager.restore_settings_signal.connect(
                plugin_manager.restore_settings)
            perspective_manager.restore_settings_without_plugin_changes_signal.connect(
                plugin_manager.restore_settings_without_plugins)

        if plugin_manager is not None and main_window is not None:
            # signal before changing plugins to save window state
            plugin_manager.plugins_about_to_change_signal.connect(
                main_window.save_setup)
            # signal changed plugins to restore window state
            plugin_manager.plugins_changed_signal.connect(
                main_window.restore_state)
            # signal save settings to store plugin setup on close
            main_window.save_settings_before_close_signal.connect(
                plugin_manager.close_application)
            # signal save and shutdown called for all plugins, trigger closing main window again
            plugin_manager.close_application_signal.connect(
                main_window.close, type=Qt.QueuedConnection)

        if main_window is not None and menu_bar is not None:
            about_handler = AboutHandler(context.qtgui_path, main_window)
            help_menu = menu_bar.addMenu(menu_bar.tr('&Help'))
            action = QAction(help_menu.tr('&About'), help_menu)
            action.setIcon(QIcon.fromTheme('help-about'))
            action.triggered.connect(about_handler.show)
            help_menu.addAction(action)

        # set initial size - only used without saved configuration
        if main_window is not None:
            main_window.resize(600, 450)
            main_window.move(100, 100)

        # ensure that qt_gui/src is in sys.path
        src_path = os.path.realpath(
            os.path.join(os.path.dirname(__file__), '..'))
        if src_path not in sys.path:
            sys.path.append(src_path)

        # load specific plugin
        plugin = None
        plugin_serial = None
        if self._options.embed_plugin is not None:
            plugin = self._options.embed_plugin
            plugin_serial = self._options.embed_plugin_serial
        elif self._options.standalone_plugin is not None:
            plugin = self._options.standalone_plugin
            plugin_serial = 0
        if plugin is not None:
            plugins = plugin_manager.find_plugins_by_name(plugin)
            if len(plugins) == 0:
                print('qt_gui_main() found no plugin matching "%s"' % plugin)
                print('try passing the option "--force-discover"')
                return 1
            elif len(plugins) > 1:
                print(
                    'qt_gui_main() found multiple plugins matching "%s"\n%s' %
                    (plugin, '\n'.join(plugins.values())))
                return 1
            plugin = list(plugins.keys())[0]

        qDebug('QtBindingHelper using %s' % QT_BINDING)

        plugin_manager.discover()

        if self._options.reload_import:
            qDebug(
                'ReloadImporter() automatically reload all subsequent imports')
            from .reload_importer import ReloadImporter
            _reload_importer = ReloadImporter()
            self._add_reload_paths(_reload_importer)
            _reload_importer.enable()

        # switch perspective
        if perspective_manager is not None:
            if plugin:
                perspective_manager.set_perspective(
                    plugin, hide_and_without_plugin_changes=True)
            elif self._options.perspective_file:
                perspective_manager.import_perspective_from_file(
                    self._options.perspective_file,
                    perspective_manager.HIDDEN_PREFIX +
                    os.path.basename(self._options.perspective_file))
            else:
                perspective_manager.set_perspective(self._options.perspective)

        # load specific plugin
        if plugin:
            plugin_manager.load_plugin(plugin, plugin_serial,
                                       self._options.plugin_args)
            running = plugin_manager.is_plugin_running(plugin, plugin_serial)
            if not running:
                return 1
            if self._options.standalone_plugin:
                # use icon of standalone plugin (if available) for application
                plugin_descriptor = plugin_manager.get_plugin_descriptor(
                    plugin)
                action_attributes = plugin_descriptor.action_attributes()
                if 'icon' in action_attributes and action_attributes[
                        'icon'] is not None:
                    base_path = plugin_descriptor.attributes().get(
                        'plugin_path')
                    try:
                        icon = get_icon(
                            action_attributes['icon'],
                            action_attributes.get('icontype', None), base_path)
                    except UserWarning:
                        pass
                    else:
                        app.setWindowIcon(icon)

        if main_window is not None:
            main_window.show()
            if sys.platform == 'darwin':
                main_window.raise_()

        return app.exec_()
Beispiel #39
0
class PlotWidget(QWidget):
    _interval = 500

    def __init__(self, initial_topics=None, start_paused=False):
        super(PlotWidget, self).__init__()
        self.setObjectName('PlotWidget')

        self._initial_topics = initial_topics

        rp = rospkg.RosPack()
        ui_file = '/home/mohammad/Documents/CMU/RED/catkin_ws/src/rqt_safeguard/resource/plot.ui'
        loadUi(ui_file, self)
        self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add'))
        self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove'))
        self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
        # self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
        # self.data_plot = None

        self.subscribe_topic_button.setEnabled(False)
        if start_paused:
            self.pause_button.setChecked(True)

        self._topic_completer = TopicCompleter(self.topic_edit)
        self._topic_completer.update_topics()
        self.topic_edit.setCompleter(self._topic_completer)

        self._start_time = rospy.get_time()
        self._rosdata = {}
        self._remove_topic_menu = QMenu()

        self.color_box.setStyleSheet("background-color: red;")

        # init and start update timer for plot
        self._update_timer = QTimer(self)
        self._update_timer.timeout.connect(self.update)

    # def switch_data_plot_widget(self, data_plot):
    #     self.enable_timer(enabled=False)

    #     self.data_plot_layout.removeWidget(self.data_plot)
    #     if self.data_plot is not None:
    #         self.data_plot.close()

    #     self.data_plot = data_plot
    #     self.data_plot_layout.addWidget(self.data_plot)
    #     self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked())

    #     # setup drag 'n drop
    #     self.data_plot.dropEvent = self.dropEvent
    #     self.data_plot.dragEnterEvent = self.dragEnterEvent

    #     if self._initial_topics:
    #         for topic_name in self._initial_topics:
    #             self.add_topic(topic_name)
    #         self._initial_topics = None
    #     else:
    #         for topic_name, rosdata in self._rosdata.items():
    #             data_x, data_y = rosdata.next()
    #             self.data_plot.add_curve(topic_name, topic_name, data_x, data_y)

    #     self._subscribed_topics_changed()

    @Slot('QDragEnterEvent*')
    def dragEnterEvent(self, event):
        # get topic name
        if not event.mimeData().hasText():
            if not hasattr(event.source(), 'selectedItems') or len(
                    event.source().selectedItems()) == 0:
                qWarning(
                    'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0'
                )
                return
            item = event.source().selectedItems()[0]
            topic_name = item.data(0, Qt.UserRole)
            if topic_name == None:
                qWarning(
                    'Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)'
                )
                return
        else:
            topic_name = str(event.mimeData().text())

        # check for plottable field type
        plottable, message = is_plottable(topic_name)
        if plottable:
            event.acceptProposedAction()
        else:
            qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message))

    @Slot('QDropEvent*')
    def dropEvent(self, event):
        if event.mimeData().hasText():
            topic_name = str(event.mimeData().text())
        else:
            droped_item = event.source().selectedItems()[0]
            topic_name = str(droped_item.data(0, Qt.UserRole))
        self.add_topic(topic_name)

    @Slot(str)
    def on_topic_edit_textChanged(self, topic_name):
        # on empty topic name, update topics
        if topic_name in ('', '/'):
            self._topic_completer.update_topics()

        plottable, message = is_plottable(topic_name)
        self.subscribe_topic_button.setEnabled(plottable)
        self.subscribe_topic_button.setToolTip(message)

    @Slot()
    def on_topic_edit_returnPressed(self):
        if self.subscribe_topic_button.isEnabled():
            self.add_topic(str(self.topic_edit.text()))

    @Slot()
    def on_subscribe_topic_button_clicked(self):
        self.add_topic(str(self.topic_edit.text()))

    @Slot(bool)
    def on_pause_button_clicked(self, checked):
        self.enable_timer(not checked)
        print("pause")

    @Slot(bool)
    def on_autoscroll_checkbox_clicked(self, checked):
        # self.data_plot.autoscroll(checked)
        if checked:
            # self.data_plot.redraw()
            print("checked is clicked")

    # @Slot()
    # def on_clear_button_clicked(self):
    #     print("Clear Button is clicked")
    #     #self.clear_plot()

    def update(self):
        print("update")

        for topic_name, rosdata in self._rosdata.items():
            decision, oriX = rosdata.next_dx()
            if oriX:
                self.oriX.setText(str(round(oriX, 2)))
            oriY, oriZ = rosdata.next_yz()
            if oriY:
                self.oriY.setText(str(round(oriY, 2)))
            if oriZ:
                self.oriZ.setText(str(round(oriZ, 2)))
            placX, placY = rosdata.next_pxy()
            if placX:
                self.placX.setText(str(round(100 * placX, 2)))
            if placY:
                self.placY.setText(str(round(100 * placY, 2)))
            placZ = rosdata.next_pz()
            if placZ:
                self.placZ.setText(str(round(100 * placZ, 2)))

        if decision:
            print(decision)
            self.color_box.setStyleSheet(
                "background-color: red; color: white; font: bold; font-size: 96px;"
            )
            self.color_box.setText("Not Safe")
        else:
            print(decision)
            self.color_box.setStyleSheet(
                "background-color: green; color: white; font: bold; font-size: 96px;"
            )
            self.color_box.setText("Safe")
        # if self.data_plot is not None:
        #     needs_redraw = False
        #     for topic_name, rosdata in self._rosdata.items():
        #         try:
        #             data_x, data_y = rosdata.next()
        #             if data_x or data_y:
        #                 self.data_plot.update_values(topic_name, data_x, data_y)
        #                 needs_redraw = True
        #         except RosPlotException as e:
        #             qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e)
        #     if needs_redraw:
        #         self.data_plot.redraw()

    def _subscribed_topics_changed(self):
        self._update_remove_topic_menu()
        if not self.pause_button.isChecked():
            # if pause button is not pressed, enable timer based on subscribed topics
            self.enable_timer()
            print("if not self.pause_button.isChecked(): IN WIDGET PY")
        #self.data_plot.redraw()

    def _update_remove_topic_menu(self):
        def make_remove_topic_function(x):
            return lambda: self.remove_topic(x)

        self._remove_topic_menu.clear()
        for topic_name in sorted(self._rosdata.keys()):
            action = QAction(topic_name, self._remove_topic_menu)
            action.triggered.connect(make_remove_topic_function(topic_name))
            self._remove_topic_menu.addAction(action)

        if len(self._rosdata) > 1:
            all_action = QAction('All', self._remove_topic_menu)
            all_action.triggered.connect(self.clean_up_subscribers)
            self._remove_topic_menu.addAction(all_action)

        self.remove_topic_button.setMenu(self._remove_topic_menu)

    def add_topic(self, topic_name):
        self.enable_timer()
        topics_changed = False
        for topic_name in get_plot_fields(topic_name)[0]:
            if topic_name in self._rosdata:
                qWarning(
                    'PlotWidget.add_topic(): topic already subscribed: %s' %
                    topic_name)
                continue
            self._rosdata[topic_name] = ROSData(topic_name, self._start_time)
            if self._rosdata[topic_name].error is not None:
                qWarning(str(self._rosdata[topic_name].error))
                del self._rosdata[topic_name]
            else:
                # data_x, data_y = self._rosdata[topic_name].next()                          INJA TAGHIR BEDE UPDATE KON MAGHADIR RO
                #self.data_plot.add_curve(topic_name, topic_name, data_x, data_y)
                topics_changed = True

        if topics_changed:
            self._subscribed_topics_changed()

    def remove_topic(self, topic_name):
        self._rosdata[topic_name].close()
        del self._rosdata[topic_name]
        # self.data_plot.remove_curve(topic_name)

        self._subscribed_topics_changed()

    # def clear_plot(self):
    #     for topic_name, _ in self._rosdata.items():
    #         self.data_plot.clear_values(topic_name)
    #     self.data_plot.redraw()

    def clean_up_subscribers(self):
        for topic_name, rosdata in self._rosdata.items():
            rosdata.close()
            # self.data_plot.remove_curve(topic_name)
        self._rosdata = {}

        self._subscribed_topics_changed()

    def enable_timer(self, enabled=True):
        if enabled:
            self._update_timer.start(self._interval)
        else:
            self._update_timer.stop()

    def alaki(self):
        print("alaki")
Beispiel #40
0
class PlotWidget(QWidget):
    _redraw_interval = 40

    def __init__(self, node, initial_topics=None, start_paused=False):
        super(PlotWidget, self).__init__()
        self.setObjectName('PlotWidget')

        self._node = node
        self._initial_topics = initial_topics

        _, package_path = get_resource('packages', 'rqt_plot')
        ui_file = os.path.join(package_path, 'share', 'rqt_plot', 'resource',
                               'plot.ui')
        loadUi(ui_file, self)
        self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add'))
        self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove'))
        self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
        self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
        self.data_plot = None

        self.subscribe_topic_button.setEnabled(False)
        if start_paused:
            self.pause_button.setChecked(True)

        self._topic_completer = TopicCompleter(self.topic_edit)
        self._topic_completer.update_topics(node)
        self.topic_edit.setCompleter(self._topic_completer)

        self._start_time = time.time()
        self._rosdata = {}
        self._remove_topic_menu = QMenu()

        # init and start update timer for plot
        self._update_plot_timer = QTimer(self)
        self._update_plot_timer.timeout.connect(self.update_plot)

    def switch_data_plot_widget(self, data_plot):
        self.enable_timer(enabled=False)

        self.data_plot_layout.removeWidget(self.data_plot)
        if self.data_plot is not None:
            self.data_plot.close()

        self.data_plot = data_plot
        self.data_plot_layout.addWidget(self.data_plot)
        self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked())

        # setup drag 'n drop
        self.data_plot.dropEvent = self.dropEvent
        self.data_plot.dragEnterEvent = self.dragEnterEvent

        if self._initial_topics:
            for topic_name in self._initial_topics:
                self.add_topic(topic_name)
            self._initial_topics = None
        else:
            for topic_name, rosdata in self._rosdata.items():
                data_x, data_y = rosdata.next()
                self.data_plot.add_curve(topic_name, topic_name, data_x,
                                         data_y)

        self._subscribed_topics_changed()

    @Slot('QDragEnterEvent*')
    def dragEnterEvent(self, event):
        # get topic name
        if not event.mimeData().hasText():
            if not hasattr(event.source(), 'selectedItems') or \
                    len(event.source().selectedItems()) == 0:
                qWarning(
                    'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or '
                    'len(event.source().selectedItems()) == 0')
                return
            item = event.source().selectedItems()[0]
            topic_name = item.data(0, Qt.UserRole)
            if topic_name == None:
                qWarning(
                    'Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)'
                )
                return
        else:
            topic_name = str(event.mimeData().text())

        # check for plottable field type
        plottable, message = is_plottable(self._node, topic_name)
        if plottable:
            event.acceptProposedAction()
        else:
            qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message))

    @Slot('QDropEvent*')
    def dropEvent(self, event):
        if event.mimeData().hasText():
            topic_name = str(event.mimeData().text())
        else:
            droped_item = event.source().selectedItems()[0]
            topic_name = str(droped_item.data(0, Qt.UserRole))
        self.add_topic(topic_name)

    @Slot(str)
    def on_topic_edit_textChanged(self, topic_name):
        # on empty topic name, update topics
        if topic_name in ('', '/'):
            self._topic_completer.update_topics(self._node)

        plottable, message = is_plottable(self._node, topic_name)
        self.subscribe_topic_button.setEnabled(plottable)
        self.subscribe_topic_button.setToolTip(message)

    @Slot()
    def on_topic_edit_returnPressed(self):
        if self.subscribe_topic_button.isEnabled():
            self.add_topic(str(self.topic_edit.text()))

    @Slot()
    def on_subscribe_topic_button_clicked(self):
        self.add_topic(str(self.topic_edit.text()))

    @Slot(bool)
    def on_pause_button_clicked(self, checked):
        self.enable_timer(not checked)

    @Slot(bool)
    def on_autoscroll_checkbox_clicked(self, checked):
        self.data_plot.autoscroll(checked)
        if checked:
            self.data_plot.redraw()

    @Slot()
    def on_clear_button_clicked(self):
        self.clear_plot()

    def update_plot(self):
        if self.data_plot is not None:
            needs_redraw = False
            for topic_name, rosdata in self._rosdata.items():
                try:
                    data_x, data_y = rosdata.next()
                    if data_x or data_y:
                        self.data_plot.update_values(topic_name, data_x,
                                                     data_y)
                        needs_redraw = True
                except RosPlotException as e:
                    qWarning('PlotWidget.update_plot(): error in rosplot: %s' %
                             e)
            if needs_redraw:
                self.data_plot.redraw()

    def _subscribed_topics_changed(self):
        self._update_remove_topic_menu()
        if not self.pause_button.isChecked():
            # if pause button is not pressed, enable timer based on subscribed topics
            self.enable_timer(self._rosdata)
        self.data_plot.redraw()

    def _update_remove_topic_menu(self):
        def make_remove_topic_function(x):
            return lambda: self.remove_topic(x)

        self._remove_topic_menu.clear()
        for topic_name in sorted(self._rosdata.keys()):
            action = QAction(topic_name, self._remove_topic_menu)
            action.triggered.connect(make_remove_topic_function(topic_name))
            self._remove_topic_menu.addAction(action)

        if len(self._rosdata) > 1:
            all_action = QAction('All', self._remove_topic_menu)
            all_action.triggered.connect(self.clean_up_subscribers)
            self._remove_topic_menu.addAction(all_action)

        self.remove_topic_button.setMenu(self._remove_topic_menu)

    def add_topic(self, topic_name):
        topics_changed = False
        for topic_name in get_plot_fields(self._node, topic_name)[0]:
            if topic_name in self._rosdata:
                qWarning(
                    'PlotWidget.add_topic(): topic already subscribed: %s' %
                    topic_name)
                continue
            self._rosdata[topic_name] = ROSData(self._node, topic_name,
                                                self._start_time)
            if self._rosdata[topic_name].error is not None:
                qWarning(str(self._rosdata[topic_name].error))
                del self._rosdata[topic_name]
            else:
                data_x, data_y = self._rosdata[topic_name].next()
                self.data_plot.add_curve(topic_name, topic_name, data_x,
                                         data_y)
                topics_changed = True

        if topics_changed:
            self._subscribed_topics_changed()

    def remove_topic(self, topic_name):
        self._rosdata[topic_name].close()
        del self._rosdata[topic_name]
        self.data_plot.remove_curve(topic_name)

        self._subscribed_topics_changed()

    def clear_plot(self):
        for topic_name, _ in self._rosdata.items():
            self.data_plot.clear_values(topic_name)
        self.data_plot.redraw()

    def clean_up_subscribers(self):
        for topic_name, rosdata in self._rosdata.items():
            rosdata.close()
            self.data_plot.remove_curve(topic_name)
        self._rosdata = {}

        self._subscribed_topics_changed()

    def enable_timer(self, enabled=True):
        if enabled:
            self._update_plot_timer.start(self._redraw_interval)
        else:
            self._update_plot_timer.stop()
Beispiel #41
0
    def __init__(self, context):
        super(JointTrajectoryController, self).__init__(context)
        self.setObjectName('JointTrajectoryController')

        self._node = context.node

        # Get the robot_description via a topic
        qos_profile = QoSProfile(depth=1,
                                 history=HistoryPolicy.KEEP_LAST,
                                 durability=DurabilityPolicy.TRANSIENT_LOCAL)
        self._robot_description_sub = self._node.create_subscription(
            String, 'robot_description',
            self._robot_description_cb, qos_profile)
        self._robot_description = None

        self._publisher = None
        self._widget = QWidget()

        _, package_path = get_resource('packages', 'rqt_joint_trajectory_controller')
        ui_file = os.path.join(package_path, 'share', 'rqt_joint_trajectory_controller', 'resource', 'joint_trajectory_controller.ui')
        loadUi(ui_file, self._widget)

        self._widget.setObjectName('JointTrajectoryControllerUi')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(
                self._widget.windowTitle() + (' (%d)' % context.serial_number()))
        context.add_widget(self._widget)

        ns = self._node.get_namespace()
        self._widget.controller_group.setTitle('ns: ' + ns)

        # Setup speed scaler
        speed_scaling = DoubleEditor(1.0, 100.0)
        speed_scaling.spin_box.setSuffix('%')
        speed_scaling.spin_box.setValue(50.0)
        speed_scaling.spin_box.setDecimals(0)
        speed_scaling.setEnabled(False)
        self._widget.speed_scaling_layout.addWidget(speed_scaling)
        self._speed_scaling_widget = speed_scaling
        speed_scaling.valueChanged.connect(self._on_speed_scaling_change)
        self._on_speed_scaling_change(speed_scaling.value())

        # 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 widget to the user interface
        context.add_widget(self._widget)

        # Initialize members
        self._jtc_name = []  # Name of selected joint trajectory controller
        self._cm_ns = []  # Namespace of the selected controller manager
        self._joint_pos = {}  # name->pos map for joints of selected controller
        self._joint_names = []  # Ordered list of selected controller joints
        self._robot_joint_limits = {} # Lazily evaluated on first use

        # Timer for sending commands to active controller
        self._update_cmd_timer = QTimer(self)
        self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq)
        self._update_cmd_timer.timeout.connect(self._update_cmd_cb)

        # Timer for updating the joint widgets from the controller state
        self._update_act_pos_timer = QTimer(self)
        self._update_act_pos_timer.setInterval(1000.0 /
                                               self._widget_update_freq)
        self._update_act_pos_timer.timeout.connect(self._update_joint_widgets)

        # Timer for controller manager updates
        # TODO: self._list_cm = ControllerManagerLister()
        self._update_cm_list_timer = QTimer(self)
        self._update_cm_list_timer.setInterval(1000.0 /
                                               self._ctrlrs_update_freq)
        self._update_cm_list_timer.timeout.connect(self._update_cm_list)
        self._update_cm_list_timer.start()

        # Timer for running controller updates
        self._update_jtc_list_timer = QTimer(self)
        self._update_jtc_list_timer.setInterval(1000.0 /
                                                self._ctrlrs_update_freq)
        self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
        self._update_jtc_list_timer.start()

        # Signal connections
        w = self._widget
        w.enable_button.toggled.connect(self._on_jtc_enabled)
        w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
        w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)

        self._cmd_pub = None    # Controller command publisher
        self._state_sub = None  # Controller state subscriber

        self._list_controllers = None

        self._traj_ns_topic_dict = None
Beispiel #42
0
class RobotMonitorWidget(QWidget):
    """
    NOTE: RobotMonitorWidget.shutdown function needs to be called
    when the instance of this class terminates.

    RobotMonitorWidget itself doesn't store previous diagnostic states.
    It instead delegates that function to Timeline class.
    """

    _TREE_ALL = 1
    _TREE_WARN = 2
    _TREE_ERR = 3

    message_updated = Signal(DiagnosticArray)

    def __init__(self, context, topic=None):
        """
        :param context: plugin context hook to enable adding widgets as a
                        ROS_GUI pane, 'PluginContext'
        :param topic: Diagnostic topic to subscribe to 'str'
        """

        super(RobotMonitorWidget, self).__init__()
        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('rqt_robot_monitor'), 'resource',
                               'robotmonitor_mainwidget.ui')
        loadUi(ui_file, self)

        obj_name = 'Robot Monitor'
        self.setObjectName(obj_name)
        self.setWindowTitle(obj_name)

        self.message_updated.connect(self.message_cb)

        # if we're given a topic, create a timeline. otherwise, don't
        #  this can be used later when writing an rqt_bag plugin
        if topic:
            # create timeline data structure
            self._timeline = Timeline(topic, DiagnosticArray)
            self._timeline.message_updated.connect(self.message_updated)

            # create timeline pane widget
            self.timeline_pane = TimelinePane(self)

            self.timeline_pane.set_timeline(self._timeline)

            self.vlayout_top.addWidget(self.timeline_pane)
            self.timeline_pane.show()
        else:
            self._timeline = None
            self.timeline_pane = None

        self._inspectors = {}
        # keep a copy of the current message for opening new inspectors
        self._current_msg = None

        self.tree_all_devices.itemDoubleClicked.connect(self._tree_clicked)
        self.warn_flattree.itemDoubleClicked.connect(self._tree_clicked)
        self.err_flattree.itemDoubleClicked.connect(self._tree_clicked)
        # TODO: resize on itemCollapsed and itemExpanded

        self._is_stale = False

        self._timer = QTimer()
        self._timer.timeout.connect(self._update_message_state)
        self._timer.start(1000)

        palette = self.tree_all_devices.palette()
        self._original_base_color = palette.base().color()
        self._original_alt_base_color = palette.alternateBase().color()

        self._tree = StatusItem(self.tree_all_devices.invisibleRootItem())
        self._warn_tree = StatusItem(self.warn_flattree.invisibleRootItem())
        self._err_tree = StatusItem(self.err_flattree.invisibleRootItem())

    @Slot(DiagnosticArray)
    def message_cb(self, msg):
        """ DiagnosticArray message callback """
        self._current_msg = msg

        # Walk the status array and update the tree
        for status in msg.status:
            # Compute path and walk to appropriate subtree
            path = status.name.split('/')
            if path[0] == '':
                path = path[1:]
            tmp_tree = self._tree
            for p in path:
                tmp_tree = tmp_tree[p]
            tmp_tree.update(status, util.get_resource_name(status.name))

            # Check for warnings
            if status.level == DiagnosticStatus.WARN:
                name = status.name
                self._warn_tree[name].update(status, status.name)

            # Check for errors
            if status.level == DiagnosticStatus.ERROR:
                name = status.name
                self._err_tree[name].update(status, status.name)

        # For any items in the tree that were not updated, remove them
        self._tree.prune()
        self._warn_tree.prune()
        self._err_tree.prune()

        # TODO(ahendrix): implement
        # Insight: for any item that is not OK, it only provides additional
        #          information if all of it's children are OK
        #
        #          otherwise, it's just an aggregation of its children
        #          and doesn't provide any additional value when added to
        #          the warning and error flat trees

        self.tree_all_devices.resizeColumnToContents(0)
        self.warn_flattree.resizeColumnToContents(0)
        self.err_flattree.resizeColumnToContents(0)

    def resizeEvent(self, evt):
        """Overridden from QWidget"""
        rospy.logdebug('RobotMonitorWidget resizeEvent')
        if self.timeline_pane:
            self.timeline_pane.redraw()

    @Slot(str)
    def _inspector_closed(self, name):
        """ Called when an inspector window is closed """
        if name in self._inspectors:
            del self._inspectors[name]

    def _tree_clicked(self, item, column):
        """
        Slot to QTreeWidget.itemDoubleClicked

        :type item: QTreeWidgetItem
        :type column: int
        """
        rospy.logdebug('RobotMonitorWidget _tree_clicked col=%d', column)
        if item.name in self._inspectors:
            self._inspectors[item.name].activateWindow()
        else:
            self._inspectors[item.name] = InspectorWindow(
                self, item.name, self._current_msg, self._timeline)
            self._inspectors[item.name].closed.connect(self._inspector_closed)
            self.message_updated.connect(
                self._inspectors[item.name].message_updated)

    def _update_message_state(self):
        """ Update the display if it's stale """
        if self._timeline is not None:
            if self._timeline.has_messages:
                previous_stale_state = self._is_stale
                self._is_stale = self._timeline.is_stale

                time_diff = int(self._timeline.data_age())

                msg_template = "Last message received %s %s ago"
                if time_diff == 1:
                    msg = msg_template % (time_diff, "second")
                else:
                    msg = msg_template % (time_diff, "seconds")
                self.timeline_pane._msg_label.setText(msg)
                if previous_stale_state != self._is_stale:
                    self._update_background_color()
            else:
                # no messages received yet
                self.timeline_pane._msg_label.setText("No messages received")

    def _update_background_color(self):
        """ Update the background color based on staleness """
        p = self.tree_all_devices.palette()
        if self._is_stale:
            p.setColor(QPalette.Base, Qt.darkGray)
            p.setColor(QPalette.AlternateBase, Qt.lightGray)
        else:
            p.setColor(QPalette.Base, self._original_base_color)
            p.setColor(QPalette.AlternateBase, self._original_alt_base_color)
        self.tree_all_devices.setPalette(p)
        self.warn_flattree.setPalette(p)
        self.err_flattree.setPalette(p)

    def shutdown(self):
        """
        This needs to be called whenever this class terminates.
        This closes all the instances on all trees.
        Also unregisters ROS' subscriber, stops timer.
        """
        rospy.logdebug('RobotMonitorWidget in shutdown')

        names = self._inspectors.keys()
        for name in names:
            self._inspectors[name].close()

        if self._timeline:
            self._timeline.shutdown()

        self._timer.stop()
        del self._timer

    def save_settings(self, plugin_settings, instance_settings):
        instance_settings.set_value('splitter', self.splitter.saveState())
        # TODO(ahendrix): persist the device paths, positions and sizes of any
        #                 inspector windows

    def restore_settings(self, plugin_settings, instance_settings):
        if instance_settings.contains('splitter'):
            self.splitter.restoreState(instance_settings.value('splitter'))
        else:
            self.splitter.setSizes([100, 100, 200])
Beispiel #43
0
class PlanViewWidget:

    # plan view
    _column_names = [
        'action_id', 'dispatch_time', 'action_name', 'duration', 'status'
    ]
    _action_list = []
    _status_list = {}
    _predicate_param_type_list = {}
    _predicate_param_label_list = {}

    # model view
    _type_list = []
    _goal_list = {}
    _fact_list = {}

    def __init__(self):

        # Create QWidget
        self.setObjectName('ROSPlanGraphUpdater')

        # init and start update timers
        self._timer_refresh_plan = QTimer(self)
        self._timer_refresh_plan.timeout.connect(self.refresh_plan)

        rospy.Subscriber("/kcl_rosplan/plan", CompletePlan, self.plan_callback)
        rospy.Subscriber("/kcl_rosplan/action_feedback", ActionFeedback,
                         self.action_feedback_callback)

    def start(self):
        self._timer_refresh_plan.start(1000)
        self._timer_refresh_goals.start(10000)

    """
    updating plan view
    """

    def refresh_plan(self):
        expanded_list = []
        root = self.planView.invisibleRootItem()
        child_count = root.childCount()
        for i in range(child_count):
            item = root.child(i)
            if item.isExpanded():
                expanded_list.append(item.text(
                    self._column_index['action_id']))
        self.planView.clear()
        for action in self._action_list:
            item = QTreeWidgetItem(self.planView)
            item.setText(self._column_index['action_id'],
                         str(action.action_id))
            item.setText(self._column_index['dispatch_time'],
                         str(action.dispatch_time))
            item.setText(self._column_index['duration'], str(action.duration))
            item.setText(self._column_index['status'],
                         self._status_list.get(str(action.action_id), "-"))
            action_name = '(' + action.name
            for keyval in action.parameters:
                param = QTreeWidgetItem(item)
                param.setText(self._column_index['action_id'], '')
                param.setText(self._column_index['action_name'],
                              '- ' + keyval.key + ': ' + keyval.value)
                param.setText(self._column_index['dispatch_time'], '')
                param.setText(self._column_index['duration'], '')
                param.setText(self._column_index['status'], '')
                action_name = action_name + ' ' + keyval.value
            item.setText(self._column_index['action_name'], action_name + ')')
            if str(action.action_id) in expanded_list:
                item.setExpanded(True)

    """
    callback for complete_plan
    """

    def plan_callback(self, data):
        self._action_list = data.plan

    """
    callback for action_feedback
    """

    def action_feedback_callback(self, data):
        self._status_list[str(data.action_id)] = data.status
        self.refresh_model()
Beispiel #44
0
class BagTimeline(QGraphicsScene):
    """
    BagTimeline contains bag files, all information required to display the bag data visualization on the screen
    Also handles events
    """
    status_bar_changed_signal = Signal()
    selected_region_changed = Signal(rospy.Time, rospy.Time)
    make_pop_up = Signal()

    def __init__(self, context, publish_clock):
        """
        :param context: plugin context hook to enable adding rqt_bag plugin widgets as ROS_GUI snapin panes, ''PluginContext''
        """
        super(BagTimeline, self).__init__()
        self._bags = []
        self._bag_lock = threading.RLock()

        self.background_task = None  # Display string
        self.background_task_cancel = False

        # Playing / Recording
        self._playhead_lock = threading.RLock()
        self._max_play_speed = 1024.0  # fastest X play speed
        self._min_play_speed = 1.0 / 1024.0  # slowest X play speed
        self._play_speed = 0.0
        self._play_all = False
        self._playhead_positions_cvs = {}
        self._playhead_positions = {}  # topic -> (bag, position)
        self._message_loaders = {}
        self._messages_cvs = {}
        self._messages = {}  # topic -> (bag, msg_data)
        self._message_listener_threads = {
        }  # listener -> MessageListenerThread
        self._player = False
        self._publish_clock = publish_clock
        self._recorder = None
        self.last_frame = None
        self.last_playhead = None
        self.desired_playhead = None
        self.wrap = True  # should the playhead wrap when it reaches the end?
        self.stick_to_end = False  # should the playhead stick to the end?
        self._play_timer = QTimer()
        self._play_timer.timeout.connect(self.on_idle)
        self._play_timer.setInterval(3)

        # Plugin popup management
        self._context = context
        self.popups = {}
        self._views = []
        self._listeners = {}

        # Initialize scene
        # the timeline renderer fixes use of black pens and fills, so ensure we fix white here for contrast.
        # otherwise a dark qt theme will default it to black and the frame render pen will be unreadable
        self.setBackgroundBrush(Qt.white)
        self._timeline_frame = TimelineFrame(self)
        self._timeline_frame.setPos(0, 0)
        self.addItem(self._timeline_frame)

        self.background_progress = 0
        self.__closed = False

    def get_context(self):
        """
        :returns: the ROS_GUI context, 'PluginContext'
        """
        return self._context

    def handle_close(self):
        """
        Cleans up the timeline, bag and any threads
        """
        # with open("/var/tmp/test.txt", "a") as myfile:
        #     myfile.write("dan\n")

        if self.__closed:
            return
        else:
            self.__closed = True
        self._play_timer.stop()
        for topic in self._get_topics():
            self.stop_publishing(topic)
            self._message_loaders[topic].stop()
        if self._player:
            self._player.stop()
        if self._recorder:
            self._recorder.stop()
        if self.background_task is not None:
            self.background_task_cancel = True
        self._timeline_frame.handle_close()
        for bag in self._bags:
            bag.close()
        for frame in self._views:
            if frame.parent():
                self._context.remove_widget(frame)

    # Bag Management and access
    def add_bag(self, bag):
        """
        creates an indexing thread for each new topic in the bag
        fixes the boarders and notifies the indexing thread to index the new items bags
        :param bag: ros bag file, ''rosbag.bag''
        """
        self._bags.append(bag)

        bag_topics = bag_helper.get_topics(bag)

        new_topics = set(bag_topics) - set(self._timeline_frame.topics)

        for topic in new_topics:
            self._playhead_positions_cvs[topic] = threading.Condition()
            self._messages_cvs[topic] = threading.Condition()
            self._message_loaders[topic] = MessageLoaderThread(self, topic)

        self._timeline_frame._start_stamp = self._get_start_stamp()
        self._timeline_frame._end_stamp = self._get_end_stamp()
        self._timeline_frame.topics = self._get_topics()
        self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype(
        )
        # If this is the first bag, reset the timeline
        if self._timeline_frame._stamp_left is None:
            self._timeline_frame.reset_timeline()

        # Invalidate entire index cache for all topics in this bag
        with self._timeline_frame.index_cache_cv:
            for topic in bag_topics:
                self._timeline_frame.invalidated_caches.add(topic)
                if topic in self._timeline_frame.index_cache:
                    del self._timeline_frame.index_cache[topic]

            self._timeline_frame.index_cache_cv.notify()

    def file_size(self):
        with self._bag_lock:
            return sum(b.size for b in self._bags)

    #TODO Rethink API and if these need to be visible
    def _get_start_stamp(self):
        """
        :return: first stamp in the bags, ''rospy.Time''
        """
        with self._bag_lock:
            start_stamp = None
            for bag in self._bags:
                bag_start_stamp = bag_helper.get_start_stamp(bag)
                if bag_start_stamp is not None and (
                        start_stamp is None or bag_start_stamp < start_stamp):
                    start_stamp = bag_start_stamp
            return start_stamp

    def _get_end_stamp(self):
        """
        :return: last stamp in the bags, ''rospy.Time''
        """
        with self._bag_lock:
            end_stamp = None
            for bag in self._bags:
                bag_end_stamp = bag_helper.get_end_stamp(bag)
                if bag_end_stamp is not None and (end_stamp is None or
                                                  bag_end_stamp > end_stamp):
                    end_stamp = bag_end_stamp
            return end_stamp

    def _get_topics(self):
        """
        :return: sorted list of topic names, ''list(str)''
        """
        with self._bag_lock:
            topics = set()
            for bag in self._bags:
                for topic in bag_helper.get_topics(bag):
                    topics.add(topic)
            return sorted(topics)

    def _get_topics_by_datatype(self):
        """
        :return: dict of list of topics for each datatype, ''dict(datatype:list(topic))''
        """
        with self._bag_lock:
            topics_by_datatype = {}
            for bag in self._bags:
                for datatype, topics in bag_helper.get_topics_by_datatype(
                        bag).items():
                    topics_by_datatype.setdefault(datatype, []).extend(topics)
            return topics_by_datatype

    def get_datatype(self, topic):
        """
        :return: datatype associated with a topic, ''str''
        :raises: if there are multiple datatypes assigned to a single topic, ''Exception''
        """
        with self._bag_lock:
            datatype = None
            for bag in self._bags:
                bag_datatype = bag_helper.get_datatype(bag, topic)
                if datatype and bag_datatype and (bag_datatype != datatype):
                    raise Exception(
                        'topic %s has multiple datatypes: %s and %s' %
                        (topic, datatype, bag_datatype))
                if bag_datatype:
                    datatype = bag_datatype
            return datatype

    def get_entries(self, topics, start_stamp, end_stamp):
        """
        generator function for bag entries
        :param topics: list of topics to query, ''list(str)''
        :param start_stamp: stamp to start at, ''rospy.Time''
        :param end_stamp: stamp to end at, ''rospy,Time''
        :returns: entries the bag file, ''msg''
        """
        with self._bag_lock:
            from rosbag import bag  # for _mergesort
            bag_entries = []
            for b in self._bags:
                bag_start_time = bag_helper.get_start_stamp(b)
                if bag_start_time is not None and bag_start_time > end_stamp:
                    continue

                bag_end_time = bag_helper.get_end_stamp(b)
                if bag_end_time is not None and bag_end_time < start_stamp:
                    continue

                connections = list(b._get_connections(topics))
                bag_entries.append(
                    b._get_entries(connections, start_stamp, end_stamp))

            for entry, _ in bag._mergesort(bag_entries,
                                           key=lambda entry: entry.time):
                yield entry

    def get_entries_with_bags(self, topic, start_stamp, end_stamp):
        """
        generator function for bag entries
        :param topics: list of topics to query, ''list(str)''
        :param start_stamp: stamp to start at, ''rospy.Time''
        :param end_stamp: stamp to end at, ''rospy,Time''
        :returns: tuple of (bag, entry) for the entries in the bag file, ''(rosbag.bag, msg)''
        """
        with self._bag_lock:
            from rosbag import bag  # for _mergesort

            bag_entries = []
            bag_by_iter = {}
            for b in self._bags:
                bag_start_time = bag_helper.get_start_stamp(b)
                if bag_start_time is not None and bag_start_time > end_stamp:
                    continue

                bag_end_time = bag_helper.get_end_stamp(b)
                if bag_end_time is not None and bag_end_time < start_stamp:
                    continue

                connections = list(b._get_connections(topic))
                it = iter(b._get_entries(connections, start_stamp, end_stamp))
                bag_by_iter[it] = b
                bag_entries.append(it)

            for entry, it in bag._mergesort(bag_entries,
                                            key=lambda entry: entry.time):
                yield bag_by_iter[it], entry

    def get_entry(self, t, topic):
        """
        Access a bag entry
        :param t: time, ''rospy.Time''
        :param topic: the topic to be accessed, ''str''
        :return: tuple of (bag, entry) corisponding to time t and topic, ''(rosbag.bag, msg)''
        """
        with self._bag_lock:
            entry_bag, entry = None, None
            for bag in self._bags:
                bag_entry = bag._get_entry(t, bag._get_connections(topic))
                if bag_entry and (not entry or bag_entry.time > entry.time):
                    entry_bag, entry = bag, bag_entry

            return entry_bag, entry

    def get_entry_before(self, t):
        """
        Access a bag entry
        :param t: time, ''rospy.Time''
        :return: tuple of (bag, entry) corresponding to time t, ''(rosbag.bag, msg)''
        """
        with self._bag_lock:
            entry_bag, entry = None, None
            for bag in self._bags:
                bag_entry = bag._get_entry(t - rospy.Duration(0, 1),
                                           bag._get_connections())
                if bag_entry and (not entry or bag_entry.time < entry.time):
                    entry_bag, entry = bag, bag_entry

            return entry_bag, entry

    def get_entry_after(self, t):
        """
        Access a bag entry
        :param t: time, ''rospy.Time''
        :return: tuple of (bag, entry) corisponding to time t, ''(rosbag.bag, msg)''
        """
        with self._bag_lock:
            entry_bag, entry = None, None
            for bag in self._bags:
                bag_entry = bag._get_entry_after(t, bag._get_connections())
                if bag_entry and (not entry or bag_entry.time < entry.time):
                    entry_bag, entry = bag, bag_entry

            return entry_bag, entry

    def get_next_message_time(self):
        """
        :return: time of the next message after the current playhead position,''rospy.Time''
        """
        if self._timeline_frame.playhead is None:
            return None

        _, entry = self.get_entry_after(self._timeline_frame.playhead)
        if entry is None:
            return self._timeline_frame._start_stamp

        return entry.time

    # def get_previous_message_time(self):
    #     """
    #     :return: time of the next message before the current playhead position,''rospy.Time''
    #     """
    #     if self._timeline_frame.playhead is None:
    #         return None
    #
    #     _, entry = self.get_entry_before(self._timeline_frame.playhead)
    #     if entry is None:
    #         return self._timeline_frame._end_stamp
    #
    #     return entry.time

    def resume(self):
        if (self._player):
            self._player.resume()

    ### Copy messages to...

    def start_background_task(self, background_task):
        """
        Verify that a background task is not currently running before starting a new one
        :param background_task: name of the background task, ''str''
        """
        if self.background_task is not None:
            QMessageBox(
                QMessageBox.Warning, 'Exclamation',
                'Background operation already running:\n\n%s' %
                self.background_task, QMessageBox.Ok).exec_()
            return False

        self.background_task = background_task
        self.background_task_cancel = False
        return True

    def stop_background_task(self):
        self.background_task = None

    def copy_region_to_bag(self, filename):
        if len(self._bags) > 0:
            self._export_region(filename, self._timeline_frame.topics,
                                self._timeline_frame.play_region[0],
                                self._timeline_frame.play_region[1])

    def _export_region(self, path, topics, start_stamp, end_stamp):
        """
        Starts a thread to save the current selection to a new bag file
        :param path: filesystem path to write to, ''str''
        :param topics: topics to write to the file, ''list(str)''
        :param start_stamp: start of area to save, ''rospy.Time''
        :param end_stamp: end of area to save, ''rospy.Time''
        """
        if not self.start_background_task('Copying messages to "%s"' % path):
            return
        # TODO implement a status bar area with information on the current save status
        bag_entries = list(
            self.get_entries_with_bags(topics, start_stamp, end_stamp))

        if self.background_task_cancel:
            return

        # Get the total number of messages to copy
        total_messages = len(bag_entries)

        # If no messages, prompt the user and return
        if total_messages == 0:
            QMessageBox(QMessageBox.Warning, 'rqt_bag', 'No messages found',
                        QMessageBox.Ok).exec_()
            self.stop_background_task()
            return

        # Open the path for writing
        try:
            export_bag = rosbag.Bag(path, 'w')
        except Exception:
            QMessageBox(QMessageBox.Warning, 'rqt_bag',
                        'Error opening bag file [%s] for writing' % path,
                        QMessageBox.Ok).exec_()
            self.stop_background_task()
            return

        # Run copying in a background thread
        self._export_thread = threading.Thread(target=self._run_export_region,
                                               args=(export_bag, topics,
                                                     start_stamp, end_stamp,
                                                     bag_entries))
        self._export_thread.start()

    def _run_export_region(self, export_bag, topics, start_stamp, end_stamp,
                           bag_entries):
        """
        Threaded function that saves the current selection to a new bag file
        :param export_bag: bagfile to write to, ''rosbag.bag''
        :param topics: topics to write to the file, ''list(str)''
        :param start_stamp: start of area to save, ''rospy.Time''
        :param end_stamp: end of area to save, ''rospy.Time''
        """
        total_messages = len(bag_entries)
        update_step = max(1, total_messages / 100)
        message_num = 1
        progress = 0
        # Write out the messages
        for bag, entry in bag_entries:
            if self.background_task_cancel:
                break
            try:
                topic, msg, t = self.read_message(bag, entry.position)
                export_bag.write(topic, msg, t)
            except Exception as ex:
                qWarning('Error exporting message at position %s: %s' %
                         (str(entry.position), str(ex)))
                export_bag.close()
                self.stop_background_task()
                return

            if message_num % update_step == 0 or message_num == total_messages:
                new_progress = int(100.0 *
                                   (float(message_num) / total_messages))
                if new_progress != progress:
                    progress = new_progress
                    if not self.background_task_cancel:
                        self.background_progress = progress
                        self.status_bar_changed_signal.emit()

            message_num += 1

        # Close the bag
        try:
            self.background_progress = 0
            self.status_bar_changed_signal.emit()
            export_bag.close()
        except Exception as ex:
            QMessageBox(
                QMessageBox.Warning, 'rqt_bag',
                'Error closing bag file [%s]: %s' %
                (export_bag.filename, str(ex)), QMessageBox.Ok).exec_()
        self.stop_background_task()

    def read_message(self, bag, position):
        with self._bag_lock:
            return bag._read_message(position)

    ### Mouse events
    def on_mouse_down(self, event):
        if event.buttons() == Qt.LeftButton:
            self._timeline_frame.on_left_down(event)
        elif event.buttons() == Qt.MidButton:
            self._timeline_frame.on_middle_down(event)
        elif event.buttons() == Qt.RightButton:
            topic = self._timeline_frame.map_y_to_topic(
                self.views()[0].mapToScene(event.pos()).y())
            TimelinePopupMenu(self, event, topic)

    def on_mouse_up(self, event):
        self._timeline_frame.on_mouse_up(event)

    def on_mouse_move(self, event):
        self._timeline_frame.on_mouse_move(event)

    def on_mousewheel(self, event):
        self._timeline_frame.on_mousewheel(event)

    # Zooming

    def zoom_in(self):
        self._timeline_frame.zoom_in()

    def zoom_out(self):
        self._timeline_frame.zoom_out()

    def reset_zoom(self):
        self._timeline_frame.reset_zoom()

    def translate_timeline_left(self):
        self._timeline_frame.translate_timeline_left()

    def translate_timeline_right(self):
        self._timeline_frame.translate_timeline_right()

    ### Publishing
    def is_publishing(self, topic):
        return self._player and self._player.is_publishing(topic)

    def start_publishing(self, topic):
        if not self._player and not self._create_player():
            return False

        self._player.start_publishing(topic)
        return True

    def stop_publishing(self, topic):
        if not self._player:
            return False

        self._player.stop_publishing(topic)
        return True

    def _create_player(self):
        if not self._player:
            try:
                self._player = Player(self)
                if self._publish_clock:
                    self._player.start_clock_publishing()
            except Exception as ex:
                qWarning('Error starting player; aborting publish: %s' %
                         str(ex))
                return False

        return True

    def set_publishing_state(self, start_publishing):
        if start_publishing:
            for topic in self._timeline_frame.topics:
                if not self.start_publishing(topic):
                    break
        else:
            for topic in self._timeline_frame.topics:
                self.stop_publishing(topic)

    # property: play_all
    def _get_play_all(self):
        return self._play_all

    def _set_play_all(self, play_all):
        if play_all == self._play_all:
            return

        self._play_all = not self._play_all

        self.last_frame = None
        self.last_playhead = None
        self.desired_playhead = None

    play_all = property(_get_play_all, _set_play_all)

    def toggle_play_all(self):
        self.play_all = not self.play_all

    ### Playing
    def on_idle(self):
        self._step_playhead()

    def _step_playhead(self):
        """
        moves the playhead to the next position based on the desired position
        """
        # Reset when the playing mode switchs
        if self._timeline_frame.playhead != self.last_playhead:
            self.last_frame = None
            self.last_playhead = None
            self.desired_playhead = None

        if self._play_all:
            self.step_next_message()
        else:
            self.step_fixed()

    def step_fixed(self):
        """
        Moves the playhead a fixed distance into the future based on the current play speed
        """
        if self.play_speed == 0.0 or not self._timeline_frame.playhead:
            self.last_frame = None
            self.last_playhead = None
            return

        now = rospy.Time.from_sec(time.time())
        if self.last_frame:
            # Get new playhead
            if self.stick_to_end:
                new_playhead = self.end_stamp
            else:
                new_playhead = self._timeline_frame.playhead + rospy.Duration.from_sec(
                    (now - self.last_frame).to_sec() * self.play_speed)

                start_stamp, end_stamp = self._timeline_frame.play_region

                if new_playhead > end_stamp:
                    if self.wrap:
                        if self.play_speed > 0.0:
                            new_playhead = start_stamp
                        else:
                            new_playhead = end_stamp
                    else:
                        new_playhead = end_stamp

                        if self.play_speed > 0.0:
                            self.stick_to_end = True

                elif new_playhead < start_stamp:
                    if self.wrap:
                        if self.play_speed < 0.0:
                            new_playhead = end_stamp
                        else:
                            new_playhead = start_stamp
                    else:
                        new_playhead = start_stamp

            # Update the playhead
            self._timeline_frame.playhead = new_playhead

        self.last_frame = now
        self.last_playhead = self._timeline_frame.playhead

    def step_next_message(self):
        """
        Move the playhead to the next message
        """
        if self.play_speed <= 0.0 or not self._timeline_frame.playhead:
            self.last_frame = None
            self.last_playhead = None
            return

        if self.last_frame:
            if not self.desired_playhead:
                self.desired_playhead = self._timeline_frame.playhead
            else:
                delta = rospy.Time.from_sec(time.time()) - self.last_frame
                if delta > rospy.Duration.from_sec(0.1):
                    delta = rospy.Duration.from_sec(0.1)
                self.desired_playhead += delta

            # Get the occurrence of the next message
            next_message_time = self.get_next_message_time()

            if next_message_time < self.desired_playhead:
                self._timeline_frame.playhead = next_message_time
            else:
                self._timeline_frame.playhead = self.desired_playhead

        self.last_frame = rospy.Time.from_sec(time.time())
        self.last_playhead = self._timeline_frame.playhead

    ### Recording

    def record_bag(self, filename, all=True, topics=[], regex=False, limit=0):
        try:
            self._recorder = Recorder(filename,
                                      bag_lock=self._bag_lock,
                                      all=all,
                                      topics=topics,
                                      regex=regex,
                                      limit=limit)
        except Exception as ex:
            qWarning('Error opening bag for recording [%s]: %s' %
                     (filename, str(ex)))
            return

        self._recorder.add_listener(self._message_recorded)

        self.add_bag(self._recorder.bag)

        self._recorder.start()

        self.wrap = False
        self._timeline_frame._index_cache_thread.period = 0.1

        self.update()

    def restart_recording(self, reindex_bag):
        self._BagWidget.apply_restart(reindex_bag)
        import os
        # self._BagWidget.record_button.setIcon(QIcon.fromTheme('view-refresh'))
        # self._BagWidget.record_button.setToolTip("Refresh Screen")
        # self._BagWidget.restart_button.setEnabled(False)
        #
        # self._BagWidget.load_button.setEnabled(True)
        # self._BagWidget.history_button.setEnabled(True)
        #
        # self._BagWidget._restarting = True
        # self.handle_close()
        # if path != "":
        #     os.remove(path)
        # if restart_flag:

        # else:
        #     self.make_pop_up.emit()

    # def apply_record_icon(self):
    #     self._BagWidget.record_button.setIcon(QIcon.fromTheme('media-record'))

    def setBagWidget(self, BagWidget):
        self._BagWidget = BagWidget

    def toggle_recording(self):
        if self._recorder:
            self._recorder.toggle_paused(self._BagWidget)
            self.update()

    def _message_recorded(self, topic, msg, t):
        if self._timeline_frame._start_stamp is None:
            self._timeline_frame._start_stamp = t
            self._timeline_frame._end_stamp = t
            self._timeline_frame._playhead = t
        elif self._timeline_frame._end_stamp is None or t > self._timeline_frame._end_stamp:
            self._timeline_frame._end_stamp = t

        if not self._timeline_frame.topics or topic not in self._timeline_frame.topics:
            self._timeline_frame.topics = self._get_topics()
            self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype(
            )

            self._playhead_positions_cvs[topic] = threading.Condition()
            self._messages_cvs[topic] = threading.Condition()
            self._message_loaders[topic] = MessageLoaderThread(self, topic)

        if self._timeline_frame._stamp_left is None:
            self.reset_zoom()

        # Notify the index caching thread that it has work to do
        with self._timeline_frame.index_cache_cv:
            self._timeline_frame.invalidated_caches.add(topic)
            self._timeline_frame.index_cache_cv.notify()

        if topic in self._listeners:
            for listener in self._listeners[topic]:
                try:
                    listener.timeline_changed()
                except Exception as ex:
                    qWarning('Error calling timeline_changed on %s: %s' %
                             (type(listener), str(ex)))

    ### Views / listeners
    def add_view(self, topic, frame):
        self._views.append(frame)

    def has_listeners(self, topic):
        return topic in self._listeners

    def add_listener(self, topic, listener):
        self._listeners.setdefault(topic, []).append(listener)

        self._message_listener_threads[(topic,
                                        listener)] = MessageListenerThread(
                                            self, topic, listener)
        # Notify the message listeners
        self._message_loaders[topic].reset()
        with self._playhead_positions_cvs[topic]:
            self._playhead_positions_cvs[topic].notify_all()

        self.update()

    def remove_listener(self, topic, listener):
        topic_listeners = self._listeners.get(topic)
        if topic_listeners is not None and listener in topic_listeners:
            topic_listeners.remove(listener)

            if len(topic_listeners) == 0:
                del self._listeners[topic]

            # Stop the message listener thread
            if (topic, listener) in self._message_listener_threads:
                self._message_listener_threads[(topic, listener)].stop()
                del self._message_listener_threads[(topic, listener)]
            self.update()

    ### Playhead

    # property: play_speed
    def _get_play_speed(self):
        if self._timeline_frame._paused:
            return 0.0
        return self._play_speed

    def _set_play_speed(self, play_speed):
        if play_speed == self._play_speed:
            return

        if play_speed > 0.0:
            self._play_speed = min(self._max_play_speed,
                                   max(self._min_play_speed, play_speed))
        elif play_speed < 0.0:
            self._play_speed = max(-self._max_play_speed,
                                   min(-self._min_play_speed, play_speed))
        else:
            self._play_speed = play_speed

        if self._play_speed < 1.0:
            self.stick_to_end = False

        self.update()

    play_speed = property(_get_play_speed, _set_play_speed)

    def toggle_play(self):
        if self._play_speed != 0.0:
            self.play_speed = 0.0
        else:
            self.play_speed = 1.0

    # def navigate_play(self):
    #     self.play_speed = 1.0
    #     self.last_frame = rospy.Time.from_sec(time.time())
    #     self.last_playhead = self._timeline_frame.playhead
    #     self._play_timer.start()

    # def navigate_stop(self):
    #     self.play_speed = 0.0
    #     self._play_timer.stop()

    # def navigate_previous(self):
    #     self.navigate_stop()
    #     self._timeline_frame.playhead = self.get_previous_message_time()
    #     self.last_playhead = self._timeline_frame.playhead

    # def navigate_next(self):
    #     self.navigate_stop()
    #     self._timeline_frame.playhead = self.get_next_message_time()
    #     self.last_playhead = self._timeline_frame.playhead

    # def navigate_rewind(self):
    #     if self._play_speed < 0.0:
    #         new_play_speed = self._play_speed * 2.0
    #     elif self._play_speed == 0.0:
    #         new_play_speed = -1.0
    #     else:
    #         new_play_speed = self._play_speed * 0.5
    #
    #     self.play_speed = new_play_speed

    def navigate_fastforward(self):
        if self._play_speed > 0.0:
            new_play_speed = self._play_speed * 2.0
        elif self._play_speed == 0.0:
            new_play_speed = 2.0
        else:
            new_play_speed = self._play_speed * 0.5

        self.play_speed = new_play_speed
class PoseViewWidget(QWidget):

    def __init__(self, plugin):
        super(PoseViewWidget, self).__init__()
        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('rqt_pose_view'), 'resource', 'PoseViewWidget.ui')
        loadUi(ui_file, self)
        self._plugin = plugin

        self._position = (0.0, 0.0, 0.0)
        self._orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
        self._topic_name = None
        self._subscriber = None

        # create GL view
        self._gl_view = GLWidget()
        self._gl_view.setAcceptDrops(True)

        # backup and replace original paint method
        self._gl_view.paintGL_original = self._gl_view.paintGL
        self._gl_view.paintGL = self._gl_view_paintGL

        # backup and replace original mouse release method
        self._gl_view.mouseReleaseEvent_original = self._gl_view.mouseReleaseEvent
        self._gl_view.mouseReleaseEvent = self._gl_view_mouseReleaseEvent

        # add GL view to widget layout
        self.layout().addWidget(self._gl_view)

        # init and start update timer with 40ms (25fps)
        self._update_timer = QTimer(self)
        self._update_timer.timeout.connect(self.update_timeout)
        self._update_timer.start(40)

    def save_settings(self, plugin_settings, instance_settings):
        view_matrix_string = repr(self._gl_view.get_view_matrix())
        instance_settings.set_value('view_matrix', view_matrix_string)

    def restore_settings(self, plugin_settings, instance_settings):
        view_matrix_string = instance_settings.value('view_matrix')
        try:
            view_matrix = eval(view_matrix_string)
        except Exception:
            view_matrix = None

        if view_matrix is not None:
            self._gl_view.set_view_matrix(view_matrix)
        else:
            self._set_default_view()

    def _set_default_view(self):
        self._gl_view.makeCurrent()
        self._gl_view.reset_view()
        self._gl_view.rotate((0, 0, 1), 45)
        self._gl_view.rotate((1, 0, 0), -45)
        self._gl_view.translate((0, -3, -15))

    def message_callback_pose(self, message):
        self._position = (message.position.x, message.position.y, message.position.z)
        self._orientation = (message.orientation.x, message.orientation.y, message.orientation.z, message.orientation.w)
            
    def message_callback_quaternion(self, message):
        self._position = (0, 0, 0)
        self._orientation = (message.x, message.y, message.z, message.w)

    def update_timeout(self):
        self._gl_view.makeCurrent()
        self._gl_view.updateGL()

    def _gl_view_paintGL(self):
        self._gl_view.paintGL_original()
        self._paintGLGrid()
        self._paintGLCoorsystem()
        self._paintGLBox()

    def _paintGLBox(self):
        self._position = (2.0, 2.0, 2.0)  # Set fixed translation for now
        glTranslatef(*self._position)     # Translate Box

        matrix = quaternion_matrix(self._orientation)  # convert quaternion to translation matrix
        glMultMatrixf(matrix)             # Rotate Box

        glBegin(GL_QUADS)                 # Start Drawing The Box

        glColor3f(0.0, 1.0, 0.0)
        glVertex3f(1.0, 1.0, -1.0)        # Top Right Of The Quad (Top)
        glVertex3f(-1.0, 1.0, -1.0)       # Top Left Of The Quad (Top)
        glVertex3f(-1.0, 1.0, 1.0)        # Bottom Left Of The Quad (Top)
        glVertex3f(1.0, 1.0, 1.0)         # Bottom Right Of The Quad (Top)

        glColor3f(0.5, 1.0, 0.5)
        glVertex3f(1.0, -1.0, 1.0)        # Top Right Of The Quad (Bottom)
        glVertex3f(-1.0, -1.0, 1.0)       # Top Left Of The Quad (Bottom)
        glVertex3f(-1.0, -1.0, -1.0)      # Bottom Left Of The Quad (Bottom)
        glVertex3f(1.0, -1.0, -1.0)       # Bottom Right Of The Quad (Bottom)

        glColor3f(0.0, 0.0, 1.0)
        glVertex3f(1.0, 1.0, 1.0)         # Top Right Of The Quad (Front)
        glVertex3f(-1.0, 1.0, 1.0)        # Top Left Of The Quad (Front)
        glVertex3f(-1.0, -1.0, 1.0)       # Bottom Left Of The Quad (Front)
        glVertex3f(1.0, -1.0, 1.0)        # Bottom Right Of The Quad (Front)

        glColor3f(0.5, 0.5, 1.0)
        glVertex3f(1.0, -1.0, -1.0)       # Bottom Left Of The Quad (Back)
        glVertex3f(-1.0, -1.0, -1.0)      # Bottom Right Of The Quad (Back)
        glVertex3f(-1.0, 1.0, -1.0)       # Top Right Of The Quad (Back)
        glVertex3f(1.0, 1.0, -1.0)        # Top Left Of The Quad (Back)

        glColor3f(1.0, 0.5, 0.5)
        glVertex3f(-1.0, 1.0, 1.0)        # Top Right Of The Quad (Left)
        glVertex3f(-1.0, 1.0, -1.0)       # Top Left Of The Quad (Left)
        glVertex3f(-1.0, -1.0, -1.0)      # Bottom Left Of The Quad (Left)
        glVertex3f(-1.0, -1.0, 1.0)       # Bottom Right Of The Quad (Left)

        glColor3f(1.0, 0.0, 0.0)
        glVertex3f(1.0, 1.0, -1.0)        # Top Right Of The Quad (Right)
        glVertex3f(1.0, 1.0, 1.0)         # Top Left Of The Quad (Right)
        glVertex3f(1.0, -1.0, 1.0)        # Bottom Left Of The Quad (Right)
        glVertex3f(1.0, -1.0, -1.0)       # Bottom Right Of The Quad (Right)
        glEnd()                           # Done Drawing The Quad

    def _paintGLGrid(self):
        resolutionMillimeters = 1
        griddedAreaSize = 100

        glLineWidth(1.0)

        glBegin(GL_LINES)

        glColor3f(1.0, 1.0, 1.0)

        glVertex3f(griddedAreaSize, 0, 0)
        glVertex3f(-griddedAreaSize, 0, 0)
        glVertex3f(0, griddedAreaSize, 0)
        glVertex3f(0, -griddedAreaSize, 0)

        numOfLines = int(griddedAreaSize / resolutionMillimeters)

        for i in range(numOfLines):
            glVertex3f(resolutionMillimeters * i, -griddedAreaSize, 0)
            glVertex3f(resolutionMillimeters * i, griddedAreaSize, 0)
            glVertex3f(griddedAreaSize, resolutionMillimeters * i, 0)
            glVertex3f(-griddedAreaSize, resolutionMillimeters * i, 0)

            glVertex3f(resolutionMillimeters * (-i), -griddedAreaSize, 0)
            glVertex3f(resolutionMillimeters * (-i), griddedAreaSize, 0)
            glVertex3f(griddedAreaSize, resolutionMillimeters * (-i), 0)
            glVertex3f(-griddedAreaSize, resolutionMillimeters * (-i), 0)

        glEnd()

    def _paintGLCoorsystem(self):
        glLineWidth(10.0)

        glBegin(GL_LINES)

        glColor3f(1.0, 0.0, 0.0)
        glVertex3f(0.0, 0.0, 0.0)
        glVertex3f(1.0, 0.0, 0.0)

        glColor3f(0.0, 1.0, 0.0)
        glVertex3f(0.0, 0.0, 0.0)
        glVertex3f(0.0, 1.0, 0.0)

        glColor3f(0.0, 0.0, 1.0)
        glVertex3f(0.0, 0.0, 0.0)
        glVertex3f(0.0, 0.0, 1.0)

        glEnd()

    def _gl_view_mouseReleaseEvent(self, event):
        if event.button() == Qt.RightButton:
            menu = QMenu(self._gl_view)
            action = QAction(self._gl_view.tr("Reset view"), self._gl_view)
            menu.addAction(action)
            action.triggered.connect(self._set_default_view)
            menu.exec_(self._gl_view.mapToGlobal(event.pos()))

    @Slot('QDragEnterEvent*')
    def dragEnterEvent(self, event):
        if not event.mimeData().hasText():
            if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
                qWarning('Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0')
                return
            item = event.source().selectedItems()[0]
            ros_topic_name = item.data(0, Qt.UserRole)
            if ros_topic_name is None:
                qWarning('Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)')
                return
        # TODO: do some checks for valid topic here
        event.acceptProposedAction()

    @Slot('QDropEvent*')
    def dropEvent(self, event):
        if event.mimeData().hasText():
            topic_name = str(event.mimeData().text())
        else:
            droped_item = event.source().selectedItems()[0]
            topic_name = str(droped_item.data(0, Qt.UserRole))

        self.unregister_topic()
        self.subscribe_topic(topic_name)

    def unregister_topic(self):
        if self._subscriber:
            self._subscriber.unregister()

    def subscribe_topic(self, topic_name):
        msg_class, self._topic_name, _ = get_topic_class(topic_name)
        if (msg_class.__name__ == 'Quaternion') :
            self._subscriber = rospy.Subscriber(self._topic_name, msg_class, self.message_callback_quaternion)
        else :
            self._subscriber = rospy.Subscriber(self._topic_name, msg_class, self.message_callback_pose)
            
    def shutdown_plugin(self):
        self.unregister_topic()
Beispiel #46
0
class Dashboard(QWidget):

    _scan_button_led = Signal(bool)
    _cancel_button_led = Signal(bool)

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

        not_latched = False
        # latched = True
        self.publishers = py_trees_ros.utilities.Publishers([
            ('scan', "~scan", std_msgs.Empty, not_latched, 1),
            ('cancel', "~cancel", std_msgs.Empty, not_latched, 1),
        ])

        self.scan_push_button = QPushButton("Scan")
        self.scan_push_button.setStyleSheet("QPushButton { font-size: 30pt; }")
        self.scan_push_button.setSizePolicy(QSizePolicy.Expanding,
                                            QSizePolicy.Expanding)
        self.scan_push_button.pressed.connect(
            functools.partial(self.publish_button_message,
                              self.publishers.scan))

        self.cancel_push_button = QPushButton("Cancel")
        self.cancel_push_button.setStyleSheet(
            "QPushButton { font-size: 30pt; }")
        self.cancel_push_button.setSizePolicy(QSizePolicy.Expanding,
                                              QSizePolicy.Expanding)
        self.cancel_push_button.pressed.connect(
            functools.partial(self.publish_button_message,
                              self.publishers.cancel))

        self.led_strip_flashing = False
        self.led_strip_on_count = 1
        self.led_strip_colour = "grey"

        self.led_strip_lock = threading.Lock()
        self.led_strip_timer = QTimer()
        self.led_strip_timer.timeout.connect(self.led_strip_timer_callback)
        self.led_strip_label = QLabel("LED Strip")
        self.led_strip_label.setAlignment(Qt.AlignCenter)
        self.led_strip_label.setSizePolicy(QSizePolicy.Expanding,
                                           QSizePolicy.Expanding)
        self.led_strip_label.setStyleSheet(
            "background-color: %s; font-size: 30pt;" % self.led_strip_colour)

        self.hbox_layout = QGridLayout(self)
        self.hbox_layout.addWidget(self.scan_push_button)
        self.hbox_layout.addWidget(self.cancel_push_button)
        self.hbox_layout.addWidget(self.led_strip_label)

        self.subscribers = py_trees_ros.utilities.Subscribers([
            ("report", "/tree/report", std_msgs.String,
             self.reality_report_callback),
            ("led_strip", "/led_strip/display", std_msgs.String,
             self.led_strip_display_callback)
        ])
        self.led_strip_timer.start(500)  # ms

    def publish_button_message(self, publisher):
        publisher.publish(std_msgs.Empty())

    def reality_report_callback(self, msg):
        if msg.data == "cancelling":
            self.set_scanning_colour(False)
            self.set_cancelling_colour(True)
            self.cancel_push_button.setEnabled(True)
        elif msg.data == "scanning":
            self.set_scanning_colour(True)
            self.set_cancelling_colour(False)
            self.cancel_push_button.setEnabled(True)
        else:
            self.set_scanning_colour(False)
            self.set_cancelling_colour(False)
            self.cancel_push_button.setEnabled(False)

    def set_cancelling_colour(self, val):
        if val:
            self.cancel_push_button.setStyleSheet(
                "QPushButton { font-size: 30pt; background-color: red}")
        else:
            self.cancel_push_button.setStyleSheet(
                "QPushButton { font-size: 30pt; }")

    def set_scanning_colour(self, val):
        if val:
            self.scan_push_button.setStyleSheet(
                "QPushButton { font-size: 30pt; background-color: green}")
        else:
            self.scan_push_button.setStyleSheet(
                "QPushButton { font-size: 30pt; }")

    def led_strip_display_callback(self, msg):
        with self.led_strip_lock:
            if not msg.data:
                self.led_strip_colour = "grey"
                self.led_strip_flashing = False
            else:
                self.led_strip_flashing = True
                self.led_strip_colour = None
                for colour in ["blue", "red", "green"]:
                    if colour in msg.data:
                        self.led_strip_colour = colour
                        break
                if not self.led_strip_colour:
                    self.led_strip_colour = "pink"
                    rospy.loginfo(
                        "Dashboard: received unknown LED colour {0}, setting 'pink'"
                        .format(msg.data))

    @Slot()
    def led_strip_timer_callback(self):
        with self.led_strip_lock:
            if self.led_strip_flashing:
                if self.led_strip_on_count > 0:
                    self.led_strip_on_count = 0
                    self.led_strip_label.setStyleSheet(
                        "background-color: none; font-size: 30pt;")
                else:
                    self.led_strip_on_count += 1
                    self.led_strip_label.setStyleSheet(
                        "background-color: %s; font-size: 30pt;" %
                        self.led_strip_colour)
            else:  # solid
                self.led_strip_on_count = 1
                self.led_strip_label.setStyleSheet(
                    "background-color: %s; font-size: 30pt;" %
                    self.led_strip_colour)
    def __init__(self, context):
        super(RobotSteering, self).__init__(context)
        self.setObjectName('RobotSteering')

        self._publisher = None

        self._widget = QWidget()
        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource',
                               'RobotSteering.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('RobotSteeringUi')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))
        context.add_widget(self._widget)

        self._widget.topic_line_edit.textChanged.connect(
            self._on_topic_changed)
        self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)

        self._widget.x_linear_slider.valueChanged.connect(
            self._on_x_linear_slider_changed)
        self._widget.z_angular_slider.valueChanged.connect(
            self._on_z_angular_slider_changed)

        self._widget.increase_x_linear_push_button.pressed.connect(
            self._on_strong_increase_x_linear_pressed)
        self._widget.reset_x_linear_push_button.pressed.connect(
            self._on_reset_x_linear_pressed)
        self._widget.decrease_x_linear_push_button.pressed.connect(
            self._on_strong_decrease_x_linear_pressed)
        self._widget.increase_z_angular_push_button.pressed.connect(
            self._on_strong_increase_z_angular_pressed)
        self._widget.reset_z_angular_push_button.pressed.connect(
            self._on_reset_z_angular_pressed)
        self._widget.decrease_z_angular_push_button.pressed.connect(
            self._on_strong_decrease_z_angular_pressed)

        self._widget.max_x_linear_double_spin_box.valueChanged.connect(
            self._on_max_x_linear_changed)
        self._widget.min_x_linear_double_spin_box.valueChanged.connect(
            self._on_min_x_linear_changed)
        self._widget.max_z_angular_double_spin_box.valueChanged.connect(
            self._on_max_z_angular_changed)
        self._widget.min_z_angular_double_spin_box.valueChanged.connect(
            self._on_min_z_angular_changed)

        self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
        self.shortcut_w.setContext(Qt.ApplicationShortcut)
        self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
        self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
        self.shortcut_x.setContext(Qt.ApplicationShortcut)
        self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
        self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
        self.shortcut_s.setContext(Qt.ApplicationShortcut)
        self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
        self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
        self.shortcut_a.setContext(Qt.ApplicationShortcut)
        self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
        self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
        self.shortcut_z.setContext(Qt.ApplicationShortcut)
        self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
        self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
        self.shortcut_d.setContext(Qt.ApplicationShortcut)
        self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)

        self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W),
                                          self._widget)
        self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
        self.shortcut_shift_w.activated.connect(
            self._on_strong_increase_x_linear_pressed)
        self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X),
                                          self._widget)
        self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
        self.shortcut_shift_x.activated.connect(
            self._on_reset_x_linear_pressed)
        self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S),
                                          self._widget)
        self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
        self.shortcut_shift_s.activated.connect(
            self._on_strong_decrease_x_linear_pressed)
        self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A),
                                          self._widget)
        self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
        self.shortcut_shift_a.activated.connect(
            self._on_strong_increase_z_angular_pressed)
        self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z),
                                          self._widget)
        self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
        self.shortcut_shift_z.activated.connect(
            self._on_reset_z_angular_pressed)
        self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D),
                                          self._widget)
        self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
        self.shortcut_shift_d.activated.connect(
            self._on_strong_decrease_z_angular_pressed)

        self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space),
                                        self._widget)
        self.shortcut_space.setContext(Qt.ApplicationShortcut)
        self.shortcut_space.activated.connect(self._on_stop_pressed)
        self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space),
                                        self._widget)
        self.shortcut_space.setContext(Qt.ApplicationShortcut)
        self.shortcut_space.activated.connect(self._on_stop_pressed)

        self._widget.stop_push_button.setToolTip(
            self._widget.stop_push_button.toolTip() + ' ' +
            self.tr('([Shift +] Space)'))
        self._widget.increase_x_linear_push_button.setToolTip(
            self._widget.increase_x_linear_push_button.toolTip() + ' ' +
            self.tr('([Shift +] W)'))
        self._widget.reset_x_linear_push_button.setToolTip(
            self._widget.reset_x_linear_push_button.toolTip() + ' ' +
            self.tr('([Shift +] X)'))
        self._widget.decrease_x_linear_push_button.setToolTip(
            self._widget.decrease_x_linear_push_button.toolTip() + ' ' +
            self.tr('([Shift +] S)'))
        self._widget.increase_z_angular_push_button.setToolTip(
            self._widget.increase_z_angular_push_button.toolTip() + ' ' +
            self.tr('([Shift +] A)'))
        self._widget.reset_z_angular_push_button.setToolTip(
            self._widget.reset_z_angular_push_button.toolTip() + ' ' +
            self.tr('([Shift +] Z)'))
        self._widget.decrease_z_angular_push_button.setToolTip(
            self._widget.decrease_z_angular_push_button.toolTip() + ' ' +
            self.tr('([Shift +] D)'))

        # timer to consecutively send twist messages
        self._update_parameter_timer = QTimer(self)
        self._update_parameter_timer.timeout.connect(
            self._on_parameter_changed)
        self._update_parameter_timer.start(100)
        self.zero_cmd_sent = False
class RobotSteering(Plugin):

    slider_factor = 1000.0

    def __init__(self, context):
        super(RobotSteering, self).__init__(context)
        self.setObjectName('RobotSteering')

        self._publisher = None

        self._widget = QWidget()
        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource',
                               'RobotSteering.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('RobotSteeringUi')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))
        context.add_widget(self._widget)

        self._widget.topic_line_edit.textChanged.connect(
            self._on_topic_changed)
        self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)

        self._widget.x_linear_slider.valueChanged.connect(
            self._on_x_linear_slider_changed)
        self._widget.z_angular_slider.valueChanged.connect(
            self._on_z_angular_slider_changed)

        self._widget.increase_x_linear_push_button.pressed.connect(
            self._on_strong_increase_x_linear_pressed)
        self._widget.reset_x_linear_push_button.pressed.connect(
            self._on_reset_x_linear_pressed)
        self._widget.decrease_x_linear_push_button.pressed.connect(
            self._on_strong_decrease_x_linear_pressed)
        self._widget.increase_z_angular_push_button.pressed.connect(
            self._on_strong_increase_z_angular_pressed)
        self._widget.reset_z_angular_push_button.pressed.connect(
            self._on_reset_z_angular_pressed)
        self._widget.decrease_z_angular_push_button.pressed.connect(
            self._on_strong_decrease_z_angular_pressed)

        self._widget.max_x_linear_double_spin_box.valueChanged.connect(
            self._on_max_x_linear_changed)
        self._widget.min_x_linear_double_spin_box.valueChanged.connect(
            self._on_min_x_linear_changed)
        self._widget.max_z_angular_double_spin_box.valueChanged.connect(
            self._on_max_z_angular_changed)
        self._widget.min_z_angular_double_spin_box.valueChanged.connect(
            self._on_min_z_angular_changed)

        self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
        self.shortcut_w.setContext(Qt.ApplicationShortcut)
        self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
        self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
        self.shortcut_x.setContext(Qt.ApplicationShortcut)
        self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
        self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
        self.shortcut_s.setContext(Qt.ApplicationShortcut)
        self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
        self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
        self.shortcut_a.setContext(Qt.ApplicationShortcut)
        self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
        self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
        self.shortcut_z.setContext(Qt.ApplicationShortcut)
        self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
        self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
        self.shortcut_d.setContext(Qt.ApplicationShortcut)
        self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)

        self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W),
                                          self._widget)
        self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
        self.shortcut_shift_w.activated.connect(
            self._on_strong_increase_x_linear_pressed)
        self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X),
                                          self._widget)
        self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
        self.shortcut_shift_x.activated.connect(
            self._on_reset_x_linear_pressed)
        self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S),
                                          self._widget)
        self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
        self.shortcut_shift_s.activated.connect(
            self._on_strong_decrease_x_linear_pressed)
        self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A),
                                          self._widget)
        self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
        self.shortcut_shift_a.activated.connect(
            self._on_strong_increase_z_angular_pressed)
        self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z),
                                          self._widget)
        self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
        self.shortcut_shift_z.activated.connect(
            self._on_reset_z_angular_pressed)
        self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D),
                                          self._widget)
        self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
        self.shortcut_shift_d.activated.connect(
            self._on_strong_decrease_z_angular_pressed)

        self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space),
                                        self._widget)
        self.shortcut_space.setContext(Qt.ApplicationShortcut)
        self.shortcut_space.activated.connect(self._on_stop_pressed)
        self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space),
                                        self._widget)
        self.shortcut_space.setContext(Qt.ApplicationShortcut)
        self.shortcut_space.activated.connect(self._on_stop_pressed)

        self._widget.stop_push_button.setToolTip(
            self._widget.stop_push_button.toolTip() + ' ' +
            self.tr('([Shift +] Space)'))
        self._widget.increase_x_linear_push_button.setToolTip(
            self._widget.increase_x_linear_push_button.toolTip() + ' ' +
            self.tr('([Shift +] W)'))
        self._widget.reset_x_linear_push_button.setToolTip(
            self._widget.reset_x_linear_push_button.toolTip() + ' ' +
            self.tr('([Shift +] X)'))
        self._widget.decrease_x_linear_push_button.setToolTip(
            self._widget.decrease_x_linear_push_button.toolTip() + ' ' +
            self.tr('([Shift +] S)'))
        self._widget.increase_z_angular_push_button.setToolTip(
            self._widget.increase_z_angular_push_button.toolTip() + ' ' +
            self.tr('([Shift +] A)'))
        self._widget.reset_z_angular_push_button.setToolTip(
            self._widget.reset_z_angular_push_button.toolTip() + ' ' +
            self.tr('([Shift +] Z)'))
        self._widget.decrease_z_angular_push_button.setToolTip(
            self._widget.decrease_z_angular_push_button.toolTip() + ' ' +
            self.tr('([Shift +] D)'))

        # timer to consecutively send twist messages
        self._update_parameter_timer = QTimer(self)
        self._update_parameter_timer.timeout.connect(
            self._on_parameter_changed)
        self._update_parameter_timer.start(100)
        self.zero_cmd_sent = False

    @Slot(str)
    def _on_topic_changed(self, topic):
        topic = str(topic)
        self._unregister_publisher()
        if topic == '':
            return
        try:
            self._publisher = rospy.Publisher(topic, Twist, queue_size=10)
        except TypeError:
            self._publisher = rospy.Publisher(topic, Twist)

    def _on_stop_pressed(self):
        self._widget.x_linear_slider.setValue(0)
        self._widget.z_angular_slider.setValue(0)

    def _on_x_linear_slider_changed(self):
        self._widget.current_x_linear_label.setText(
            '%0.2f m/s' % (self._widget.x_linear_slider.value() /
                           RobotSteering.slider_factor))
        self._on_parameter_changed()

    def _on_z_angular_slider_changed(self):
        self._widget.current_z_angular_label.setText(
            '%0.2f rad/s' % (self._widget.z_angular_slider.value() /
                             RobotSteering.slider_factor))
        self._on_parameter_changed()

    def _on_increase_x_linear_pressed(self):
        self._widget.x_linear_slider.setValue(
            self._widget.x_linear_slider.value() +
            self._widget.x_linear_slider.singleStep())

    def _on_reset_x_linear_pressed(self):
        self._widget.x_linear_slider.setValue(0)

    def _on_decrease_x_linear_pressed(self):
        self._widget.x_linear_slider.setValue(
            self._widget.x_linear_slider.value() -
            self._widget.x_linear_slider.singleStep())

    def _on_increase_z_angular_pressed(self):
        self._widget.z_angular_slider.setValue(
            self._widget.z_angular_slider.value() +
            self._widget.z_angular_slider.singleStep())

    def _on_reset_z_angular_pressed(self):
        self._widget.z_angular_slider.setValue(0)

    def _on_decrease_z_angular_pressed(self):
        self._widget.z_angular_slider.setValue(
            self._widget.z_angular_slider.value() -
            self._widget.z_angular_slider.singleStep())

    def _on_max_x_linear_changed(self, value):
        self._widget.x_linear_slider.setMaximum(value *
                                                RobotSteering.slider_factor)

    def _on_min_x_linear_changed(self, value):
        self._widget.x_linear_slider.setMinimum(value *
                                                RobotSteering.slider_factor)

    def _on_max_z_angular_changed(self, value):
        self._widget.z_angular_slider.setMaximum(value *
                                                 RobotSteering.slider_factor)

    def _on_min_z_angular_changed(self, value):
        self._widget.z_angular_slider.setMinimum(value *
                                                 RobotSteering.slider_factor)

    def _on_strong_increase_x_linear_pressed(self):
        self._widget.x_linear_slider.setValue(
            self._widget.x_linear_slider.value() +
            self._widget.x_linear_slider.pageStep())

    def _on_strong_decrease_x_linear_pressed(self):
        self._widget.x_linear_slider.setValue(
            self._widget.x_linear_slider.value() -
            self._widget.x_linear_slider.pageStep())

    def _on_strong_increase_z_angular_pressed(self):
        self._widget.z_angular_slider.setValue(
            self._widget.z_angular_slider.value() +
            self._widget.z_angular_slider.pageStep())

    def _on_strong_decrease_z_angular_pressed(self):
        self._widget.z_angular_slider.setValue(
            self._widget.z_angular_slider.value() -
            self._widget.z_angular_slider.pageStep())

    def _on_parameter_changed(self):
        self._send_twist(
            self._widget.x_linear_slider.value() / RobotSteering.slider_factor,
            self._widget.z_angular_slider.value() /
            RobotSteering.slider_factor)

    def _send_twist(self, x_linear, z_angular):
        if self._publisher is None:
            return
        twist = Twist()
        twist.linear.x = x_linear
        twist.linear.y = 0
        twist.linear.z = 0
        twist.angular.x = 0
        twist.angular.y = 0
        twist.angular.z = z_angular

        # Only send the zero command once so other devices can take control
        if x_linear == 0 and z_angular == 0:
            if not self.zero_cmd_sent:
                self.zero_cmd_sent = True
                self._publisher.publish(twist)
        else:
            self.zero_cmd_sent = False
            self._publisher.publish(twist)

    def _unregister_publisher(self):
        if self._publisher is not None:
            self._publisher.unregister()
            self._publisher = None

    def shutdown_plugin(self):
        self._update_parameter_timer.stop()
        self._unregister_publisher()

    def save_settings(self, plugin_settings, instance_settings):
        instance_settings.set_value('topic',
                                    self._widget.topic_line_edit.text())
        instance_settings.set_value(
            'vx_max', self._widget.max_x_linear_double_spin_box.value())
        instance_settings.set_value(
            'vx_min', self._widget.min_x_linear_double_spin_box.value())
        instance_settings.set_value(
            'vw_max', self._widget.max_z_angular_double_spin_box.value())
        instance_settings.set_value(
            'vw_min', self._widget.min_z_angular_double_spin_box.value())

    def restore_settings(self, plugin_settings, instance_settings):

        value = instance_settings.value('topic', "/cmd_vel")
        value = rospy.get_param("~default_topic", value)
        self._widget.topic_line_edit.setText(value)

        value = self._widget.max_x_linear_double_spin_box.value()
        value = instance_settings.value('vx_max', value)
        value = rospy.get_param("~default_vx_max", value)
        self._widget.max_x_linear_double_spin_box.setValue(float(value))

        value = self._widget.min_x_linear_double_spin_box.value()
        value = instance_settings.value('vx_min', value)
        value = rospy.get_param("~default_vx_min", value)
        self._widget.min_x_linear_double_spin_box.setValue(float(value))

        value = self._widget.max_z_angular_double_spin_box.value()
        value = instance_settings.value('vw_max', value)
        value = rospy.get_param("~default_vw_max", value)
        self._widget.max_z_angular_double_spin_box.setValue(float(value))

        value = self._widget.min_z_angular_double_spin_box.value()
        value = instance_settings.value('vw_min', value)
        value = rospy.get_param("~default_vw_min", value)
        self._widget.min_z_angular_double_spin_box.setValue(float(value))
class RuntimeMonitorWidget(QWidget):
    def __init__(self, topic="/diagnostics"):
        super(RuntimeMonitorWidget, self).__init__()
        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('rqt_runtime_monitor'), 'resource', 'runtime_monitor_widget.ui')
        loadUi(ui_file, self)
        self.setObjectName('RuntimeMonitorWidget')

        self._mutex = threading.Lock()

        self._error_icon = QIcon.fromTheme('dialog-error')
        self._warning_icon = QIcon.fromTheme('dialog-warning')
        self._ok_icon = QIcon.fromTheme('dialog-information')

        self._stale_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Stale (0)'])
        self._stale_node.setIcon(0, self._error_icon)
        self.tree_widget.addTopLevelItem(self._stale_node)

        self._error_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Errors (0)'])
        self._error_node.setIcon(0, self._error_icon)
        self.tree_widget.addTopLevelItem(self._error_node)

        self._warning_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Warnings (0)'])
        self._warning_node.setIcon(0, self._warning_icon)
        self.tree_widget.addTopLevelItem(self._warning_node)

        self._ok_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Ok (0)'])
        self._ok_node.setIcon(0, self._ok_icon)
        self.tree_widget.addTopLevelItem(self._ok_node)
        self.tree_widget.itemSelectionChanged.connect(self._refresh_selection)
        self.keyPressEvent = self._on_key_press

        self._name_to_item = {}
        self._new_errors_callback = None

        self._subscriber = rospy.Subscriber(topic, DiagnosticArray, self._diagnostics_callback)

        self._timer = QTimer()
        self._timer.timeout.connect(self._on_timer)
        self._timer.start(5000)

        self._msg_timer = QTimer()
        self._msg_timer.timeout.connect(self._update_messages)
        self._msg_timer.start(100)

        self._messages = []
        self._used_items = 0

    def __del__(self):
        self.shutdown()
 
    def shutdown(self):
        """
        Unregisters subscriber and stops timers
        """
        self._msg_timer.stop()
        self._timer.stop()

        if rospy.is_shutdown():
            return

        if self._subscriber:
            self._subscriber.unregister()
            self._subscriber = None

    def change_diagnostic_topic(self, topic):
        """
        Changes diagnostics topic name. Must be of type diagnostic_msgs/DiagnosticArray
        """
        if not topic:
            self.reset_monitor()
            return

        if self._subscriber:
            self._subscriber.unregister()
            self._subscriber = rospy.Subscriber(str(topic), DiagnosticArray, self._diagnostics_callback)
        self.reset_monitor()

    def reset_monitor(self):
        """
        Removes all values from monitor display, resets buffers
        """
        self._name_to_item = {}  # Reset all stale topics
        self._messages = []
        self._clear_tree()

    def _clear_tree(self):
        for index in range(self._stale_node.childCount()):
            self._stale_node.removeChild(self._stale_node.child(index))
        for index in range(self._error_node.childCount()):
            self._error_node.removeChild(self._error_node.child(index))
        for index in range(self._warning_node.childCount()):
            self._warning_node.removeChild(self._warning_node.child(index))
        for index in range(self._ok_node.childCount()):
            self._ok_node.removeChild(self._ok_node.child(index))
        self._update_root_labels()

    # Diagnostics callbacks (subscriber thread)
    def _diagnostics_callback(self, message):
        with self._mutex:
            self._messages.append(message)

    # Update display of messages from main thread
    def _update_messages(self):
        with self._mutex:
            messages = self._messages[:]
            self._messages = []

        had_errors = False
        for message in messages:
            for status in message.status:
                was_selected = False
                if (self._name_to_item.has_key(status.name)):
                    item = self._name_to_item[status.name]
                    if self.tree_widget.isItemSelected(item.tree_node):
                        was_selected = True
                    if (item.status.level == DiagnosticStatus.ERROR and status.level != DiagnosticStatus.ERROR):
                        had_errors = True
                    self._update_item(item, status, was_selected)
                else:
                    self._create_item(status, was_selected, True)
                    if (status.level == DiagnosticStatus.ERROR):
                        had_errors = True

        if (had_errors and self._new_errors_callback != None):
            self._new_errors_callback()

        self._update_root_labels()
        self.update()
        self._refresh_selection()

    def _update_item(self, item, status, was_selected):
        change_parent = False
        if (item.status.level != status.level):
            change_parent = True
        if (change_parent):
            if (item.status.level == DiagnosticStatus.OK):
                self._ok_node.removeChild(item.tree_node)
            elif (item.status.level == DiagnosticStatus.WARN):
                self._warning_node.removeChild(item.tree_node)
            elif (item.status.level == -1): 
                self._stale_node.removeChild(item.tree_node)
            else: # ERROR
                self._error_node.removeChild(item.tree_node)

            if (status.level == DiagnosticStatus.OK):
                parent_node = self._ok_node
            elif (status.level == DiagnosticStatus.WARN):
                parent_node = self._warning_node
            elif (status.level == -1):
                parent_node = self._stale_node
            else: # ERROR
                parent_node = self._error_node

            item.tree_node.setText(0, status.name + ": " + status.message)
            item.tree_node.setData(0, Qt.UserRole, item)
            parent_node.addChild(item.tree_node)

            # expand errors automatically
            if (status.level > 1 or status.level == -1):
                parent_node.setExpanded(True)

            parent_node.sortChildren(0, Qt.AscendingOrder)

            if (was_selected):
                self.tree_widget.setCurrentItem(item.tree_node)

        else:
            item.tree_node.setText(0, status.name + ": " + status.message)

        item.status = status

        if (was_selected):
            self._fillout_info(item.tree_node)

        item.mark = True

    def _create_item(self, status, select, expand_if_error):
        if (status.level == DiagnosticStatus.OK):
            parent_node = self._ok_node
        elif (status.level == DiagnosticStatus.WARN):
            parent_node = self._warning_node
        elif (status.level == -1):
            parent_node = self._stale_node
        else: # ERROR
            parent_node = self._error_node

        item = TreeItem(status, QTreeWidgetItem(parent_node, [status.name + ": " + status.message]))
        item.tree_node.setData(0, Qt.UserRole, item)
        parent_node.addChild(item.tree_node)

        self._name_to_item[status.name] = item

        parent_node.sortChildren(0, Qt.AscendingOrder)

        if (select):
            item.tree_node.setSelected(True)

        if (expand_if_error and (status.level > 1 or status.level == -1)):
            parent_node.setExpanded(True)

        item.mark = True

        return item

    def _fillout_info(self, node):
        item = node.data(0, Qt.UserRole)
        if not item:
            return

        scroll_value = self.html_browser.verticalScrollBar().value()
        status = item.status

        s = cStringIO.StringIO()

        s.write("<html><body>")
        s.write("<b>Component</b>: %s<br>\n" % (status.name))
        s.write("<b>Message</b>: %s<br>\n" % (status.message))
        s.write("<b>Hardware ID</b>: %s<br><br>\n\n" % (status.hardware_id))

        s.write('<table border="1" cellpadding="2" cellspacing="0">')
        for value in status.values:
            value.value = value.value.replace("\n", "<br>")
            s.write("<tr><td><b>%s</b></td> <td>%s</td></tr>\n" % (value.key, value.value))

        s.write("</table></body></html>")

        self.html_browser.setHtml(s.getvalue())
        if self.html_browser.verticalScrollBar().maximum() < scroll_value:
            scroll_value = self.html_browser.verticalScrollBar().maximum()
        self.html_browser.verticalScrollBar().setValue(scroll_value)

    def _refresh_selection(self):
        current_item = self.tree_widget.selectedItems()
        if current_item:
            self._fillout_info(current_item[0])

    def _on_key_press(self, event):
        key = event.key()
        if key == Qt.Key_Delete:
            nodes = self.tree_widget.selectedItems()
            if (nodes != [] and nodes[0] not in (self._ok_node, self._warning_node, self._stale_node, self._error_node)):
                item = nodes[0].data(0, Qt.UserRole)
                if (item.status.level == 0):
                    self._ok_node.removeChild(item.tree_node)
                elif (item.status.level == 1):
                    self._warning_node.removeChild(item.tree_node)
                elif (item.status.level == -1):
                    self._stale_node.removeChild(item.tree_node)
                else:
                    self._error_node.removeChild(item.tree_node)
                del self._name_to_item[item.status.name]
            self._update_root_labels()
            self.update()
            event.accept()
        else:
            event.ignore()

    def _on_timer(self):
        for name, item in self._name_to_item.iteritems():
            node = item.tree_node
            if (item != None):
                if (not item.mark):
                    was_selected = False
                    selected = self.tree_widget.selectedItems()
                    if selected != [] and selected[0] == node:
                        was_selected = True

                    new_status = copy.deepcopy(item.status)
                    new_status.level = -1 # mark stale
                    self._update_item(item, new_status, was_selected)
                item.mark = False
        self._update_root_labels()
        self.update()

    def set_new_errors_callback(self, callback):
        self._new_errors_callback = callback

    def get_num_errors(self):
        return self._error_node.childCount() + self._stale_node.childCount()

    def get_num_warnings(self):
        return self._warning_node.childCount()

    def get_num_ok(self):
        return self._ok_node.childCount()

    def _update_root_labels(self):
        self._stale_node.setText(0, "Stale (%s)" % (self._stale_node.childCount()))
        self._error_node.setText(0, "Errors (%s)" % (self._error_node.childCount()))
        self._warning_node.setText(0, "Warnings (%s)" % (self._warning_node.childCount()))
        self._ok_node.setText(0, "Ok (%s)" % (self._ok_node.childCount()))
Beispiel #50
0
class Plot2DWidget(QWidget):
    _redraw_interval = 10

    def __init__(self, topics):
        super(Plot2DWidget, self).__init__()
        self.setObjectName('Plot2DWidget')
        rp = rospkg.RosPack()
        ui_file = os.path.join(
            rp.get_path('jsk_rqt_plugins'), 'resource', 'plot_histogram.ui')
        loadUi(ui_file, self)
        self.cv_bridge = CvBridge()
        self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
        self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
        self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
        self.data_plot = MatPlot2D(self)
        self.data_plot_layout.addWidget(self.data_plot)
        self._topic_completer = TopicCompleter(self.topic_edit)
        self._topic_completer.update_topics()
        self.topic_edit.setCompleter(self._topic_completer)
        self.data_plot.dropEvent = self.dropEvent
        self.data_plot.dragEnterEvent = self.dragEnterEvent
        self._start_time = rospy.get_time()
        self._rosdata = None
        if len(topics) != 0:
            self.subscribe_topic(topics)
        self._update_plot_timer = QTimer(self)
        self._update_plot_timer.timeout.connect(self.update_plot)
        self._update_plot_timer.start(self._redraw_interval)

    @Slot('QDropEvent*')
    def dropEvent(self, event):
        if event.mimeData().hasText():
            topic_name = str(event.mimeData().text())
        else:
            droped_item = event.source().selectedItems()[0]
            topic_name = str(droped_item.data(0, Qt.UserRole))
        self.subscribe_topic(topic_name)

    @Slot()
    def on_topic_edit_returnPressed(self):
        "callback function when form is entered"
        if self.subscribe_topic_button.isEnabled():
            self.subscribe_topic(str(self.topic_edit.text()))

    @Slot()
    def on_subscribe_topic_button_clicked(self):
        self.subscribe_topic(str(self.topic_edit.text()))

    def subscribe_topic(self, topic_name):
        rospy.loginfo("subscribe topic")
        self.topic_with_field_name = topic_name
        self.pub_image = rospy.Publisher(
            topic_name + "/plot_image", Image, queue_size=1)
        if not self._rosdata:
            self._rosdata = ROSData(topic_name, self._start_time)
        else:
            if self._rosdata != topic_name:
                self._rosdata.close()
                self.data_plot.clear()
                self._rosdata = ROSData(topic_name, self._start_time)
            else:
                rospy.logwarn("%s is already subscribed", topic_name)

    def enable_timer(self, enabled=True):
        if enabled:
            self._update_plot_timer.start(self._redraw_interval)
        else:
            self._update_plot_timer.stop()

    @Slot()
    def on_clear_button_clicked(self):
        self.data_plot.clear()

    @Slot(bool)
    def on_pause_button_clicked(self, checked):
        self.enable_timer(not checked)

    def plot_one(self, msg, axes):
        concatenated_data = zip(msg.xs, msg.ys)
        if self.sort_x:
            concatenated_data.sort(key=lambda x: x[0])
        xs = [d[0] for d in concatenated_data]
        ys = [d[1] for d in concatenated_data]
        if self.is_line or msg.type is PlotData.LINE:
            axes.plot(xs, ys,
                      label=msg.label or self.topic_with_field_name)
        else:
            axes.scatter(xs, ys,
                         label=msg.label or self.topic_with_field_name)
        if msg.fit_line or self.fit_line:
            X = np.array(msg.xs)
            Y = np.array(msg.ys)
            A = np.array([X, np.ones(len(X))])
            A = A.T
            a, b = np.linalg.lstsq(A, Y, rcond=-1)[0]
            axes.plot(X, (a*X+b), "g--", label="{0} x + {1}".format(a, b))
        if msg.fit_line_ransac or self.fit_line_ransac:
            model_ransac = linear_model.RANSACRegressor(
                linear_model.LinearRegression(), min_samples=2,
                residual_threshold=self.fit_line_ransac_outlier)
            X = np.array(msg.xs).reshape((len(msg.xs), 1))
            Y = np.array(msg.ys)
            model_ransac.fit(X, Y)
            line_X = X
            line_y_ransac = model_ransac.predict(line_X)
            if len(model_ransac.estimator_.coef_) == 1:
                coef = model_ransac.estimator_.coef_[0]
            else:
                coef = model_ransac.estimator_.coef_[0][0]
            if not isinstance(model_ransac.estimator_.intercept_, list):
                intercept = model_ransac.estimator_.intercept_
            else:
                intercept = model_ransac.estimator_.intercept_[0]
            axes.plot(
                line_X, line_y_ransac, "r--",
                label="{0} x + {1}".format(coef, intercept))

    def update_plot(self):
        if not self._rosdata:
            return
        try:
            data_x, data_y = self._rosdata.next()
        except RosPlotException as e:
            rospy.logerr("Exception in subscribing topic")
            rospy.logerr(e.message)
            return
        if len(data_y) == 0:
            return
        axes = self.data_plot._canvas.axes
        axes.cla()
        # matplotlib
        # concatenate x and y in order to sort
        latest_msg = data_y[-1]
        min_x = None
        max_x = None
        min_y = None
        max_y = None
        if isinstance(latest_msg, PlotData):
            data = [latest_msg]
            legend_size = 8
            no_legend = False
        elif isinstance(latest_msg, PlotDataArray):
            data = latest_msg.data
            legend_size = latest_msg.legend_font_size or 8
            no_legend = latest_msg.no_legend
            if latest_msg.min_x != latest_msg.max_x:
                min_x = latest_msg.min_x
                max_x = latest_msg.max_x
            if latest_msg.min_y != latest_msg.max_y:
                min_y = latest_msg.min_y
                max_y = latest_msg.max_y
        else:
            rospy.logerr(
                "Topic should be jsk_recognition_msgs/PlotData",
                "or jsk_recognition_msgs/PlotDataArray")
        for d in data:
            self.plot_one(d, axes)
        xs = add_list([d.xs for d in data])
        ys = add_list([d.ys for d in data])
        if min_x is None:
            min_x = min(xs)
        if max_x is None:
            max_x = max(xs)
        if min_y is None:
            min_y = min(ys)
        if max_y is None:
            max_y = max(ys)
        axes.set_xlim(min_x, max_x)
        axes.set_ylim(min_y, max_y)
        # line fitting
        if not no_legend and not self.no_legend:
            axes.legend(prop={'size': legend_size})
        axes.grid()
        if self.xtitle:
            axes.set_xlabel(self.xtitle)
        if self.ytitle:
            axes.set_ylabel(self.ytitle)
        self.data_plot._canvas.draw()
        buffer = StringIO()
        self.data_plot._canvas.figure.savefig(buffer, format="png")
        buffer.seek(0)
        img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8)
        if LooseVersion(cv2.__version__).version[0] < 2:
            iscolor = cv2.CV_LOAD_IMAGE_COLOR
        else:
            iscolor = cv2.IMREAD_COLOR
        img = cv2.imdecode(img_array, iscolor)
        self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
Beispiel #51
0
class Plot2DWidget(QWidget):
    _redraw_interval = 40

    def __init__(self, topics):
        super(Plot2DWidget, self).__init__()
        self.setObjectName('Plot2DWidget')
        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource',
                               'plot_histogram.ui')
        loadUi(ui_file, self)
        self.cv_bridge = CvBridge()
        self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
        self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
        self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
        self.data_plot = MatPlot2D(self)
        self.data_plot_layout.addWidget(self.data_plot)
        self._topic_completer = TopicCompleter(self.topic_edit)
        self._topic_completer.update_topics()
        self.topic_edit.setCompleter(self._topic_completer)
        self.data_plot.dropEvent = self.dropEvent
        self.data_plot.dragEnterEvent = self.dragEnterEvent
        self._start_time = rospy.get_time()
        self._rosdata = None
        if len(topics) != 0:
            self.subscribe_topic(topics)
        self._update_plot_timer = QTimer(self)
        self._update_plot_timer.timeout.connect(self.update_plot)
        self._update_plot_timer.start(self._redraw_interval)

    @Slot('QDropEvent*')
    def dropEvent(self, event):
        if event.mimeData().hasText():
            topic_name = str(event.mimeData().text())
        else:
            droped_item = event.source().selectedItems()[0]
            topic_name = str(droped_item.data(0, Qt.UserRole))
        self.subscribe_topic(topic_name)

    @Slot()
    def on_topic_edit_returnPressed(self):
        if self.subscribe_topic_button.isEnabled():
            self.subscribe_topic(str(self.topic_edit.text()))

    @Slot()
    def on_subscribe_topic_button_clicked(self):
        self.subscribe_topic(str(self.topic_edit.text()))

    def subscribe_topic(self, topic_name):
        self.topic_with_field_name = topic_name
        self.pub_image = rospy.Publisher(topic_name + "/histogram_image",
                                         Image)
        if not self._rosdata:
            self._rosdata = ROSData(topic_name, self._start_time)
        else:
            if self._rosdata != topic_name:
                self._rosdata.close()
                self.data_plot.clear()
                self._rosdata = ROSData(topic_name, self._start_time)
            else:
                rospy.logwarn("%s is already subscribed", topic_name)

    def enable_timer(self, enabled=True):
        if enabled:
            self._update_plot_timer.start(self._redraw_interval)
        else:
            self._update_plot_timer.stop()

    @Slot()
    def on_clear_button_clicked(self):
        self.data_plot.clear()

    @Slot(bool)
    def on_pause_button_clicked(self, checked):
        self.enable_timer(not checked)

    def update_plot(self):
        if not self._rosdata:
            return
        data_x, data_y = self._rosdata.next()

        if len(data_y) == 0:
            return
        axes = self.data_plot._canvas.axes
        axes.cla()
        # matplotlib
        # concatenate x and y in order to sort
        concatenated_data = zip(data_y[-1].xs, data_y[-1].ys)
        if self.sort_x:
            concatenated_data.sort(key=lambda x: x[0])
        xs = [d[0] for d in concatenated_data]
        ys = [d[1] for d in concatenated_data]
        if self.is_line:
            axes.plot(xs, ys, label=self.topic_with_field_name)
        else:
            axes.scatter(xs, ys)
        # set limit
        axes.set_xlim(min(xs), max(xs))
        axes.set_ylim(min(ys), max(ys))
        # line fitting
        if self.fit_line:
            X = np.array(data_y[-1].xs)
            Y = np.array(data_y[-1].ys)
            A = np.array([X, np.ones(len(X))])
            A = A.T
            a, b = np.linalg.lstsq(A, Y)[0]
            axes.plot(X, (a * X + b), "g--", label="{0} x + {1}".format(a, b))
        if self.fit_line_ransac:
            model_ransac = linear_model.RANSACRegressor(
                linear_model.LinearRegression(),
                min_samples=2,
                residual_threshold=self.fit_line_ransac_outlier)
            X = np.array(data_y[-1].xs).reshape((len(data_y[-1].xs), 1))
            Y = np.array(data_y[-1].ys)
            model_ransac.fit(X, Y)
            line_X = X
            line_y_ransac = model_ransac.predict(line_X)
            axes.plot(line_X,
                      line_y_ransac,
                      "r--",
                      label="{0} x + {1}".format(
                          model_ransac.estimator_.coef_[0][0],
                          model_ransac.estimator_.intercept_[0]))
        if not self.no_legend:
            axes.legend(prop={'size': '8'})
        axes.grid()
        if self.xtitle:
            axes.set_xlabel(self.xtitle)
        if self.ytitle:
            axes.set_ylabel(self.ytitle)
        self.data_plot._canvas.draw()
        buffer = StringIO()
        self.data_plot._canvas.figure.savefig(buffer, format="png")
        buffer.seek(0)
        img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8)
        img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR)
        self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
Beispiel #52
0
class PoseViewWidget(QWidget):

    def __init__(self, plugin):
        super(PoseViewWidget, self).__init__()
        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('rqt_pose_view'), 'resource', 'PoseViewWidget.ui')
        loadUi(ui_file, self)
        self._plugin = plugin

        self._position = (2.0, 2.0, 2.0)
        self._orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
        self._topic_name = None
        self._subscriber = None

        # create GL view
        self._gl_view = GLWidget()
        self._gl_view.setAcceptDrops(True)

        # backup and replace original paint method
        self._gl_view.paintGL_original = self._gl_view.paintGL
        self._gl_view.paintGL = self._gl_view_paintGL

        # backup and replace original mouse release method
        self._gl_view.mouseReleaseEvent_original = self._gl_view.mouseReleaseEvent
        self._gl_view.mouseReleaseEvent = self._gl_view_mouseReleaseEvent

        # add GL view to widget layout
        self.layout().addWidget(self._gl_view)

        # init and start update timer with 40ms (25fps)
        self._update_timer = QTimer(self)
        self._update_timer.timeout.connect(self.update_timeout)
        self._update_timer.start(40)

        # subscribe immediately if topic name found on param server
        if rospy.has_param('~pose_view_topic'):
            topic = rospy.get_param('~pose_view_topic')
            try:
                self._subscribe_topic(topic)
            except AttributeError:
                rospy.logwarn("invalid topic name '{}'".format(topic))

    def save_settings(self, plugin_settings, instance_settings):
        view_matrix_string = repr(self._gl_view.get_view_matrix())
        instance_settings.set_value('view_matrix', view_matrix_string)

    def restore_settings(self, plugin_settings, instance_settings):
        view_matrix_string = instance_settings.value('view_matrix')
        try:
            view_matrix = eval(view_matrix_string)
        except Exception:
            view_matrix = None

        if view_matrix is not None:
            self._gl_view.set_view_matrix(view_matrix)
        else:
            self._set_default_view()

    def _set_default_view(self):
        self._gl_view.makeCurrent()
        self._gl_view.reset_view()
        self._gl_view.rotate((0, 0, 1), 45)
        self._gl_view.rotate((1, 0, 0), -65)
        self._gl_view.translate((0, -3, -15))

    def update_timeout(self):
        self._gl_view.makeCurrent()
        self._gl_view.updateGL()

    def _gl_view_paintGL(self):
        self._gl_view.paintGL_original()
        self._paintGLGrid()
        self._paintGLCoorsystem()
        self._paintGLBox()

    def _paintGLBox(self):
        # FIXME: add user configurable setting to allow use of translation as well
        self._position = (2.0, 2.0, 2.0)  # Set fixed translation for now

        glTranslatef(*self._position)     # Translate Box

        matrix = quaternion_matrix(self._orientation)  # convert quaternion to translation matrix
        # tf uses row-major while gl expects column-major
        matrix = matrix.transpose()
        glMultMatrixf(matrix)             # Rotate Box

        glBegin(GL_QUADS)                 # Start Drawing The Box

        glColor3f(0.0, 1.0, 0.0)
        glVertex3f(1.0, 1.0, -1.0)        # Top Right Of The Quad (Top)
        glVertex3f(-1.0, 1.0, -1.0)       # Top Left Of The Quad (Top)
        glVertex3f(-1.0, 1.0, 1.0)        # Bottom Left Of The Quad (Top)
        glVertex3f(1.0, 1.0, 1.0)         # Bottom Right Of The Quad (Top)

        glColor3f(0.5, 1.0, 0.5)
        glVertex3f(1.0, -1.0, 1.0)        # Top Right Of The Quad (Bottom)
        glVertex3f(-1.0, -1.0, 1.0)       # Top Left Of The Quad (Bottom)
        glVertex3f(-1.0, -1.0, -1.0)      # Bottom Left Of The Quad (Bottom)
        glVertex3f(1.0, -1.0, -1.0)       # Bottom Right Of The Quad (Bottom)

        glColor3f(0.0, 0.0, 1.0)
        glVertex3f(1.0, 1.0, 1.0)         # Top Right Of The Quad (Front)
        glVertex3f(-1.0, 1.0, 1.0)        # Top Left Of The Quad (Front)
        glVertex3f(-1.0, -1.0, 1.0)       # Bottom Left Of The Quad (Front)
        glVertex3f(1.0, -1.0, 1.0)        # Bottom Right Of The Quad (Front)

        glColor3f(0.5, 0.5, 1.0)
        glVertex3f(1.0, -1.0, -1.0)       # Bottom Left Of The Quad (Back)
        glVertex3f(-1.0, -1.0, -1.0)      # Bottom Right Of The Quad (Back)
        glVertex3f(-1.0, 1.0, -1.0)       # Top Right Of The Quad (Back)
        glVertex3f(1.0, 1.0, -1.0)        # Top Left Of The Quad (Back)

        glColor3f(1.0, 0.5, 0.5)
        glVertex3f(-1.0, 1.0, 1.0)        # Top Right Of The Quad (Left)
        glVertex3f(-1.0, 1.0, -1.0)       # Top Left Of The Quad (Left)
        glVertex3f(-1.0, -1.0, -1.0)      # Bottom Left Of The Quad (Left)
        glVertex3f(-1.0, -1.0, 1.0)       # Bottom Right Of The Quad (Left)

        glColor3f(1.0, 0.0, 0.0)
        glVertex3f(1.0, 1.0, -1.0)        # Top Right Of The Quad (Right)
        glVertex3f(1.0, 1.0, 1.0)         # Top Left Of The Quad (Right)
        glVertex3f(1.0, -1.0, 1.0)        # Bottom Left Of The Quad (Right)
        glVertex3f(1.0, -1.0, -1.0)       # Bottom Right Of The Quad (Right)
        glEnd()                           # Done Drawing The Quad

    def _paintGLGrid(self):
        resolution_millimeters = 1
        gridded_area_size = 100

        glLineWidth(1.0)

        glBegin(GL_LINES)

        glColor3f(1.0, 1.0, 1.0)

        glVertex3f(gridded_area_size, 0, 0)
        glVertex3f(-gridded_area_size, 0, 0)
        glVertex3f(0, gridded_area_size, 0)
        glVertex3f(0, -gridded_area_size, 0)

        num_of_lines = int(gridded_area_size / resolution_millimeters)

        for i in range(num_of_lines):
            glVertex3f(resolution_millimeters * i, -gridded_area_size, 0)
            glVertex3f(resolution_millimeters * i, gridded_area_size, 0)
            glVertex3f(gridded_area_size, resolution_millimeters * i, 0)
            glVertex3f(-gridded_area_size, resolution_millimeters * i, 0)

            glVertex3f(resolution_millimeters * (-i), -gridded_area_size, 0)
            glVertex3f(resolution_millimeters * (-i), gridded_area_size, 0)
            glVertex3f(gridded_area_size, resolution_millimeters * (-i), 0)
            glVertex3f(-gridded_area_size, resolution_millimeters * (-i), 0)

        glEnd()

    def _paintGLCoorsystem(self):
        glLineWidth(10.0)

        glBegin(GL_LINES)

        glColor3f(1.0, 0.0, 0.0)
        glVertex3f(0.0, 0.0, 0.0)
        glVertex3f(1.0, 0.0, 0.0)

        glColor3f(0.0, 1.0, 0.0)
        glVertex3f(0.0, 0.0, 0.0)
        glVertex3f(0.0, 1.0, 0.0)

        glColor3f(0.0, 0.0, 1.0)
        glVertex3f(0.0, 0.0, 0.0)
        glVertex3f(0.0, 0.0, 1.0)

        glEnd()

    def _gl_view_mouseReleaseEvent(self, event):
        if event.button() == Qt.RightButton:
            menu = QMenu(self._gl_view)
            action = QAction(self._gl_view.tr("Reset view"), self._gl_view)
            menu.addAction(action)
            action.triggered.connect(self._set_default_view)
            menu.exec_(self._gl_view.mapToGlobal(event.pos()))

    @Slot('QDragEnterEvent*')
    def dragEnterEvent(self, event):
        if event.mimeData().hasText():
            topic_name = str(event.mimeData().text())
            if len(topic_name) == 0:
                qWarning('PoseViewWidget.dragEnterEvent(): event.mimeData() text is empty')
                return
        else:
            if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
                qWarning('PoseViewWidget.dragEnterEvent(): event.source() has no attribute selectedItems or length of selectedItems is 0')
                return
            item = event.source().selectedItems()[0]
            topic_name = item.data(0, Qt.UserRole)

            if topic_name is None:
                qWarning('PoseViewWidget.dragEnterEvent(): selectedItem has no UserRole data with a topic name')
                return

        # check for valid topic
        msg_class, self._topic_name, _ = get_topic_class(topic_name)
        if msg_class is None:
            qWarning('PoseViewWidget.dragEnterEvent(): No message class was found for topic "%s".' % topic_name)
            return

        # check for valid message class
        quaternion_slot_path, point_slot_path = self._get_slot_paths(msg_class)

        if quaternion_slot_path is None and point_slot_path is None:
            qWarning('PoseViewWidget.dragEnterEvent(): No Pose, Quaternion or Point data was found outside of arrays in "%s" on topic "%s".'
                     % (msg_class._type, topic_name))
            return

        event.acceptProposedAction()

    @Slot('QDropEvent*')
    def dropEvent(self, event):
        if event.mimeData().hasText():
            topic_name = str(event.mimeData().text())
        else:
            dropped_item = event.source().selectedItems()[0]
            topic_name = str(dropped_item.data(0, Qt.UserRole))

        self._unregister_topic()
        self._subscribe_topic(topic_name)

    def _unregister_topic(self):
        if self._subscriber:
            self._subscriber.unregister()

    @staticmethod
    def _make_path_list_from_path_string(path):
        path = path.split('/')
        if path == ['']:
            return []
        return path

    @staticmethod
    def _get_slot_paths(msg_class):
        # find first Pose in msg_class
        pose_slot_paths = find_slots_by_type_bfs(msg_class, Pose)
        for path in pose_slot_paths:
            # make sure the path does not contain an array, because we don't want to deal with empty arrays...
            if '[' not in path:
                path = PoseViewWidget._make_path_list_from_path_string(pose_slot_paths[0])
                return path + ['orientation'], path + ['position']

        # if no Pose is found, find first Quaternion and Point
        quaternion_slot_paths = find_slots_by_type_bfs(msg_class, Quaternion)
        for path in quaternion_slot_paths:
            if '[' not in path:
                quaternion_slot_path = PoseViewWidget._make_path_list_from_path_string(path)
                break
        else:
            quaternion_slot_path = None

        point_slot_paths = find_slots_by_type_bfs(msg_class, Point)
        for path in point_slot_paths:
            if '[' not in path:
                point_slot_path = PoseViewWidget._make_path_list_from_path_string(path)
                break
        else:
            point_slot_path = None

        return quaternion_slot_path, point_slot_path


    def _subscribe_topic(self, topic_name):
        msg_class, self._topic_name, _ = get_topic_class(topic_name)
        quaternion_slot_path, point_slot_path = self._get_slot_paths(msg_class)

        self._subscriber = rospy.Subscriber(
                self._topic_name,
                msg_class,
                self.message_callback,
                callback_args=(quaternion_slot_path, point_slot_path)
        )

    def message_callback(self, message, slot_paths):
        quaternion_slot_path = slot_paths[0]
        point_slot_path = slot_paths[1]

        if quaternion_slot_path is None:
            self._orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
        else:
            orientation = message
            for slot_name in quaternion_slot_path:
                orientation = getattr(orientation, slot_name)
            self._orientation = (orientation.x, orientation.y, orientation.z, orientation.w)

        if point_slot_path is None:
            # if no point is given, set it to a fixed offset so the axes can be seen
            self._position = (2.0, 2.0, 2.0)
        else:
            position = message
            for slot_name in point_slot_path:
                position = getattr(position, slot_name)
            self._position = (position.x, position.y, position.z)

    def shutdown_plugin(self):
        self._unregister_topic()
Beispiel #53
0
class DRCEnvironmentViewerWidget(QWidget):
    _SMILEY = ":)"
    _FROWN = ":("
    _OK_COLOR = QColor("#18FFFF")
    _DISABLED_COLOR = QColor("#BDBDBD")
    _BLACKOUT_COLOR = QColor("#F44336")

    def __init__(self):
        self.lock = Lock()
        super(DRCEnvironmentViewerWidget, self).__init__()
        self.is_disabled = False
        self.is_blackout = False
        self.next_whiteout_time = rospy.Time.now()
        self.blackout_time = rospy.Time.now()
        self.event = None
        self.sub_is_disabled = rospy.Subscriber(
            "/drc_2015_environment/is_disabled", Bool, self.isDisabledCallback)
        self.sub_is_blackout = rospy.Subscriber(
            "/drc_2015_environment/is_blackout", Bool, self.isBlackoutCallback)
        self.sub_next_whiteout_time = rospy.Subscriber(
            "/drc_2015_environment/next_whiteout_time", Time,
            self.nextWhiteoutTimeCallback)
        self._update_plot_timer = QTimer(self)
        self._update_plot_timer.timeout.connect(self.redraw)
        self._update_plot_timer.start(1000 / 15)

    def isDisabledCallback(self, msg):
        with self.lock:
            self.is_disabled = msg.data

    def isBlackoutCallback(self, msg):
        with self.lock:
            if not self.is_blackout and msg.data:
                self.blackout_time = rospy.Time.now()
            self.is_blackout = msg.data

    def nextWhiteoutTimeCallback(self, msg):
        with self.lock:
            self.next_whiteout_time = msg.data

    def redraw(self):
        self.update()
        # if self.event:
        #     self.paintEvent(self.event)

    def paintEvent(self, event):
        with self.lock:
            self.event = event
            rect = event.rect()
            qp = QPainter()
            qp.begin(self)
            radius = min(rect.width(), rect.height()) - 50
            qp.setFont(QFont('Helvetica', 100))
            qp.setPen(QPen(QBrush(QColor(255, 255, 255)), 20))

            if self.is_disabled:
                qp.fillRect(rect, self._DISABLED_COLOR)
                qp.drawText(rect, QtCore.Qt.AlignCenter, self._FROWN)
            elif self.is_blackout:
                qp.fillRect(rect, self._BLACKOUT_COLOR)
                qp.drawText(rect, QtCore.Qt.AlignCenter, self._FROWN)
                time_diff = (self.next_whiteout_time -
                             rospy.Time.now()).to_sec()
                if time_diff < 0:
                    time_diff = 0
                time_ratio = time_diff / (self.next_whiteout_time -
                                          self.blackout_time).to_sec()
                qp.setFont(QFont('Helvetica', 30))
                qp.drawText(0,
                            rect.height() - 150, rect.width(), 150,
                            QtCore.Qt.AlignCenter, "%.1f sec" % time_diff)
                # 0-360
                if time_ratio > 0:
                    rad = int(math.fmod(time_ratio * 360 + 90 * 16, 360) * 16)
                    qp.drawArc((rect.width() - radius) / 2,
                               (rect.height() - radius) / 2, radius, radius,
                               90 * 16, rad)
            else:
                qp.fillRect(rect, self._OK_COLOR)
                qp.drawText(rect, QtCore.Qt.AlignCenter, self._SMILEY)
            qp.end()
Beispiel #54
0
class Smach(Plugin):
    update_graph_sig = Signal(str)

    def __init__(self, context):
        super(Smach, self).__init__(context)

        self.initialized = 0

        self._dotcode_sub = None
        self._topic_dict = {}
        self._update_thread = WorkerThread(self._update_thread_run,
                                           self._update_finished)

        # Give QObjects reasonable names
        self.setObjectName('Smach')

        # 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 is a sibling of this file
        # in this example the .ui and .py file are in the same folder
        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
                               'rqt_smach.ui')

        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)

        # Give QObjects reasonable names
        self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
        self._widget.setObjectName('SmachPluginUi')

        # 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 widget to the user interface
        context.add_widget(self._widget)
        palette = QPalette()
        palette.setColor(QPalette.Background, Qt.white)
        self._widget.setPalette(palette)

        #Connect widgets with corresponding methods
        self._widget.namespace_input.currentIndexChanged.connect(
            self._handle_ns_changed)
        self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
        self._widget.restrict_ns.clicked.connect(self.refresh_combo_box)
        self._widget.ud_path_input.currentIndexChanged.connect(
            self._handle_ud_path)
        self._widget.ud_set_initial.clicked.connect(self._handle_ud_set_path)
        self._widget.ud_text_browser.setReadOnly(1)
        self._widget.show_implicit_button.clicked.connect(
            self._handle_show_implicit)
        self._widget.help_button.setIcon(QIcon.fromTheme('help-contents'))
        self._widget.help_button.clicked.connect(self._handle_help)
        self._widget.tree.clicked.connect(self._handle_tree_clicked)

        #Depth and width spinners:
        self._widget.depth_input.setRange(-1, 1337)
        self._widget.depth_input.setValue(-1)
        self._widget.depth_input.valueChanged.connect(self._set_depth)

        self._widget.label_width_input.setRange(1, 1337)
        self._widget.label_width_input.setValue(40)
        self._widget.label_width_input.valueChanged.connect(self._set_width)

        self._widget.tree.setColumnCount(1)
        self._widget.tree.setHeaderLabels(["Containers"])
        self._widget.tree.show()

        self._ns = ""
        self.refresh_combo_box()

        # Bind path list
        self._widget.path_input.currentIndexChanged.connect(
            self._handle_path_changed)

        #Keep Combo Boxes sorted
        self._widget.namespace_input.setInsertPolicy(6)
        self._widget.path_input.setInsertPolicy(6)
        self._widget.ud_path_input.setInsertPolicy(6)

        #Set up mouse actions for xdot widget
        self._widget.xdot_widget.register_select_callback(self.select_cb)

        # Create graph data structures
        # Containers is a map of (container path) -> container proxy
        self._containers = {}
        self._top_containers = {}

        # smach introspection client
        self._client = smach_ros.IntrospectionClient()
        self._selected_paths = []

        # Message subscribers
        self._structure_subs = {}
        self._status_subs = {}

        # Initialize xdot display state
        self._path = '/'
        self._needs_zoom = True
        self._structure_changed = True
        self._max_depth = -1
        self._show_all_transitions = False
        self._label_wrapper = textwrap.TextWrapper(40, break_long_words=True)
        self._graph_needs_refresh = True
        self._tree_needs_refresh = True

        self._keep_running = True

        self._update_server_list()
        self._update_graph()
        self._update_tree()

        # Start a timer to update the server list
        self._server_timer = QTimer(self)
        self._server_timer.timeout.connect(self._update_server_list)
        self._server_timer.start(1000)

        # Start a timer to update the graph display
        self._graph_timer = QTimer(self)
        self._graph_timer.timeout.connect(self._update_graph)
        self._graph_timer.start(1093)

        # Start a timer to update the._widget.tree display
        self._tree_timer = QTimer(self)
        self._tree_timer.timeout.connect(self._update_tree)
        self._tree_timer.start(1217)

    def _handle_tree_clicked(self):
        selected = self._widget.tree.selectedItems()[0]
        path = "/" + str(selected.text(0))
        parent = selected.parent()
        while parent:
            path = "/" + str(parent.text(0)) + path
            parent = parent.parent()
        self._widget.ud_path_input.setCurrentIndex(
            self._widget.ud_path_input.findText(path))

    def _handle_help(self):
        """Event: Help button pressed"""
        helpMsg = QMessageBox()
        helpMsg.setWindowTitle("Keyboard Controls")
        helpMsg.setIcon(QMessageBox.Information)
        helpMsg.setText(
            "Pan: Arrow Keys\nZoom: PageUp / PageDown\nZoom To Fit: F\nRefresh: R"
        )
        ret = helpMsg.exec_()

    def select_cb(self, event):
        """Event: Click to select a graph node to display user data and update the graph."""
        # Only set string status
        x = event.x()
        y = event.y()
        url = self._widget.xdot_widget.get_url(x, y)

        if url is None:
            return

        item = self._widget.xdot_widget.items_by_url.get(url.url, None)

        if item:
            self._widget.status_bar.showMessage(item.url)
            # Left button-up

            if item.url not in self._containers:
                if ':' in item.url:
                    container_url = '/'.join(item.url.split(':')[:-1])
                else:
                    container_url = '/'.join(item.url.split('/')[:-1])
            else:
                container_url = item.url

            if event.button() == Qt.LeftButton:
                self._selected_paths = [item.url]
                self._widget.ud_path_input.setCurrentIndex(
                    self._widget.ud_path_input.findText(item.url))

        self._graph_needs_refresh = True

    def _handle_show_implicit(self):
        """Event: Show Implicit button checked"""
        self._show_all_transitions = not self._show_all_transitions
        self._graph_needs_refresh = True
        self._structure_changed = True
        if self._widget.zoom_checkbox.isChecked():
            self._needs_zoom = True

    def _set_depth(self):
        """Event: Depth value changed"""
        self._max_depth = self._widget.depth_input.value()
        self._graph_needs_refresh = True
        self._structure_changed = True
        if self._widget.zoom_checkbox.isChecked():
            self._needs_zoom = True

    def _set_width(self):
        """Event: Label Width value changed"""
        self._label_wrapper.width = self._widget.label_width_input.value()
        self._graph_needs_refresh = True

    def _handle_ud_set_path(self):
        """Event: Set as Initial State button pressed"""
        state_path = self._widget.ud_path_input.currentText()
        parent_path = get_parent_path(state_path)
        if len(parent_path) > 0:
            state = get_label(state_path)
            server_name = self._containers[parent_path]._server_name
            self._client.set_initial_state(server_name,
                                           parent_path, [state],
                                           timeout=rospy.Duration(60.0))
            self._structure_changed = True
            self._graph_needs_refresh = True
            if self._widget.zoom_checkbox.isChecked():
                self._needs_zoom = True

    def _handle_ud_path(self):
        """Event: User Data selection combo box changed"""
        path = self._widget.ud_path_input.currentText()
        #Check the path is non-zero length
        if len(path) > 0:
            # Get the container corresponding to this path, since userdata is
            # stored in the containers
            if path not in self._containers:
                parent_path = get_parent_path(path)
            else:
                parent_path = path

            if parent_path not in self._containers:
                parent_path = get_parent_path(parent_path)

            if parent_path in self._containers:
                # Get the container
                container = self._containers[parent_path]

                # Generate the userdata string
                ud_str = ''
                for (k, v) in container._local_data._data.iteritems():
                    ud_str += str(k) + ": "
                    vstr = str(v)
                    # Add a line break if this is a multiline value
                    if vstr.find('\n') != -1:
                        ud_str += '\n'
                    ud_str += vstr + '\n\n'
                #Display the user data
                self._widget.ud_text_browser.setPlainText(ud_str)
                self._widget.ud_text_browser.show()

    def _update_server_list(self):
        """Update the list of known SMACH introspection servers."""
        # Discover new servers
        if self._widget.restrict_ns.isChecked():
            server_names = [self._widget.namespace_input.currentText()[0:-1]]
            #self._status_subs = {}
        else:
            server_names = self._client.get_servers()

        new_server_names = [
            sn for sn in server_names if sn not in self._status_subs
        ]

        # Create subscribers for newly discovered servers
        for server_name in new_server_names:

            # Create a subscriber for the plan structure (topology) published by this server
            self._structure_subs[server_name] = rospy.Subscriber(
                server_name + smach_ros.introspection.STRUCTURE_TOPIC,
                SmachContainerStructure,
                callback=self._structure_msg_update,
                callback_args=server_name,
                queue_size=50)

            # Create a subscriber for the active states in the plan published by this server
            self._status_subs[server_name] = rospy.Subscriber(
                server_name + smach_ros.introspection.STATUS_TOPIC,
                SmachContainerStatus,
                callback=self._status_msg_update,
                queue_size=50)

    def _structure_msg_update(self, msg, server_name):
        """Update the structure of the SMACH plan (re-generate the dotcode)."""

        # Just return if we're shutting down
        if not self._keep_running:
            return

        # Get the node path
        path = msg.path
        pathsplit = path.split('/')
        parent_path = '/'.join(pathsplit[0:-1])

        rospy.logdebug("RECEIVED: " + path)
        rospy.logdebug("CONTAINERS: " + str(self._containers.keys()))

        # Initialize redraw flag
        needs_redraw = False

        # Determine if we need to update one of the proxies or create a new one
        if path in self._containers:
            rospy.logdebug("UPDATING: " + path)

            # Update the structure of this known container
            needs_redraw = self._containers[path].update_structure(msg)
        else:
            rospy.logdebug("CONSTRUCTING: " + path)

            # Create a new container
            container = ContainerNode(server_name, msg)
            self._containers[path] = container

            #Add items to ud path combo box
            if self._widget.ud_path_input.findText(path) == -1:
                self._widget.ud_path_input.addItem(path)

            for item in container._children:
                if self._widget.ud_path_input.findText(path + "/" +
                                                       item) == -1:
                    self._widget.ud_path_input.addItem(path + "/" + item)

            # Store this as a top container if it has no parent
            if parent_path == '':
                self._top_containers[path] = container

            # Append the path to the appropriate widgets
            self._append_new_path(path)

            # We need to redraw the graph if this container's parent is being viewed
            if path.find(self._widget.path_input.currentText()) == 0:
                needs_redraw = True

        if needs_redraw:
            self._structure_changed = True
            if self._widget.zoom_checkbox.isChecked():
                self._needs_zoom = True
            self._tree_needs_refresh = True
            self._graph_needs_refresh = True

    def _status_msg_update(self, msg):
        """Process status messages."""

        # Check if we're in the process of shutting down
        if not self._keep_running:
            return

        # Get the path to the updating conainer
        path = msg.path
        rospy.logdebug("STATUS MSG: " + path)

        # Check if this is a known container
        if path in self._containers:
            # Get the container and check if the status update requires regeneration
            container = self._containers[path]
            if container.update_status(msg):
                self._graph_needs_refresh = True
                self._tree_needs_refresh = True

    def _append_new_path(self, path):
        """Append a new path to the path selection widgets"""
        if ((not self._widget.restrict_ns.isChecked())
                or ((self._widget.restrict_ns.isChecked()) and
                    (self._widget.namespace_input.currentText() in path))
                or (path == self._widget.namespace_input.currentText()[0:-1])):
            self._widget.path_input.addItem(path)

    def _update_graph(self):
        """This thread continuously updates the graph when it changes.

        The graph gets updated in one of two ways:

          1: The structure of the SMACH plans has changed, or the display
          settings have been changed. In this case, the dotcode needs to be
          regenerated.

          2: The status of the SMACH plans has changed. In this case, we only
          need to change the styles of the graph.
        """
        if self._keep_running and self._graph_needs_refresh and not rospy.is_shutdown(
        ):
            # Get the containers to update
            containers_to_update = {}
            # Check if the path that's currently being viewed is in the
            # list of existing containers
            if self._path in self._containers:
                # Some non-root path
                containers_to_update = {
                    self._path: self._containers[self._path]
                }
            elif self._path == '/':
                # Root path
                containers_to_update = self._top_containers

            # Check if we need to re-generate the dotcode (if the structure changed)
            # TODO: needs_zoom is a misnomer
            if self._structure_changed or self._needs_zoom:
                dotstr = "digraph {\n\t"
                dotstr += ';'.join([
                    "compound=true",
                    "outputmode=nodesfirst",
                    "labeljust=l",
                    "nodesep=0.5",
                    "minlen=2",
                    "mclimit=5",
                    "clusterrank=local",
                    "ranksep=0.75",
                    # "remincross=true",
                    # "rank=sink",
                    "ordering=\"\"",
                ])
                dotstr += ";\n"

                # Generate the rest of the graph
                # TODO: Only re-generate dotcode for containers that have changed
                for path, container in containers_to_update.items():
                    dotstr += container.get_dotcode(self._selected_paths, [],
                                                    0, self._max_depth,
                                                    self._containers,
                                                    self._show_all_transitions,
                                                    self._label_wrapper)

                # The given path isn't available
                if len(containers_to_update) == 0:
                    dotstr += '"__empty__" [label="Path not available.", shape="plaintext"]'

                dotstr += '\n}\n'

                # Set the dotcode to the new dotcode, reset the flags
                self.set_dotcode(dotstr, zoom=False)
                self._structure_changed = False
                self._graph_needs_refresh = False

            # Update the styles for the graph if there are any updates
            for path, container in containers_to_update.items():
                container.set_styles(self._selected_paths, 0, self._max_depth,
                                     self._widget.xdot_widget.items_by_url,
                                     self._widget.xdot_widget.subgraph_shapes,
                                     self._containers)
            self._widget.xdot_widget.repaint()

    def set_dotcode(self, dotcode, zoom=True):
        """Set the xdot view's dotcode and refresh the display."""
        # Set the new dotcode
        if self._widget.xdot_widget.set_dotcode(dotcode, False):
            # Re-zoom if necessary
            if zoom or self._needs_zoom:
                self._widget.xdot_widget.zoom_to_fit()
                self._needs_zoom = False

    def _update_tree(self):
        """Update the tree view."""
        if self._keep_running and self._tree_needs_refresh and not rospy.is_shutdown(
        ):
            self._tree_nodes = {}
            self._widget.tree.clear()
            for path, tc in self._top_containers.iteritems():
                if ((not self._widget.restrict_ns.isChecked()) or
                    ((self._widget.restrict_ns.isChecked()) and
                     (self._widget.namespace_input.currentText() in path)) or
                    (path
                     == self._widget.namespace_input.currentText()[0:-1])):
                    self.add_to_tree(path, None)
            self._tree_needs_refresh = False
            self._widget.tree.sortItems(0, 0)

    def add_to_tree(self, path, parent):
        """Add a path to the tree view."""
        if parent is None:
            container = QTreeWidgetItem()
            container.setText(0, self._containers[path]._label)
            self._widget.tree.addTopLevelItem(container)
        else:
            container = QTreeWidgetItem(parent)
            container.setText(0, self._containers[path]._label)

        # Add children to_tree
        for label in self._containers[path]._children:
            child_path = '/'.join([path, label])
            if child_path in self._containers.keys():
                self.add_to_tree(child_path, container)
            else:
                child = QTreeWidgetItem(container)
                child.setText(0, label)

    def append_tree(self, container, parent=None):
        """Append an item to the tree view."""
        if not parent:
            node = QTreeWidgetItem()
            node.setText(0, container._label)
            self._widget.tree.addTopLevelItem(node)
            for child_label in container._children:
                child = QTreeWidgetItem(node)
                child.setText(0, child_label)

    def _update_thread_run(self):
        """Update the list of namespaces."""
        _, _, topic_types = rospy.get_master().getTopicTypes()
        self._topic_dict = dict(topic_types)
        keys = list(self._topic_dict.keys())
        namespaces = list()
        for i in keys:
            print i
            if i.endswith("smach/container_status"):
                namespaces.append(i[0:i.index("smach/container_status")])
        self._widget.namespace_input.setItems.emit(namespaces)

    @Slot()
    def _update_finished(self):
        """Enable namespace combo box."""
        self._widget.namespace_input.setEnabled(True)

    def _handle_ns_changed(self):
        """If namespace selection is changed then reinitialize everything."""
        ns = self._widget.namespace_input.currentText()
        if len(ns) > 0:
            if self._ns != ns:
                self._actions_connected = False
                self._ns = ns
                self.enable_widgets(False)
                rospy.loginfo("Creating action clients on namespace '%s'..." %
                              ns)
                rospy.loginfo("Action clients created.")
                self._actions_connected = True
                self.enable_widgets(True)

                self._containers = {}
                self._top_containers = {}
                self._selected_paths = []

                self._structure_subs = {}
                self._status_subs = {}

                self._needs_zoom = True
                self._structure_changed = True
                self._max_depth = -1
                self._show_all_transitions = False
                self._graph_needs_refresh = True
                self._tree_needs_refresh = True
                #self._widget.namespace_input.clear()
                self._widget.path_input.clear()
                self._widget.ud_path_input.clear()
                self._widget.tree.clear()

    @Slot()
    def refresh_combo_box(self):
        """Refresh namespace combo box."""
        self._update_thread.kill()
        self._containers = {}
        self._top_containers = {}
        self._selected_paths = []

        self._structure_subs = {}
        self._status_subs = {}

        self._structure_changed = True
        self._tree_needs_refresh = True
        #self._graph_needs_refresh = True

        #self._widget.namespace_input.clear()
        self._widget.path_input.clear()
        self._widget.ud_path_input.clear()
        self._widget.tree.clear()
        if self._widget.restrict_ns.isChecked():
            self._widget.namespace_input.setEnabled(False)
            self._widget.namespace_input.setEditText('updating...')
            self._widget.ns_refresh_button.setEnabled(True)
            self._update_thread.start()
        else:
            self._widget.namespace_input.setEnabled(False)
            self._widget.namespace_input.setEditText('Unrestricted')
            self._widget.ns_refresh_button.setEnabled(False)
            self._graph_needs_refresh = True
            self._tree_needs_refresh = True
            self._widget.path_input.addItem('/')

    def _handle_path_changed(self, checked):
        """If the path input is changed, update graph."""
        self._path = self._widget.path_input.currentText()
        self._graph_needs_refresh = True
        self._structure_changed = True
        if self._widget.zoom_checkbox.isChecked():
            self._needs_zoom = True

    def enable_widgets(self, enable):
        """Enable all widgets."""
        self._widget.xdot_widget.setEnabled(enable)
        self._widget.path_input.setEnabled(enable)
        self._widget.depth_input.setEnabled(enable)
        self._widget.label_width_input.setEnabled(enable)
        self._widget.ud_path_input.setEnabled(enable)
        self._widget.ud_text_browser.setEnabled(enable)
Beispiel #55
0
class TopicWidget(QWidget):
    """
    main class inherits from the ui window class.

    You can specify the topics that the topic pane.

    TopicWidget.start must be called in order to update topic pane.
    """

    SELECT_BY_NAME = 0
    SELECT_BY_MSGTYPE = 1

    _column_names = ['topic', 'type', 'bandwidth', 'rate', 'value']

    def __init__(self,
                 plugin=None,
                 selected_topics=None,
                 select_topic_type=SELECT_BY_NAME):
        """
        @type selected_topics: list of tuples.
        @param selected_topics: [($NAME_TOPIC$, $TYPE_TOPIC$), ...]
        @type select_topic_type: int
        @param select_topic_type: Can specify either the name of topics or by
                                  the type of topic, to filter the topics to
                                  show. If 'select_topic_type' argument is
                                  None, this arg shouldn't be meaningful.
        """
        super(TopicWidget, self).__init__()

        self._select_topic_type = select_topic_type

        rp = rospkg.RosPack()
        ui_file = os.path.join(rp.get_path('rqt_topic'), 'resource',
                               'TopicWidget.ui')
        loadUi(ui_file, self)
        self._plugin = plugin
        self.topics_tree_widget.sortByColumn(0, Qt.AscendingOrder)
        header = self.topics_tree_widget.header()
        header.setResizeMode(QHeaderView.ResizeToContents)
        header.customContextMenuRequested.connect(
            self.handle_header_view_customContextMenuRequested)
        header.setContextMenuPolicy(Qt.CustomContextMenu)

        # Whether to get all topics or only the topics that are set in advance.
        # Can be also set by the setter method "set_selected_topics".
        self._selected_topics = selected_topics

        self._current_topic_list = []
        self._topics = {}
        self._tree_items = {}
        self._column_index = {}
        for column_name in self._column_names:
            self._column_index[column_name] = len(self._column_index)

        # self.refresh_topics()

        # init and start update timer
        self._timer_refresh_topics = QTimer(self)
        self._timer_refresh_topics.timeout.connect(self.refresh_topics)

    def set_topic_specifier(self, specifier):
        self._select_topic_type = specifier

    def start(self):
        """
        This method needs to be called to start updating topic pane.
        """
        self._timer_refresh_topics.start(1000)

    @Slot()
    def refresh_topics(self):
        """
        refresh tree view items
        """

        if self._selected_topics is None:
            topic_list = rospy.get_published_topics()
            if topic_list is None:
                rospy.logerr(
                    'Not even a single published topic found. Check network configuration'
                )
                return
        else:  # Topics to show are specified.
            topic_list = self._selected_topics
            topic_specifiers_server_all = None
            topic_specifiers_required = None

            rospy.logdebug('refresh_topics) self._selected_topics=%s' %
                           (topic_list, ))

            if self._select_topic_type == self.SELECT_BY_NAME:
                topic_specifiers_server_all = [
                    name for name, type in rospy.get_published_topics()
                ]
                topic_specifiers_required = [name for name, type in topic_list]
            elif self._select_topic_type == self.SELECT_BY_MSGTYPE:
                # The topics that are required (by whoever uses this class).
                topic_specifiers_required = [type for name, type in topic_list]

                # The required topics that match with published topics.
                topics_match = [(name, type)
                                for name, type in rospy.get_published_topics()
                                if type in topic_specifiers_required]
                topic_list = topics_match
                rospy.logdebug('selected & published topic types=%s' %
                               (topic_list, ))

            rospy.logdebug('server_all=%s\nrequired=%s\ntlist=%s' %
                           (topic_specifiers_server_all,
                            topic_specifiers_required, topic_list))
            if len(topic_list) == 0:
                rospy.logerr(
                    'None of the following required topics are found.\n(NAME, TYPE): %s'
                    % (self._selected_topics, ))
                return

        if self._current_topic_list != topic_list:
            self._current_topic_list = topic_list

            # start new topic dict
            new_topics = {}

            for topic_name, topic_type in topic_list:
                # if topic is new or has changed its type
                if topic_name not in self._topics or \
                   self._topics[topic_name]['type'] != topic_type:
                    # create new TopicInfo
                    topic_info = TopicInfo(topic_name, topic_type)
                    message_instance = None
                    if topic_info.message_class is not None:
                        message_instance = topic_info.message_class()
                    # add it to the dict and tree view
                    topic_item = self._recursive_create_widget_items(
                        self.topics_tree_widget, topic_name, topic_type,
                        message_instance)
                    new_topics[topic_name] = {
                        'item': topic_item,
                        'info': topic_info,
                        'type': topic_type,
                    }
                else:
                    # if topic has been seen before, copy it to new dict and
                    # remove it from the old one
                    new_topics[topic_name] = self._topics[topic_name]
                    del self._topics[topic_name]

            # clean up old topics
            for topic_name in self._topics.keys():
                self._topics[topic_name]['info'].stop_monitoring()
                index = self.topics_tree_widget.indexOfTopLevelItem(
                    self._topics[topic_name]['item'])
                self.topics_tree_widget.takeTopLevelItem(index)
                del self._topics[topic_name]

            # switch to new topic dict
            self._topics = new_topics

        self._update_topics_data()

    def _update_topics_data(self):
        for topic in self._topics.values():
            topic_info = topic['info']
            if topic_info.monitoring:
                # update rate
                rate, _, _, _ = topic_info.get_hz()
                rate_text = '%1.2f' % rate if rate != None else 'unknown'

                # update bandwidth
                bytes_per_s, _, _, _ = topic_info.get_bw()
                if bytes_per_s is None:
                    bandwidth_text = 'unknown'
                elif bytes_per_s < 1000:
                    bandwidth_text = '%.2fB/s' % bytes_per_s
                elif bytes_per_s < 1000000:
                    bandwidth_text = '%.2fKB/s' % (bytes_per_s / 1000.)
                else:
                    bandwidth_text = '%.2fMB/s' % (bytes_per_s / 1000000.)

                # update values
                value_text = ''
                self.update_value(topic_info._topic_name,
                                  topic_info.last_message)

            else:
                rate_text = ''
                bandwidth_text = ''
                value_text = 'not monitored' if topic_info.error is None else topic_info.error

            self._tree_items[topic_info._topic_name].setText(
                self._column_index['rate'], rate_text)
            self._tree_items[topic_info._topic_name].setText(
                self._column_index['bandwidth'], bandwidth_text)
            self._tree_items[topic_info._topic_name].setText(
                self._column_index['value'], value_text)

    def update_value(self, topic_name, message):
        if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
            for slot_name in message.__slots__:
                self.update_value(topic_name + '/' + slot_name,
                                  getattr(message, slot_name))

        elif type(message) in (list, tuple) and (len(message) > 0) and hasattr(
                message[0], '__slots__'):

            for index, slot in enumerate(message):
                if topic_name + '[%d]' % index in self._tree_items:
                    self.update_value(topic_name + '[%d]' % index, slot)
                else:
                    base_type_str, _ = self._extract_array_info(
                        self._tree_items[topic_name].text(
                            self._column_index['type']))
                    self._recursive_create_widget_items(
                        self._tree_items[topic_name],
                        topic_name + '[%d]' % index, base_type_str, slot)
            # remove obsolete children
            if len(message) < self._tree_items[topic_name].childCount():
                for i in range(len(message),
                               self._tree_items[topic_name].childCount()):
                    item_topic_name = topic_name + '[%d]' % i
                    self._recursive_delete_widget_items(
                        self._tree_items[item_topic_name])
        else:
            if topic_name in self._tree_items:
                self._tree_items[topic_name].setText(
                    self._column_index['value'], repr(message))

    def _extract_array_info(self, type_str):
        array_size = None
        if '[' in type_str and type_str[-1] == ']':
            type_str, array_size_str = type_str.split('[', 1)
            array_size_str = array_size_str[:-1]
            if len(array_size_str) > 0:
                array_size = int(array_size_str)
            else:
                array_size = 0

        return type_str, array_size

    def _recursive_create_widget_items(self, parent, topic_name, type_name,
                                       message):
        if parent is self.topics_tree_widget:
            # show full topic name with preceding namespace on toplevel item
            topic_text = topic_name
            item = TreeWidgetItem(self._toggle_monitoring, topic_name, parent)
        else:
            topic_text = topic_name.split('/')[-1]
            if '[' in topic_text:
                topic_text = topic_text[topic_text.index('['):]
            item = QTreeWidgetItem(parent)
        item.setText(self._column_index['topic'], topic_text)
        item.setText(self._column_index['type'], type_name)
        item.setData(0, Qt.UserRole, topic_name)
        self._tree_items[topic_name] = item
        if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
            for slot_name, type_name in zip(message.__slots__,
                                            message._slot_types):
                self._recursive_create_widget_items(
                    item, topic_name + '/' + slot_name, type_name,
                    getattr(message, slot_name))

        else:
            base_type_str, array_size = self._extract_array_info(type_name)
            try:
                base_instance = roslib.message.get_message_class(
                    base_type_str)()
            except (ValueError, TypeError):
                base_instance = None
            if array_size is not None and hasattr(base_instance, '__slots__'):
                for index in range(array_size):
                    self._recursive_create_widget_items(
                        item, topic_name + '[%d]' % index, base_type_str,
                        base_instance)
        return item

    def _toggle_monitoring(self, topic_name):
        item = self._tree_items[topic_name]
        if item.checkState(0):
            self._topics[topic_name]['info'].start_monitoring()
        else:
            self._topics[topic_name]['info'].stop_monitoring()

    def _recursive_delete_widget_items(self, item):
        def _recursive_remove_items_from_tree(item):
            for index in reversed(range(item.childCount())):
                _recursive_remove_items_from_tree(item.child(index))
            topic_name = item.data(0, Qt.UserRole)
            del self._tree_items[topic_name]

        _recursive_remove_items_from_tree(item)
        item.parent().removeChild(item)

    @Slot('QPoint')
    def handle_header_view_customContextMenuRequested(self, pos):
        header = self.topics_tree_widget.header()

        # show context menu
        menu = QMenu(self)
        action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize')
        action = menu.exec_(header.mapToGlobal(pos))

        # evaluate user action
        if action is action_toggle_auto_resize:
            if header.resizeMode(0) == QHeaderView.ResizeToContents:
                header.setResizeMode(QHeaderView.Interactive)
            else:
                header.setResizeMode(QHeaderView.ResizeToContents)

    @Slot('QPoint')
    def on_topics_tree_widget_customContextMenuRequested(self, pos):
        item = self.topics_tree_widget.itemAt(pos)
        if item is None:
            return

        # show context menu
        menu = QMenu(self)
        action_item_expand = menu.addAction(QIcon.fromTheme('zoom-in'),
                                            'Expand All Children')
        action_item_collapse = menu.addAction(QIcon.fromTheme('zoom-out'),
                                              'Collapse All Children')
        action = menu.exec_(self.topics_tree_widget.mapToGlobal(pos))

        # evaluate user action
        if action in (action_item_expand, action_item_collapse):
            expanded = (action is action_item_expand)

            def recursive_set_expanded(item):
                item.setExpanded(expanded)
                for index in range(item.childCount()):
                    recursive_set_expanded(item.child(index))

            recursive_set_expanded(item)

    def shutdown_plugin(self):
        for topic in self._topics.values():
            topic['info'].stop_monitoring()
        self._timer_refresh_topics.stop()

    def set_selected_topics(self, selected_topics):
        """
        @param selected_topics: list of tuple. [(topic_name, topic_type)]
        @type selected_topics: []
        """
        rospy.logdebug('set_selected_topics topics={}'.format(
            len(selected_topics)))
        self._selected_topics = selected_topics

    # TODO(Enhancement) Save/Restore tree expansion state
    def save_settings(self, plugin_settings, instance_settings):
        header_state = self.topics_tree_widget.header().saveState()
        instance_settings.set_value('tree_widget_header_state', header_state)

    def restore_settings(self, pluggin_settings, instance_settings):
        if instance_settings.contains('tree_widget_header_state'):
            header_state = instance_settings.value('tree_widget_header_state')
            if not self.topics_tree_widget.header().restoreState(header_state):
                rospy.logwarn("rqt_topic: Failed to restore header state.")
Beispiel #56
0
    def __init__(self, context):
        super(Smach, self).__init__(context)

        self.initialized = 0

        self._dotcode_sub = None
        self._topic_dict = {}
        self._update_thread = WorkerThread(self._update_thread_run,
                                           self._update_finished)

        # Give QObjects reasonable names
        self.setObjectName('Smach')

        # 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 is a sibling of this file
        # in this example the .ui and .py file are in the same folder
        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
                               'rqt_smach.ui')

        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)

        # Give QObjects reasonable names
        self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
        self._widget.setObjectName('SmachPluginUi')

        # 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 widget to the user interface
        context.add_widget(self._widget)
        palette = QPalette()
        palette.setColor(QPalette.Background, Qt.white)
        self._widget.setPalette(palette)

        #Connect widgets with corresponding methods
        self._widget.namespace_input.currentIndexChanged.connect(
            self._handle_ns_changed)
        self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
        self._widget.restrict_ns.clicked.connect(self.refresh_combo_box)
        self._widget.ud_path_input.currentIndexChanged.connect(
            self._handle_ud_path)
        self._widget.ud_set_initial.clicked.connect(self._handle_ud_set_path)
        self._widget.ud_text_browser.setReadOnly(1)
        self._widget.show_implicit_button.clicked.connect(
            self._handle_show_implicit)
        self._widget.help_button.setIcon(QIcon.fromTheme('help-contents'))
        self._widget.help_button.clicked.connect(self._handle_help)
        self._widget.tree.clicked.connect(self._handle_tree_clicked)

        #Depth and width spinners:
        self._widget.depth_input.setRange(-1, 1337)
        self._widget.depth_input.setValue(-1)
        self._widget.depth_input.valueChanged.connect(self._set_depth)

        self._widget.label_width_input.setRange(1, 1337)
        self._widget.label_width_input.setValue(40)
        self._widget.label_width_input.valueChanged.connect(self._set_width)

        self._widget.tree.setColumnCount(1)
        self._widget.tree.setHeaderLabels(["Containers"])
        self._widget.tree.show()

        self._ns = ""
        self.refresh_combo_box()

        # Bind path list
        self._widget.path_input.currentIndexChanged.connect(
            self._handle_path_changed)

        #Keep Combo Boxes sorted
        self._widget.namespace_input.setInsertPolicy(6)
        self._widget.path_input.setInsertPolicy(6)
        self._widget.ud_path_input.setInsertPolicy(6)

        #Set up mouse actions for xdot widget
        self._widget.xdot_widget.register_select_callback(self.select_cb)

        # Create graph data structures
        # Containers is a map of (container path) -> container proxy
        self._containers = {}
        self._top_containers = {}

        # smach introspection client
        self._client = smach_ros.IntrospectionClient()
        self._selected_paths = []

        # Message subscribers
        self._structure_subs = {}
        self._status_subs = {}

        # Initialize xdot display state
        self._path = '/'
        self._needs_zoom = True
        self._structure_changed = True
        self._max_depth = -1
        self._show_all_transitions = False
        self._label_wrapper = textwrap.TextWrapper(40, break_long_words=True)
        self._graph_needs_refresh = True
        self._tree_needs_refresh = True

        self._keep_running = True

        self._update_server_list()
        self._update_graph()
        self._update_tree()

        # Start a timer to update the server list
        self._server_timer = QTimer(self)
        self._server_timer.timeout.connect(self._update_server_list)
        self._server_timer.start(1000)

        # Start a timer to update the graph display
        self._graph_timer = QTimer(self)
        self._graph_timer.timeout.connect(self._update_graph)
        self._graph_timer.start(1093)

        # Start a timer to update the._widget.tree display
        self._tree_timer = QTimer(self)
        self._tree_timer.timeout.connect(self._update_tree)
        self._tree_timer.start(1217)
Beispiel #57
0
class StringLabelWidget(QWidget):
    def __init__(self):
        super(StringLabelWidget, self).__init__()
        self.lock = Lock()
        vbox = QtGui.QVBoxLayout(self)
        self.label = QLabel()
        self.label.setAlignment(Qt.AlignLeft)
        self.label.setSizePolicy(
            QSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored))
        font = QFont("Helvetica", 14)
        self.label.setFont(font)
        self.label.setWordWrap(True)
        vbox.addWidget(self.label)
        self.string_sub = None
        self._string_topics = []
        self._update_topic_timer = QTimer(self)
        self._update_topic_timer.timeout.connect(self.updateTopics)
        self._update_topic_timer.start(1)
        self._active_topic = None
        self._dialog = ComboBoxDialog()

    def trigger_configuration(self):
        self._dialog.exec_()
        self.setupSubscriber(self._string_topics[self._dialog.number])

    def updateTopics(self):
        need_to_update = False
        for (topic, topic_type) in rospy.get_published_topics():
            if topic_type == "std_msgs/String":
                if not topic in self._string_topics:
                    self._string_topics.append(topic)
                    need_to_update = True
        if need_to_update:
            self._string_topics = sorted(self._string_topics)
            self._dialog.combo_box.clear()
            for topic in self._string_topics:
                self._dialog.combo_box.addItem(topic)
            if self._active_topic:
                if self._active_topic not in self._string_topics:
                    self._string_topics.append(self._active_topic)
                    self._dialog.combo_box.addItem(self._active_topic)
                self._dialog.combo_box.setCurrentIndex(
                    self._string_topics.index(self._active_topic))

    def setupSubscriber(self, topic):
        if self.string_sub:
            self.string_sub.unregister()
        self.string_sub = rospy.Subscriber(topic, String, self.stringCallback)
        self._active_topic = topic

    def onActivated(self, number):
        self.setupSubscriber(self._string_topics[number])

    def stringCallback(self, msg):
        self.string = msg.data
        self.label.setText(self.string)

    def save_settings(self, plugin_settings, instance_settings):
        if self._active_topic:
            instance_settings.set_value("active_topic", self._active_topic)

    def restore_settings(self, plugin_settings, instance_settings):
        if instance_settings.value("active_topic"):
            topic = instance_settings.value("active_topic")
            self._dialog.combo_box.addItem(topic)
            self.setupSubscriber(topic)
Beispiel #58
0
class JointTrajectoryController(Plugin):
    """
    Graphical frontend for a C{JointTrajectoryController}.

    There are two modes for interacting with a controller:
        1. B{Monitor mode} Joint displays are updated with the state reported
          by the controller. This is a read-only mode and does I{not} send
          control commands. Every time a new controller is selected, it starts
          in monitor mode for safety reasons.

        2. B{Control mode} Joint displays update the control command that is
        sent to the controller. Commands are sent periodically evan if the
        displays are not being updated by the user.

    To control the aggressiveness of the motions, the maximum speed of the
    sent commands can be scaled down using the C{Speed scaling control}

    This plugin is able to detect and keep track of all active controller
    managers, as well as the JointTrajectoryControllers that are I{running}
    in each controller manager.
    For a controller to be compatible with this plugin, it must comply with
    the following requisites:
        - The controller type contains the C{JointTrajectoryController}
        substring, e.g., C{position_controllers/JointTrajectoryController}
        - The controller exposes the C{command} and C{state} topics in its
        ROS interface.

    Additionally, there must be a URDF loaded with a valid joint limit
    specification, namely position (when applicable) and velocity limits.

    A reference implementation of the C{JointTrajectoryController} is
    available in the C{joint_trajectory_controller} ROS package.
    """

    _cmd_pub_freq = 10.0  # Hz
    _widget_update_freq = 30.0  # Hz
    _ctrlrs_update_freq = 1  # Hz
    _min_traj_dur = 5.0 / _cmd_pub_freq  # Minimum trajectory duration

    jointStateChanged = Signal([dict])

    def __init__(self, context):
        super(JointTrajectoryController, self).__init__(context)
        self.setObjectName('JointTrajectoryController')

        self._node = context.node

        # Get the robot_description via a topic
        qos_profile = QoSProfile(depth=1,
                                 history=HistoryPolicy.KEEP_LAST,
                                 durability=DurabilityPolicy.TRANSIENT_LOCAL)
        self._robot_description_sub = self._node.create_subscription(
            String, 'robot_description',
            self._robot_description_cb, qos_profile)
        self._robot_description = None

        self._publisher = None
        self._widget = QWidget()

        _, package_path = get_resource('packages', 'rqt_joint_trajectory_controller')
        ui_file = os.path.join(package_path, 'share', 'rqt_joint_trajectory_controller', 'resource', 'joint_trajectory_controller.ui')
        loadUi(ui_file, self._widget)

        self._widget.setObjectName('JointTrajectoryControllerUi')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(
                self._widget.windowTitle() + (' (%d)' % context.serial_number()))
        context.add_widget(self._widget)

        ns = self._node.get_namespace()
        self._widget.controller_group.setTitle('ns: ' + ns)

        # Setup speed scaler
        speed_scaling = DoubleEditor(1.0, 100.0)
        speed_scaling.spin_box.setSuffix('%')
        speed_scaling.spin_box.setValue(50.0)
        speed_scaling.spin_box.setDecimals(0)
        speed_scaling.setEnabled(False)
        self._widget.speed_scaling_layout.addWidget(speed_scaling)
        self._speed_scaling_widget = speed_scaling
        speed_scaling.valueChanged.connect(self._on_speed_scaling_change)
        self._on_speed_scaling_change(speed_scaling.value())

        # 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 widget to the user interface
        context.add_widget(self._widget)

        # Initialize members
        self._jtc_name = []  # Name of selected joint trajectory controller
        self._cm_ns = []  # Namespace of the selected controller manager
        self._joint_pos = {}  # name->pos map for joints of selected controller
        self._joint_names = []  # Ordered list of selected controller joints
        self._robot_joint_limits = {} # Lazily evaluated on first use

        # Timer for sending commands to active controller
        self._update_cmd_timer = QTimer(self)
        self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq)
        self._update_cmd_timer.timeout.connect(self._update_cmd_cb)

        # Timer for updating the joint widgets from the controller state
        self._update_act_pos_timer = QTimer(self)
        self._update_act_pos_timer.setInterval(1000.0 /
                                               self._widget_update_freq)
        self._update_act_pos_timer.timeout.connect(self._update_joint_widgets)

        # Timer for controller manager updates
        # TODO: self._list_cm = ControllerManagerLister()
        self._update_cm_list_timer = QTimer(self)
        self._update_cm_list_timer.setInterval(1000.0 /
                                               self._ctrlrs_update_freq)
        self._update_cm_list_timer.timeout.connect(self._update_cm_list)
        self._update_cm_list_timer.start()

        # Timer for running controller updates
        self._update_jtc_list_timer = QTimer(self)
        self._update_jtc_list_timer.setInterval(1000.0 /
                                                self._ctrlrs_update_freq)
        self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
        self._update_jtc_list_timer.start()

        # Signal connections
        w = self._widget
        w.enable_button.toggled.connect(self._on_jtc_enabled)
        w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
        w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)

        self._cmd_pub = None    # Controller command publisher
        self._state_sub = None  # Controller state subscriber

        self._list_controllers = None

        self._traj_ns_topic_dict = None

    def shutdown_plugin(self):
        self._update_cmd_timer.stop()
        self._update_act_pos_timer.stop()
        self._update_cm_list_timer.stop()
        self._update_jtc_list_timer.stop()
        self._unregister_state_sub()
        self._unregister_cmd_pub()

    def save_settings(self, plugin_settings, instance_settings):
        instance_settings.set_value('cm_ns', self._cm_ns)
        instance_settings.set_value('jtc_name', self._jtc_name)

    def restore_settings(self, plugin_settings, instance_settings):
        # Restore last session's controller_manager, if present
        self._update_cm_list()
        cm_ns = instance_settings.value('cm_ns')
        cm_combo = self._widget.cm_combo
        cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
        try:
            idx = cm_list.index(cm_ns)
            cm_combo.setCurrentIndex(idx)
            # Resore last session's controller, if running
            self._update_jtc_list()
            jtc_name = instance_settings.value('jtc_name')
            jtc_combo = self._widget.jtc_combo
            jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())]
            try:
                idx = jtc_list.index(jtc_name)
                jtc_combo.setCurrentIndex(idx)
            except (ValueError):
                pass
        except (ValueError):
            pass

    # def trigger_configuration(self):
        # Comment in to signal that the plugin has a way to configure
        # This will enable a setting button (gear icon) in each dock widget
        # title bar
        # Usually used to open a modal configuration dialog

    def _update_cm_list(self):
        trajectory_topics = _search_for_trajectory_topics(self._node)

        ## TODO: remove test code
        #trajectory_topics.append('/my_test/controller')
        #trajectory_topics.append('/no_namespace')
        #trajectory_topics.append('no_root')

        self._traj_ns_topic_dict = {}
        for full_topic in trajectory_topics:
            ns, topic = _split_namespace_from_topic(full_topic)
            self._traj_ns_topic_dict.setdefault(ns, []).append(topic)

        update_combo(self._widget.cm_combo, list(self._traj_ns_topic_dict.keys()))

    def _update_jtc_list(self):
        # Clear controller list if no controller information is available
        if self._traj_ns_topic_dict is None:
            self._widget.jtc_combo.clear()
            return

        #print(get_joint_limits(self._robot_description))

        ## List of running controllers with a valid joint limits specification
        ## for _all_ their joints
        #running_jtc = self._running_jtc_info()
        #if running_jtc and not self._robot_joint_limits:
        #    self._robot_joint_limits = get_joint_limits()  # Lazy evaluation
        #valid_jtc = []
        #for jtc_info in running_jtc:
        #    has_limits = all(name in self._robot_joint_limits
        #                     for name in _jtc_joint_names(jtc_info))
        #    if has_limits:
        #        valid_jtc.append(jtc_info);
        #valid_jtc_names = [data.name for data in valid_jtc]

        # Get the currently selected namespace
        curr_ns = self._widget.cm_combo.currentText()
        topics = self._traj_ns_topic_dict[curr_ns]
        update_combo(self._widget.jtc_combo, topics)

    def _on_speed_scaling_change(self, val):
        self._speed_scale = val / self._speed_scaling_widget.slider.maximum()

    def _on_joint_state_change(self, actual_pos):
        assert(len(actual_pos) == len(self._joint_pos))
        for name in actual_pos.keys():
            self._joint_pos[name]['position'] = actual_pos[name]

    def _on_cm_change(self, cm_ns):
        self._cm_ns = cm_ns
        if cm_ns:
            self._widget.jtc_combo.clear()
            self._update_jtc_list()
        else:
            self._list_controllers = None

    def _on_jtc_change(self, jtc_name):
        self._unload_jtc()
        self._jtc_name = jtc_name
        if self._jtc_name:
            self._load_jtc()

    def _on_jtc_enabled(self, val):
        # Don't allow enabling if there are no controllers selected
        if not self._jtc_name:
            self._widget.enable_button.setChecked(False)
            return

        # Enable/disable joint displays
        for joint_widget in self._joint_widgets():
            joint_widget.setEnabled(val)

        # Enable/disable speed scaling
        self._speed_scaling_widget.setEnabled(val)

        if val:
            # Widgets send desired position commands to controller
            self._update_act_pos_timer.stop()
            self._update_cmd_timer.start()
        else:
            # Controller updates widgets with actual position
            self._update_cmd_timer.stop()
            self._update_act_pos_timer.start()

    def _load_jtc(self):
        self._robot_joint_limits = get_joint_limits(self._robot_description)
        self._joint_names = list(self._robot_joint_limits.keys())
        self._joint_pos = { name: {} for name in self._joint_names }

        # Update joint display
        try:
            layout = self._widget.joint_group.layout()
            for name in self._joint_names:
                limits = self._robot_joint_limits[name]
                joint_widget = DoubleEditor(limits['min_position'],
                                            limits['max_position'])
                layout.addRow(name, joint_widget)
                # NOTE: Using partial instead of a lambda because lambdas
                # "will not evaluate/look up the argument values before it is
                # effectively called, breaking situations like using a loop
                # variable inside it"
                from functools import partial
                par = partial(self._update_single_cmd_cb, name=name)
                joint_widget.valueChanged.connect(par)
        except:
            # TODO: Can we do better than swallow the exception?
            from sys import exc_info
            print('Unexpected error:', exc_info()[0])

        # Enter monitor mode (sending commands disabled)
        self._on_jtc_enabled(False)

        # Setup ROS interfaces
        jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name)
        #state_topic = jtc_ns + '/state'
        state_topic = 'state'
        #cmd_topic = jtc_ns + '/command'
        cmd_topic = '/joint_trajectory_controller/joint_trajectory'
        #self._state_sub = rospy.Subscriber(state_topic,
        #                                   JointTrajectoryControllerState,
        #                                   self._state_cb,
        #                                   queue_size=1)

        qos_profile = QoSProfile(depth=1,
                                 history=HistoryPolicy.KEEP_LAST,
                                 durability=DurabilityPolicy.TRANSIENT_LOCAL)

        self._state_sub = self._node.create_subscription(
            JointTrajectoryControllerState, state_topic, self._state_cb,
            qos_profile)

        self._cmd_pub = self._node.create_publisher(JointTrajectory, cmd_topic, 1)

        # Start updating the joint positions
        self.jointStateChanged.connect(self._on_joint_state_change)

    def _unload_jtc(self):
        # Stop updating the joint positions
        try:
            self.jointStateChanged.disconnect(self._on_joint_state_change)
        except:
            pass

        # Reset ROS interfaces
        self._unregister_state_sub()
        self._unregister_cmd_pub()

        # Clear joint widgets
        # NOTE: Implementation is a workaround for:
        # https://bugreports.qt-project.org/browse/QTBUG-15990 :(

        layout = self._widget.joint_group.layout()
        if layout is not None:
            while layout.count():
                layout.takeAt(0).widget().deleteLater()
            # Delete existing layout by reparenting to temporary
            QWidget().setLayout(layout)
        self._widget.joint_group.setLayout(QFormLayout())

        # Reset joint data
        self._joint_names = []
        self._joint_pos = {}

        # Enforce monitor mode (sending commands disabled)
        self._widget.enable_button.setChecked(False)

    def _running_jtc_info(self):
        from controller_manager_msgs.utils\
            import filter_by_type, filter_by_state

        controller_list = self._list_controllers()
        jtc_list = filter_by_type(controller_list,
                                  'JointTrajectoryController',
                                  match_substring=True)
        running_jtc_list = filter_by_state(jtc_list, 'running')
        return running_jtc_list

    def _unregister_cmd_pub(self):
        if self._cmd_pub is not None:
            self._node.destroy_publisher(self._cmd_pub)
            self._cmd_pub = None

    def _unregister_state_sub(self):
        if self._state_sub is not None:
            self._node.destroy_subscription(self._state_sub)
            self._state_sub = None

    def _robot_description_cb(self, msg):
        self._robot_description = msg.data

    def _state_cb(self, msg):
        actual_pos = {}
        for i in range(len(msg.joint_names)):
            joint_name = msg.joint_names[i]
            joint_pos = msg.actual.positions[i]
            actual_pos[joint_name] = joint_pos
        self.jointStateChanged.emit(actual_pos)

    def _update_single_cmd_cb(self, val, name):
        self._joint_pos[name]['command'] = val

    def _update_cmd_cb(self):
        dur = []
        traj = JointTrajectory()
        traj.joint_names = self._joint_names
        point = JointTrajectoryPoint()
        for name in traj.joint_names:
            pos = self._joint_pos[name]['position']
            cmd = pos
            try:
                cmd = self._joint_pos[name]['command']
            except (KeyError):
                pass
            max_vel = self._robot_joint_limits[name]['max_velocity']
            dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur))
            point.positions.append(cmd)

        max_duration_scaled = max(dur) / self._speed_scale
        seconds = floor(max_duration_scaled)
        nanoseconds = (max_duration_scaled - seconds) * 1e9
        duration = Duration(seconds=seconds, nanoseconds=nanoseconds)
        point.time_from_start = duration.to_msg()
        traj.points.append(point)

        self._cmd_pub.publish(traj)

    def _update_joint_widgets(self):
        joint_widgets = self._joint_widgets()
        for id in range(len(joint_widgets)):
            joint_name = self._joint_names[id]
            try:
                joint_pos = self._joint_pos[joint_name]['position']
                joint_widgets[id].setValue(joint_pos)
            except (KeyError):
                pass  # Can happen when first connected to controller

    def _joint_widgets(self):  # TODO: Cache instead of compute every time?
        widgets = []
        layout = self._widget.joint_group.layout()
        for row_id in range(layout.rowCount()):
            widgets.append(layout.itemAt(row_id,
                                         QFormLayout.FieldRole).widget())
        return widgets
Beispiel #59
0
class LifeFrame(QFrame):
    STATE_STOPPED = "stopped"
    STATE_RUN = "running"
    STATE_IDLE = "idle"

    def __init__(self, parent=None):
        super(LifeFrame, self).__init__(parent)
        self._ui = Ui_life_frame()
        self._motion = Rotate('/mobile_base/commands/velocity')
        self._motion_thread = None
        self._timer = QTimer()
        #self._timer.setInterval(60000) #60s
        self._timer.setInterval(250)  #60s
        QObject.connect(self._timer, SIGNAL('timeout()'), self,
                        SLOT('update_progress_callback()'))
        self._state = LifeFrame.STATE_STOPPED
        self._is_alive = False  # Used to indicate whether the frame is alive or not (see hibernate/restore methods)

    def setupUi(self):
        self._ui.setupUi(self)
        self._ui.start_button.setEnabled(True)
        self._ui.stop_button.setEnabled(False)
        self._motion.init(self._ui.angular_speed_spinbox.value())

    def shutdown(self):
        '''
          Used to terminate the plugin
        '''
        rospy.loginfo("Kobuki TestSuite: life frame shutdown")
        self._motion.shutdown()

    ##########################################################################
    # Widget Management
    ##########################################################################

    def hibernate(self):
        '''
          This gets called when the frame goes out of focus (tab switch). 
          Disable everything to avoid running N tabs in parallel when in
          reality we are only running one.
        '''
        pass

    def restore(self):
        '''
          Restore the frame after a hibernate.
        '''
        pass

    ##########################################################################
    # Motion Callbacks
    ##########################################################################

    def start(self):
        self._state = LifeFrame.STATE_RUN
        self._ui.run_progress.reset()
        self._ui.idle_progress.reset()
        self._motion_thread = WorkerThread(self._motion.execute, None)
        self._motion_thread.start()

    def stop(self):
        self._state = LifeFrame.STATE_STOPPED
        self._motion.stop()
        if self._motion_thread:
            self._motion_thread.wait()

    ##########################################################################
    # Qt Callbacks
    ##########################################################################
    @Slot()
    def on_start_button_clicked(self):
        self._ui.start_button.setEnabled(False)
        self._ui.stop_button.setEnabled(True)
        self._timer.start()
        self.start()

    @Slot()
    def on_stop_button_clicked(self):
        self.stop()
        self._timer.stop()
        self._ui.start_button.setEnabled(True)
        self._ui.stop_button.setEnabled(False)

    @pyqtSlot(float)
    def on_angular_speed_spinbox_valueChanged(self, value):
        self._motion.init(self._ui.angular_speed_spinbox.value())

    ##########################################################################
    # Timer Callbacks
    ##########################################################################

    @Slot()
    def update_progress_callback(self):
        if self._state == LifeFrame.STATE_RUN:
            new_value = self._ui.run_progress.value() + 1
            if new_value == self._ui.run_progress.maximum():
                print("  Switching to idle")
                self._motion.stop()
                self._state = LifeFrame.STATE_IDLE
            else:
                self._ui.run_progress.setValue(new_value)
        if self._state == LifeFrame.STATE_IDLE:
            new_value = self._ui.idle_progress.value() + 1
            if new_value == self._ui.idle_progress.maximum():
                print("  Switching to run")
                self.start()
            else:
                self._ui.idle_progress.setValue(new_value)
Beispiel #60
0
class Console(Plugin):
    """
    rqt_console plugin's main class. Handles communication with ros_gui and contains
    callbacks to handle incoming message
    """
    def __init__(self, context):
        """
        :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext''
        """
        super(Console, self).__init__(context)
        self.setObjectName('Console')

        self._datamodel = MessageDataModel()
        self._proxymodel = MessageProxyModel()
        self._proxymodel.setSourceModel(self._datamodel)

        self._mainwindow = ConsoleWidget(self._proxymodel)
        if context.serial_number() > 1:
            self._mainwindow.setWindowTitle(self._mainwindow.windowTitle() +
                                            (' (%d)' %
                                             context.serial_number()))
        context.add_widget(self._mainwindow)

        self._consolesubscriber = ConsoleSubscriber(self.message_callback)

        # Timer and Mutex for flushing recieved messages to datamodel.
        # Required since QSortProxyModel can not handle a high insert rate
        self._mutex = QMutex()
        self._timer = QTimer()
        self._timer.timeout.connect(self.insert_messages)
        self._timer.start(100)

    def insert_messages(self):
        """
        Callback for flushing incoming Log messages from the queue to the datamodel
        """
        with QMutexLocker(self._mutex):
            msgs = self._datamodel._insert_message_queue
            self._datamodel._insert_message_queue = []
        self._datamodel.insert_rows(msgs)
        self._mainwindow.update_status()

    def message_callback(self, msg):
        """
        Callback for adding an incomming message to the queue
        """
        if not self._datamodel._paused:
            with QMutexLocker(self._mutex):
                self._datamodel._insert_message_queue.append(msg)

    def shutdown_plugin(self):
        self._consolesubscriber.unsubscribe_topic()
        self._timer.stop()
        self._mainwindow.cleanup_browsers_on_close()

    def save_settings(self, plugin_settings, instance_settings):
        self._mainwindow.save_settings(plugin_settings, instance_settings)

    def restore_settings(self, plugin_settings, instance_settings):
        self._mainwindow.restore_settings(plugin_settings, instance_settings)

    def trigger_configuration(self):
        self._consolesubscriber.set_message_limit(
            self._datamodel._message_limit)
        if self._consolesubscriber.show_dialog():
            self._datamodel.set_message_limit(
                self._consolesubscriber.get_message_limit())