def monitor_status(self, feedback): '''Signal from callback calls this function to do the work to avoid threading issued with the GUI, changes the status on the sent plans table to match the current plan status. Plan can be Pending, Running, Finished or Failed.''' num_rows = self._widget.sentPlansTable.rowCount() #if completed and previously running we set status as finished if feedback == "COMPLETE": for i in range(num_rows): current_status = self._widget.sentPlansTable.item(i,1).text() if current_status == "Running...": self._widget.sentPlansTable.item(i, 1).setText("Finished") self._widget.sentPlansTable.item(i, 1).setBackground(QColor(0,128,0)) break else: #splitting into plan name and status status = feedback.rsplit(":")[0] running_plan = feedback.rsplit(":")[1].rsplit(".")[0] #we set status to running or Failed depending on status for i in range(num_rows): plan_name = self._widget.sentPlansTable.item(i,0).text() current_status = self._widget.sentPlansTable.item(i,1).text() if plan_name == running_plan and current_status == "Pending...": if status == "SUCCESS": self._widget.sentPlansTable.item(i, 1).setText("Running...") break else: self._widget.sentPlansTable.item(i, 1).setText("Failed") self._widget.sentPlansTable.item(i, 1).setBackground(QColor(230,38,0)) break return
def add_curve(self, curve_id, curve_name, curve_color=QColor(Qt.blue), markers_on=False, dashed=False): pen = mkPen(curve_color, width=1) symbol = "o" symbolPen = mkPen(QColor(Qt.black)) if dashed: symbolBrush = mkBrush(curve_color, style=Qt.DashLine) else: symbolBrush = mkBrush(curve_color) # this adds the item to the plot and legend if markers_on: plot = self._plot_widget.plot(name=curve_name, pen=pen, symbol=symbol, symbolPen=symbolPen, symbolBrush=symbolBrush, symbolSize=4) else: plot = self._plot_widget.plot(name=curve_name, pen=pen) self._curves[curve_id] = plot
def _draw_joy_target(self, painter): # JoyStickのControlTargetを描画する # 経路の線を描画するための変数 prev_point = None for i, pose in enumerate(self._joy_target.path): point = self._convert_to_view(pose.x, pose.y) size = self._ROBOT_RADIUS * self._scale_field_to_view joy_color = QColor(Qt.magenta) painter.setPen(Qt.black) painter.setBrush(joy_color) painter.drawEllipse(point, size, size) # 角度 line_x = self._ROBOT_RADIUS * math.cos(pose.theta) line_y = self._ROBOT_RADIUS * math.sin(pose.theta) line_point = point + self._convert_to_view(line_x, line_y) painter.drawLine(point, line_point) # インデックス text_point = point + self._convert_to_view(self._ID_POS[0], self._ID_POS[1]) painter.drawText(text_point, str(i)) # 経路を描画 if prev_point is None: prev_point = point else: painter.setPen(QPen(QColor(0, 0, 255, 127), 4)) painter.drawLine(prev_point, point) prev_point = point
def add_curve(self, curve_id, curve_name, x, y): color = QColor(self._colors[self._color_index % len(self._colors)]) self._color_index += 1 line = self._canvas.axes.plot([], [], label=curve_name, linewidth=1, picker=5, color=color.name())[0] self._curves[curve_id] = ([], [], line, [None, None]) self.update_values(curve_id, x, y) self._update_legend()
def __init__(self, parent=None, info=False): QFrame.__init__(self, parent=parent) self.setObjectName('MessageFrame') self.questionid = self.TYPE_INVALID self.text = "" self.data = MessageData(None) self.IMAGES = {1: QPixmap(), 2: QPixmap(':/icons/crystal_clear_question.png').scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation), 3: QPixmap(':/icons/crystal_clear_launch_file.png').scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation), 4: QPixmap(":/icons/default_cfg.png").scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation), 5: QPixmap(":/icons/crystal_clear_nodelet_q.png").scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation), 6: QPixmap(":/icons/crystal_clear_launch_file_transfer.png").scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation), 7: QPixmap(":/icons/crystal_clear_question.png").scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation), 8: QPixmap(":/icons/crystal_clear_no_io.png").scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation) } self._new_request = False self.frameui = QFrame() ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'MessageFrame.ui') loadUi(ui_file, self.frameui) color = QColor(255, 207, 121) self.frameui.setVisible(False) self.frameui.listLabel.setVisible(False) self.frameui.questionOkButton.clicked.connect(self._on_question_ok) self.frameui.questionCancelButton.clicked.connect(self._on_question_cancel) self.frameui.checkBox_dnaa.stateChanged.connect(self._on_checkbox_state_changed) self._ask = 'ask' if info: color = QColor(232, 104, 80) self.frameui.questionCancelButton.setVisible(False) self._ask = 'show' bg_style = "QFrame#questionFame { background-color: %s;}" % color.name() self.frameui.setStyleSheet("%s" % (bg_style)) self._queue = MessageQueue() self._do_not_ask = {}
def setNodeState(self, running_nodes, stopped_nodes, error_nodes): ''' Sets the state of this capability. :param running_nodes: a list with running nodes. :type running_nodes: [str] :param stopped_nodes: a list with not running nodes. :type stopped_nodes: [str] :param error_nodes: a list with nodes having a problem. :type error_nodes: [str] ''' self.setAutoFillBackground(True) self.setBackgroundRole(QPalette.Base) palette = QPalette() if error_nodes: brush = QBrush(QColor(255, 100, 0)) elif running_nodes and stopped_nodes: brush = QBrush(QColor(140, 185, 255)) # 30, 50, 255 elif running_nodes: self.on_button.setFlat(True) self.off_button.setFlat(False) brush = QBrush(QColor(59, 223, 18)) # 59, 223, 18 else: brush = QBrush(QColor(255, 255, 255)) self.on_button.setFlat(False) self.off_button.setFlat(True) palette.setBrush(QPalette.Active, QPalette.Base, brush) brush.setStyle(Qt.SolidPattern) palette.setBrush(QPalette.Inactive, QPalette.Base, brush) self.setPalette(palette)
def _create_log_bar(self): self.log_dock = QDockWidget(self) self.log_dock.setObjectName('LogFrame') self.log_dock.setFeatures(QDockWidget.DockWidgetMovable | QDockWidget.DockWidgetFloatable) self.log_bar = QWidget(self) self.horizontal_layout_log_bar = QHBoxLayout(self.log_bar) self.horizontal_layout_log_bar.setContentsMargins(2, 0, 2, 0) self.horizontal_layout_log_bar.setObjectName("horizontal_layout_log_bar") # add info label self._log_warning_count = 0 self.log_browser = QTextEdit() self.log_browser.setObjectName("log_browser") self.log_browser.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) self.log_browser.setLineWrapMode(QTextEdit.NoWrap) # self.log_browser.setMaximumHeight(120) color = QColor(255, 255, 235) bg_style = "QTextEdit#log_browser { background-color: %s;}" % color.name() self.log_bar.setStyleSheet("%s" % (bg_style)) self.horizontal_layout_log_bar.addWidget(self.log_browser) # add hide button self.clear_log_button = QPushButton("clear", self) self.clear_log_button.setObjectName("clear_log_button") self.clear_log_button.setSizePolicy(QSizePolicy.Minimum, QSizePolicy.Maximum) self.clear_log_button.clicked.connect(self.on_clear_log_button_clicked) self.clear_log_button.setFlat(True) self.horizontal_layout_log_bar.addWidget(self.clear_log_button) self.log_dock.setWidget(self.log_bar) return self.log_dock
def _update_kill_entry(self, row, kill): color = QColor(255, 255, 255) if not kill.active else QColor( 255, 200, 200) self._update_item(row, 0, color, kill.id) self._update_item(row, 1, color, "Killed" if kill.active else "Unkilled") self._update_item(row, 2, color, kill.description)
def __init__(self, parent=None, info=False): QFrame.__init__(self, parent=parent) self.setObjectName('MessageFrame') self.questionid = self.TYPE_INVALID self.text = "" self.data = MessageData(None) self.IMAGES = {1: QPixmap(), 2: QPixmap(':/icons/crystal_clear_question.png').scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation), 3: QPixmap(':/icons/crystal_clear_launch_file.png').scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation), 4: QPixmap(":/icons/default_cfg.png").scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation), 5: QPixmap(":/icons/crystal_clear_nodelet_q.png").scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation), 6: QPixmap(":/icons/crystal_clear_question.png").scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation), 7: QPixmap(":/icons/crystal_clear_launch_file_transfer.png").scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation), 8: QPixmap(":/icons/crystal_clear_no_io.png").scaled(self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio, Qt.SmoothTransformation) } self._new_request = False self.frameui = QFrame() ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'MessageFrame.ui') loadUi(ui_file, self.frameui) color = QColor(255, 207, 121) self.frameui.setVisible(False) self.frameui.scrollArea.setVisible(False) self.frameui.questionOkButton.clicked.connect(self._on_question_ok) self.frameui.questionCancelButton.clicked.connect(self._on_question_cancel) self.frameui.checkBox_dnaa.stateChanged.connect(self._on_checkbox_state_changed) self._ask = 'ask' if info: color = QColor(232, 104, 80) self.frameui.questionCancelButton.setVisible(False) self._ask = 'show' bg_style = "QFrame#questionFame { background-color: %s;}" % color.name() self.frameui.setStyleSheet("%s" % (bg_style)) self._queue = MessageQueue() self._do_not_ask = {}
def _draw_annotation(self, data): old_item = None if self.annotation_item: old_item = self.annotation_item annotation_item = None if not data == {}: resolution = self._drawing_objects['map'].info.resolution origin = self._drawing_objects['map'].info.origin w = self._drawing_objects['map'].info.width h = self._drawing_objects['map'].info.height x = -((data['x'] - origin.position.x) / resolution - w) y = (data['y'] - origin.position.y) / resolution if data['type'] == "yocs_msgs/Waypoint": #annotation_item = self._scene.addEllipse(x - (data['scale'][0] / resolution), y -(data['scale'][1] / resolution), data['scale'][0] / resolution * 2, data['scale'][1] / resolution * 2, pen=QPen(QColor(0, 0, 255)), brush=QBrush(QColor(0, 0, 255, 125))) viz_marker_pose = QMatrix().rotate(-data['yaw'] + 180).map( self._viz_marker_polygon).translated(x, y) annotation_item = self._scene.addPolygon( viz_marker_pose, pen=QPen(QColor(0, 0, 255)), brush=QBrush(QColor(0, 0, 255, 125))) elif data['type'] == "ar_track_alvar_msgs/AlvarMarker": viz_marker_pose = QMatrix().rotate( -(data['yaw'] - 90) + 180).map( self._viz_marker_polygon).translated(x, y) annotation_item = self._scene.addPolygon( viz_marker_pose, pen=QPen(QColor(0, 0, 255)), brush=QBrush(QColor(0, 0, 255, 125))) annotation_item.setZValue(2) if old_item: self._scene.removeItem(old_item) self.annotation_item = annotation_item
def paintEvent(self, ev): QPushButton.paintEvent(self, ev) self.drawRect() self.drawCircle(self.state, QColor(0, 0, 0), False) self.drawCircle(self.center, QColor(0, 0, 0)) self.drawCircle(self.current_pos, QColor(255, 0, 0)) self.drawCrosshair(self.state)
def __init__(self, ros_package): super(GraphWidget, self).__init__() self._current_graph = None self._lock = Lock() self._load_ui(ros_package) self._scene = QGraphicsScene() self._scene.setBackgroundBrush(Qt.white) factory = DmgItemFactory() factory.set_color(QColor(50, 50, 50)) factory.set_highlighted_color(QColor(0, 150, 0)) self._dot_to_qt = DotToQtGenerator(factory) self.initialized = False self.setObjectName('GraphWidget') self.graphics_view.setScene(self._scene) self.open_button.setIcon(QIcon.fromTheme('document-open')) self.open_button.pressed.connect(self._import) self.export_button.setIcon(QIcon.fromTheme('document-export')) self.export_button.pressed.connect(self._export) self.fit_to_view_button.setIcon(QIcon.fromTheme('zoom-fit-best')) self.fit_to_view_button.pressed.connect(self._fit_to_view) self.decision_graphs_combo_box.setSizeAdjustPolicy(QComboBox.AdjustToMinimumContentsLength) self.decision_graphs_combo_box.currentIndexChanged['QString'].connect(self._graph_item_changed) self._dot_processor = DotProcessor(self._dot_to_qt) self.decision_graphs = dict() self.states = dict()
def getComponentBrush(state): b = QBrush(QColor(0, 0, 255)) # unconfigured/preoperational if state == 'S': # stopped b = QBrush(QColor(255, 255, 255)) elif state == 'R': # running b = QBrush(QColor(0, 255, 0)) return b
def draw_position(self, e): p = self.mapToScene(e.x(), e.y()) v = (p.x() - self.drag_start[0], p.y() - self.drag_start[1]) mag = sqrt(pow(v[0], 2) + pow(v[1], 2)) v = (v[0] / mag, v[1] / mag) # Normalize diff vector u = (-v[1], v[0]) # Project diff vector to mirrored map if self.last_path: self._scene.removeItem(self.last_path) self.last_path = None res = (v[0] * 25, v[1] * 25) if self._pose_mode: pen = QPen(QColor("red")) elif self._goal_mode: pen = QPen(QColor("green")) self.last_path = self._scene.addLine(self.drag_start[0], self.drag_start[1], self.drag_start[0] + res[0], self.drag_start[1] + res[1], pen) map_p = self.point_qt_to_map(self.drag_start) angle = atan2(u[0], u[1]) quat = quaternion_from_euler(0, 0, angle) self.drag_start = None return map_p, quat
def getComponentPen(state): b = QPen(QColor(0, 0, 255)) # unconfigured/preoperational if state == 'A': # stopped b = QPen(QBrush(QColor(255, 0, 0)), 5) elif state == 'N': # running b = QPen(QBrush(QColor(0, 0, 0)), 1) return b
def _draw_control_target(self, painter, color, robot): # ロボットの制御目標値を描画する # 経路の線を描画するための変数 prev_point = None robot_id = robot.robot_id target = self._control_targets[color][robot_id] # 経路を描画 for pose in target.path: point = self._convert_to_view(pose.x, pose.y) size = self._ROBOT_RADIUS * self._scale_field_to_view target_color = QColor(Qt.magenta) target_color.setAlphaF(0.5) painter.setPen(Qt.black) painter.setBrush(target_color) painter.drawEllipse(point, size, size) # 角度 line_x = self._ROBOT_RADIUS * math.cos(pose.theta) line_y = self._ROBOT_RADIUS * math.sin(pose.theta) line_point = point + self._convert_to_view(line_x, line_y) painter.drawLine(point, line_point) # IDを添える text_point = point + self._convert_to_view(self._ID_POS[0], self._ID_POS[1]) painter.drawText(text_point, str(robot_id)) # 経路を描画 if prev_point is None: prev_point = point else: painter.setPen(QPen(QColor(0, 0, 255, 127), 4)) painter.drawLine(prev_point, point) prev_point = point # キック・ドリブルフラグを、ロボットの現在位置周辺に描画 text_flag = "" if target.kick_power > 0.0: text_flag += "K:" + str(target.kick_power) if target.chip_enable is True: text_flag += "\nC:ON" if target.dribble_power > 0.0: text_flag += "\nD:" + str(target.dribble_power) point = self._convert_to_view(robot.pose.x, robot.pose.y) text_point = point + self._convert_to_view(self._FLAG_POS[0], self._FLAG_POS[1]) # 複数行に分けてテキストを描画するため、widthとheightを設定する text_width = 50 text_height = 100 painter.setPen(QPen(Qt.red, 2)) painter.drawText(text_point.x(), text_point.y(), text_width, text_height, 0, text_flag)
def add_curve(self, curve_id, curve_name, data_x, data_y): data_x = collections.deque(data_x) data_y = collections.deque(data_y) color = QColor(self._colors[self._color_index % len(self._colors)]) self._color_index += 1 plot = self._canvas.axes.plot(data_x, data_y, label=curve_name, linewidth=1, picker=5, color=color.name())[0] self._curves[curve_id] = (data_x, data_y, plot) self._update_legend()
def __init__(self, scene, time, color=None): y2 = len(scene._tracks) * TimelineTrack.height super(TimelineMarker, self).__init__(time, - TimelineTrack.height, time, y2, None, scene) self.setZValue(self._z) if color is None: color = QColor(self._default_color) color.setAlphaF(0.5) self.setPen(QPen(QBrush(color), 0.05))
def __init__(self, context): super(LaunchtreeWidget, self).__init__() self._rp = rospkg.RosPack() self._rp_package_list = self._rp.list() res_folder = os.path.join(self._rp.get_path('rqt_launchtree'), 'resource') ui_file = os.path.join(res_folder, 'launchtree_widget.ui') loadUi(ui_file, self) self._block_load = True self.editor = 'gedit' # configure via settings self.setObjectName('LaunchtreeWidget') self.reload_button.setIcon(QIcon.fromTheme('view-refresh')) self._properties_empty_ui = os.path.join(res_folder, 'properties_empty.ui') self._properties_param_ui = os.path.join(res_folder, 'properties_param.ui') self._icon_include = QIcon(os.path.join(res_folder, 'img/include.png')) self._icon_node = QIcon(os.path.join(res_folder, 'img/node.png')) self._icon_param = QIcon(os.path.join(res_folder, 'img/param.png')) self._icon_arg = QIcon(os.path.join(res_folder, 'img/arg.png')) self._icon_remap = QIcon(os.path.join(res_folder, 'img/remap.png')) self._icon_rosparam = QIcon( os.path.join(res_folder, 'img/rosparam_load.png')) self._icon_default = QIcon(os.path.join(res_folder, 'img/default.png')) self._icon_warn = QIcon(os.path.join(res_folder, 'img/warn.png')) self._launch_separator = ' -- ' self._highlight_color = QColor(255, 255, 150) self._neutral_color = QColor(255, 255, 255, 0) # connect signals self.update_launch_view.connect(self._update_launch_view) self.display_load_error.connect(self._display_load_error) self.package_select.currentIndexChanged.connect( self.update_launchfiles) self.launchfile_select.currentIndexChanged.connect( lambda idx: self.load_launchfile()) self.reload_button.clicked.connect(self.load_launchfile) self.open_button.clicked.connect(self._root_open_clicked) self.launch_view.currentItemChanged.connect(self.launch_entry_changed) self.filter_nodes.toggled.connect(lambda t: self._filter_launch_view()) self.filter_params.toggled.connect( lambda t: self._filter_launch_view()) self.filter_args.toggled.connect(lambda t: self._filter_launch_view()) self.filter_remaps.toggled.connect( lambda t: self._filter_launch_view()) self.filter_empty.toggled.connect(lambda t: self._filter_launch_view()) self.search_input.textChanged.connect( lambda t: self._filter_launch_view(collapse=t == '')) self.launch_open_button.clicked.connect(self._launch_open_clicked) self.reset()
def __init__(self, parent=None): QSyntaxHighlighter.__init__(self, parent) self.rules = [] self.commentStart = QRegExp("#") self.commentEnd = QRegExp("\n|\r") self.default_format = QTextCharFormat() self.default_format.setForeground(QColor(24, 24, 24)) self.commentFormat = QTextCharFormat() self.commentFormat.setFontItalic(True) self.commentFormat.setForeground(Qt.darkGray) tagList = ["\\btrue\\b", "\\bfalse\\b"] # create patterns for tags for tag in tagList: self.rules.append( (self._create_regexp(tag), self._create_format(Qt.blue))) # create pattern for digits self.rules.append((self._create_regexp("\\d+"), self._create_format(QColor(127, 64, 127)))) # create pattern for params self.rules.append((self._create_regexp("\s*[_.\w]*\s*:"), self._create_format(Qt.darkBlue))) # create pattern for params self.rules.append( (self._create_regexp(":\s*:[_\.\w]*$|:\s*\@[_\.\w]*$"), self._create_format(Qt.darkBlue))) # create pattern for list signes self.rules.append((self._create_regexp("^\s*-"), self._create_format(Qt.darkRed, 'bold'))) # create pattern for ??? self.rules.append( (self._create_regexp("^---$"), self._create_format(Qt.darkRed))) # create pattern for braces self.rules.append((self._create_regexp("[\[\]\{\}\,]"), self._create_format(Qt.darkGreen))) # create patterns for strings self.rules.append((self._create_regexp("\".*\"|\'.*\'"), self._create_format(Qt.blue))) # create patterns for substitutions self.rules.append((self._create_regexp("\\$\\(.*\\)"), self._create_format(QColor(127, 64, 127)))) # create patterns for DOCTYPE self.rules.append((self._create_regexp("<!DOCTYPE.*>"), self._create_format(Qt.lightGray))) self.rules.append((self._create_regexp("<\\?xml.*\\?>"), self._create_format(Qt.lightGray)))
def __init__(self, parent): super(AngularLabel, self).__init__(parent) self.colorGreen = QColor(66, 255, 100, 128) self.colorRed = QColor(255, 66, 100, 128) self.brushGreen = QBrush(self.colorGreen) self.brushRed = QBrush(self.colorRed) self.penGreen = QPen(self.brushGreen, 1) self.penRed = QPen(self.brushRed, 1) self.setFixedSize(100, 100) self.angle = 0 self.velocity = 0
def _signal_redraw(self): """ Gets called either when new msg comes in or when marker is moved by user. """ if self._levels is None: return # update the limits self._min = 0 self._max = len(self._levels) - 1 self._scene.clear() qsize = self.size() width_tl = qsize.width() w = width_tl / float(max(len(self._levels), 1)) is_enabled = self.isEnabled() for i, level in enumerate(self._levels): h = self.viewport().height() # Figure out each cell's color. qcolor = QColor('grey') if is_enabled and level is not None: if level > DiagnosticStatus.ERROR: # Stale items should be reported as errors unless all stale level = DiagnosticStatus.ERROR qcolor = util.level_to_color(level) # TODO Use this code for adding gradation to the cell color. # end_color = QColor(0.5 * QColor('red').value(), # 0.5 * QColor('green').value(), # 0.5 * QColor('blue').value()) self._scene.addRect(w * i, 0, w, h, QColor('white'), qcolor) # Getting marker index. xpos_marker = self._xpos_marker # If marker is -1 for latest use (number_of_cells -1) if xpos_marker == -1: xpos_marker = len(self._levels) - 1 # Convert get horizontal pixel value of selected cell's center xpos_marker_in_pixel = (xpos_marker * w + (w / 2.0) - (self._timeline_marker_width / 2.0)) pos_marker = QPointF(xpos_marker_in_pixel, 0) # Need to instantiate marker everytime since it gets deleted # in every loop by scene.clear() timeline_marker = self._instantiate_tl_icon() timeline_marker.setPos(pos_marker) self._scene.addItem(timeline_marker)
def add_curve(self, curve_id, curve_name, x, y): color = QColor(self._colors[self._color_index % len(self._colors)]) self._color_index += 1 # line = self._canvas.axes.plot( # [], [], label=curve_name, linewidth=1, picker=5, # color=color.name())[0] line = None self._curves[curve_id] = [[], [], line, [None, None], (color.red() / 255.0, color.green() / 255.0, color.blue() / 255.0, 0.6)] self.update_values(curve_id, x, y) self._update_legend()
def add_curve(self, curve_id, curve_name, x, y): color = QColor(self._colors[self._color_index % len(self._colors)]) self._color_index += 1 #line = self._canvas.axes.plot([], [], label=curve_name, linewidth=1, picker=5, color=color.name())[0] line = None self._curves[curve_id] = [[], [], line, [None, None], (color.red() / 255.0, color.green() / 255.0, color.blue() / 255.0, 0.6)] self.update_values(curve_id, x, y) self._update_legend()
def slot_redraw(self): """ Gets called either when new msg comes in or when marker is moved by user. """ self._parent._scene.clear() qsize = self.size() width_tl = qsize.width() length_tl = ((self._max_num_seconds + 1) - self._min_num_seconds) len_queue = length_tl w = width_tl / float(len_queue) is_enabled = self.isEnabled() for i, m in enumerate(self._parent.get_diagnostic_queue()): h = self.viewport().height() # Figure out each cell's color. qcolor = None color_index = i + self._min_num_seconds if (is_enabled): qcolor = self._color_callback( self._parent.get_diagnostic_queue(), color_index) else: qcolor = QColor('grey') # TODO Use this code for adding gradation to the cell color. # end_color = QColor(0.5 * QColor('red').value(), # 0.5 * QColor('green').value(), # 0.5 * QColor('blue').value()) self._parent._scene.addRect(w * i, 0, w, h, QColor('white'), qcolor) rospy.logdebug('slot_redraw #%d th loop w=%s width_tl=%s', i, w, width_tl) # Setting marker. xpos_marker = ((self._xpos_marker - 1) * w + (w / 2.0) - (self._timeline_marker_width / 2.0)) pos_marker = QPointF(xpos_marker, 0) # Need to instantiate marker everytime since it gets deleted # in every loop by scene.clear() timeline_marker = self._instantiate_tl_icon() timeline_marker.setPos(pos_marker) self._parent._scene.addItem(timeline_marker) rospy.logdebug(' slot_redraw xpos_marker(int)=%s length_tl=%s', int(xpos_marker), length_tl)
def __init__(self, parent=None): QSyntaxHighlighter.__init__(self, parent) self.rules = [] self.comment_start = QRegExp("<!--") self.comment_end = QRegExp("-->") self.comment_format = self._create_format(Qt.darkGray, 'italic') # self.mark_background = QBrush(QColor(251, 247, 222)) # create patterns for braces self.rules.append((self._create_regexp("</?|/?>"), self._create_format(QColor(24, 24, 24)))) # create patterns for TAG tag_list = '|'.join( ["\\b%s\\b" % t for t in self.LAUNCH_CHILDS.keys()]) self.rules.append( (self._create_regexp(tag_list), self._create_format(Qt.darkRed))) # create patterns for ATTRIBUTES attr_list = '|'.join( set([ "\\b%s" % attr for v in self.LAUNCH_ATTR.values() for attr in v.keys() ])) self.rules.append((self._create_regexp(attr_list), self._create_format(QColor(0, 100, 0)))) # darkGreen # create patterns for substitutions self.rule_arg = (self._create_regexp("\\$\\(.*\\)"), self._create_format(QColor(77, 0, 38))) # create patterns for DOCTYPE self.rules.append((self._create_regexp("<!DOCTYPE.*>"), self._create_format(Qt.lightGray))) self.rules.append((self._create_regexp("<\\?xml.*\\?>"), self._create_format(Qt.lightGray))) # create patterns for yaml parameter inside self.rules.append((self._create_regexp("^\s*[_.\w]*\s*:"), self._create_format(Qt.darkBlue))) # create patterns for yaml oneline strings inside self.rules.append( (self._create_regexp("'.*'"), self._create_format(Qt.blue))) # create pattern for list signes self.rules.append((self._create_regexp("^\s*-"), self._create_format(Qt.darkRed, 'bold'))) # create pattern for digits self.rules.append((self._create_regexp("\\d+"), self._create_format(QColor(127, 64, 127)))) # create patterns for strings self.string_pattern = QRegExp("\"") self.string_format = self._create_format(Qt.blue) # part to select an XML block self._tag_hl_range = [] # list with puples (start, length) self._tag_hl_last = set() # set with blocks of last highlighted tags
def transitionSelected(self, index): for e in self.prev_selected_connections: e.setPen(QPen(QBrush(QColor(0, 0, 0)), 0)) self.prev_selected_connections = [] if not self.component_selected: return for e in self.edges: data = e.data(0) if (data[0] == self.component_selected) and ( data[1] == self.selected_component_next_states_names[index]): e.setPen(QPen(QBrush(QColor(255, 0, 0)), 5)) self.prev_selected_connections.append(e)
def draw_robot_pose(self, data): old_item = None if self._robot_pose_item: old_item = self._robot_pose_item robot_pose = QMatrix().rotate(data['yaw']).map( self._robot_polygon).translated(data['x'], data['y']) self._robot_pose_item = self._scene.addPolygon( robot_pose, pen=QPen(QColor(255, 0, 0)), brush=QBrush(QColor(255, 0, 0))) # Everything must be mirrored self._mirror(self._robot_pose_item) if old_item: self._scene.removeItem(old_item)
def choose_brush(index): """ Chooses the brush according o the content of a cell. :param index: the index of the item :type index: QModelIndex """ if index.data() == "ok": return QColor(127, 255, 0) elif index.data() == "warning": return QColor(255, 165, 0) elif index.data() == "error": return QColor(255, 0, 0) return None
def getNodeItemForSubgraph(self, subgraph, highlight_level, scene=None): # let pydot imitate pygraphviz api attr = {} for name in subgraph.get_attributes().keys(): value = get_unquoted(subgraph, name) attr[name] = value obj_dic = subgraph.__getattribute__('obj_dict') for name in obj_dic: if name not in ['nodes', 'attributes', 'parent_graph' ] and obj_dic[name] is not None: attr[name] = get_unquoted(obj_dic, name) elif name == 'nodes': for key in obj_dic['nodes']['graph'][0]['attributes']: attr[key] = get_unquoted( obj_dic['nodes']['graph'][0]['attributes'], key) subgraph.attr = attr bb = subgraph.attr.get('bb', None) if bb is None: # no bounding box return None bb = bb.strip('"').split(',') if len(bb) < 4: # bounding box is empty return None bounding_box = QRectF(0, 0, float(bb[2]) - float(bb[0]), float(bb[3]) - float(bb[1])) if 'lp' in subgraph.attr: label_pos = subgraph.attr['lp'].strip('"').split(',') else: label_pos = (float(bb[0]) + (float(bb[2]) - float(bb[0])) / 2, float(bb[1]) + (float(bb[3]) - float(bb[1])) - LABEL_HEIGHT / 2) bounding_box.moveCenter( QPointF( float(bb[0]) + (float(bb[2]) - float(bb[0])) / 2, -float(bb[1]) - (float(bb[3]) - float(bb[1])) / 2)) name = subgraph.attr.get('label', '') color = QColor( subgraph.attr['color']) if 'color' in subgraph.attr else None subgraph_nodeitem = NodeItem( highlight_level, bounding_box, label=name, shape='box', color=color, parent=scene.activePanel() if scene is not None else None, label_pos=QPointF(float(label_pos[0]), -float(label_pos[1]))) bounding_box = QRectF(bounding_box) # With clusters we have the problem that mouse hovers cannot # decide whether to be over the cluster or a subnode. Using # just the "title area" solves this. TODO: Maybe using a # border region would be even better (multiple RectF) bounding_box.setHeight(LABEL_HEIGHT) subgraph_nodeitem.set_hovershape(bounding_box) if scene is not None: scene.addItem(subgraph_nodeitem) return subgraph_nodeitem
def add_curve(self, curve_id, curve_name, data_x, data_y, curve_color=None, dashed=False): """Add a new, named curve to this plot Add a curve named `curve_name` to the plot, with initial data series `data_x` and `data_y`. Future references to this curve should use the provided `curve_id` Note that the plot is not redraw automatically; call `redraw()` to make any changes visible to the user. """ if curve_color is None: curve_color = QColor(self._colors[self._color_index % len(self._colors)]) self._color_index += 1 self._curves[curve_id] = { 'x': numpy.array(data_x), 'y': numpy.array(data_y), 'name': curve_name, 'color': curve_color, 'dashed': dashed } if self._data_plot_widget: self._add_curve.emit(curve_id, curve_name, curve_color, self._markers_on, dashed)
class DmgEdgeItem(HoveredEdgeItem): HIGHLIGHTED_COLOR = QColor(0, 0, 0) HIGHLIGHTED_PEN_WIDTH = 2.0 def __init__(self, spline, label, label_center, from_node, to_node, parent=None, **kwargs): super(DmgEdgeItem, self).__init__(spline, label, label_center, from_node, to_node, parent, **kwargs) self._highlighted_color = kwargs.get('highlighted_color', self.HIGHLIGHTED_COLOR) self._highlighted_pen_width = kwargs.get('highlighted_pen_width', self.HIGHLIGHTED_PEN_WIDTH) self._current_color = self._color self._previous_color = self._color self._current_pen_width = self._pen_width self._previous_pen_width = self._pen_width def set_color(self, color=None): if color is None: color = self._color self._previous_color = self._current_color self._current_color = color self._brush.setColor(color) self._edge_pen.setColor(color) self._label_pen.setColor(color) self._path.setPen(self._edge_pen) if self._arrow is not None: self._arrow.setBrush(self._brush) self._arrow.setPen(self._edge_pen) if self._label is not None: self._label.setBrush(self._brush) self._label.setPen(self._label_pen) def highlight(self, highlighted=True): if highlighted: self.set_color(self._highlighted_color) else: self.set_color(self._previous_color) def hoverEnterEvent(self, event): self.highlight() self._highlight_connections() def hoverLeaveEvent(self, event): self.highlight(False) self._highlight_connections(False)
def getUniqueNameAndColor(self): """ Use golden ratio to generate unique colors. http://martin.ankerl.com/2009/12/09/how-to-create-random-colors-programmatically/ """ name = "new_loc" + str(self.unique_loc_counter) h = int(359 * (self.unique_loc_counter * 0.618033988749895)) h = h % 359 self.unique_loc_counter += 1 return name, QColor.fromHsv(h, 255, 255)
def _update_status(self, status): if status == -1: self._widget.safety_mode.setText('UNKNOWN SAFETY MODE') self._widget.safety_mode.setToolTip('The WAM is in an unknown state. Proceed with caution.') color = QColor(200,200,200) self._widget.button_set_home.setEnabled(False) self._widget.button_idle_wam.setEnabled(False) self._widget.button_run_wam.setEnabled(False) elif status == 0: self._widget.safety_mode.setText('E-STOP') self._widget.safety_mode.setToolTip('The WAM is stopped and unpowered. Determine the source of the fault and reset the arm to continue using it.') self._widget.button_set_home.setEnabled(False) self._widget.button_idle_wam.setEnabled(False) self._widget.button_run_wam.setEnabled(False) color = self.barrett_red else: if not self.homed: self._widget.safety_mode.setText('UNCALIBRATED') self._widget.safety_mode.setToolTip('The WAM is not calibrated. Please place it in the calibration posture and click the "Calibrate" button.') self._widget.button_set_home.setEnabled(True) self._widget.button_idle_wam.setEnabled(False) self._widget.button_run_wam.setEnabled(False) color = self.barrett_orange else: if status == 1: self._widget.safety_mode.setText('IDLE') self._widget.safety_mode.setToolTip('The WAM is running but all joints are passive. It is safe to home the arm.') self._widget.button_set_home.setEnabled(True) self._widget.button_idle_wam.setEnabled(True) self._widget.button_run_wam.setEnabled(True) color = self.barrett_blue elif status == 2: self._widget.safety_mode.setText('ACTIVE') self._widget.safety_mode.setToolTip('The WAM is running and all joints are active. Proceed with caution.') self._widget.button_set_home.setEnabled(False) self._widget.button_idle_wam.setEnabled(False) self._widget.button_run_wam.setEnabled(False) color = self.barrett_green darker = color.darker() lighter = color.lighter() self._widget.safety_mode.setStyleSheet("QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }" % ( color.red(), color.green(), color.blue(), lighter.red(), lighter.green(), lighter.blue()))
def updateOverlay(self, rect = None): # Redraw the overlay image from scratch using the location image and current location. self.image.overlay_image.fill(Qt.transparent) painter = QPainter(self.image.overlay_image) painter.setBackgroundMode(Qt.TransparentMode) painter.setCompositionMode(QPainter.CompositionMode_Source) for location in self.locations: if self.draw_location[location]: color = self.location_colors[location] if self.edit_properties_location == location and self.editing_properties: color = self.edit_area_selection_color lineColor = QColor(color) lineColor.setAlpha(255) brushColor = QColor(color) brushColor.setAlpha(128) painter.setPen(lineColor) painter.setBrush(brushColor) painter.drawPolygon(self.locations[location]) if (self.current_selection is not None) or (self.new_selection is not None): lineColor = QColor(self.edit_area_selection_color) lineColor.setAlpha(255) brushColor = QColor(self.edit_area_selection_color) brushColor.setAlpha(128) painter.setPen(lineColor) painter.setBrush(brushColor) if self.new_selection is not None: # Create a temporary polygon as the new selection is being drawn. if self.current_selection is not None: current_selection = QPolygon(self.current_selection) if self.subtract_new_selection: current_selection = current_selection.subtracted(self.new_selection) else: current_selection = current_selection.united(self.new_selection) painter.drawPolygon(current_selection) elif self.subtract_new_selection == False: painter.drawPolygon(self.new_selection) else: painter.drawPolygon(self.current_selection) painter.end() if rect is None: self.image.update() else: self.image.update(rect)
def __init__(self, context): super(BarrettDashboard, self).__init__(context) self._joint_sub = None # Give QObjects reasonable names self.setObjectName('BarrettDashboard') # 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__)), 'barrett_dashboard.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('BarrettDashboardPluginUi') # 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) self.context = context jp_widgets = [getattr(self._widget,'jp_%d' % i) for i in range(7)] jn_widgets = [getattr(self._widget,'jn_%d' % i) for i in range(7)] self.joint_widgets = zip(jp_widgets,jn_widgets) tp_widgets = [getattr(self._widget,'tp_%d' % i) for i in range(7)] tn_widgets = [getattr(self._widget,'tn_%d' % i) for i in range(7)] self.torque_widgets = zip(tp_widgets,tn_widgets) self.joint_signals = [] self.torque_signals = [] for (tp,tn) in self.torque_widgets: tp.setRange(0.0,1.0,False) tn.setRange(1.0,0.0,False) tp.setValue(0.0) tn.setValue(0.0) # set random values for testing #v = (2.0*random.random()) - 1.0 #tp.setValue(v if v >=0 else 0) #tn.setValue(-v if v <0 else 0) for (jp,jn) in self.joint_widgets: jp.setRange(0.0,1.0,False) jn.setRange(1.0,0.0,False) jp.setValue(0.0) jn.setValue(0.0) # set random values for testing #v = (2.0*random.random()) - 1.0 #jp.setValue(v if v >=0 else 0) #jn.setValue(-v if v <0 else 0) self.barrett_green = QColor(87,186,142) self.barrett_green_dark = self.barrett_green.darker() self.barrett_green_light = self.barrett_green.lighter() self.barrett_blue = QColor(80,148,204) self.barrett_blue_dark = self.barrett_blue.darker() self.barrett_blue_light = self.barrett_blue.lighter() self.barrett_red = QColor(232,47,47) self.barrett_red_dark = self.barrett_red.darker() self.barrett_red_light = self.barrett_red.lighter() self.barrett_orange = QColor(255,103,43) self.barrett_orange_dark = self.barrett_orange.darker() #background_color = p.mid().color() joint_bg_color = self.barrett_blue_dark.darker() joint_fill_color = self.barrett_blue joint_alarm_color = self.barrett_blue #self.barrett_blue_light torque_bg_color = self.barrett_green_dark.darker() torque_fill_color = self.barrett_green torque_alarm_color = self.barrett_orange #self.barrett_green_light temp_bg_color = self.barrett_red_dark.darker() temp_fill_color = self.barrett_orange temp_alarm_color = self.barrett_red for w in jp_widgets + jn_widgets: w.setAlarmLevel(0.80) w.setFillColor(joint_fill_color) w.setAlarmColor(joint_alarm_color) p = w.palette() p.setColor(tp.backgroundRole(), joint_bg_color) w.setPalette(p) for w in tp_widgets + tn_widgets: w.setAlarmLevel(0.66) w.setFillColor(torque_fill_color) w.setAlarmColor(torque_alarm_color) p = w.palette() p.setColor(tp.backgroundRole(), torque_bg_color) w.setPalette(p) self._widget.hand_temp.setAlarmLevel(0.66) self._widget.hand_temp.setFillColor(temp_fill_color) self._widget.hand_temp.setAlarmColor(temp_alarm_color) p = self._widget.hand_temp.palette() p.setColor(self._widget.hand_temp.backgroundRole(), temp_bg_color) self._widget.hand_temp.setPalette(p) self._widget.hand_temp.setOrientation(Qt.Horizontal, QwtThermo.RightScale) self._widget.jf_0.setStyleSheet("QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }" % ( joint_bg_color.red(), joint_bg_color.green(), joint_bg_color.blue(), joint_fill_color.red(), joint_fill_color.green(), joint_fill_color.blue())) self.urdf = rospy.get_param('robot_description') self.robot = URDF() self.robot = self.robot.from_xml_string(self.urdf) self.pos_norm = [0] * 7 self.torque_norm = [0] * 7 self._joint_sub = rospy.Subscriber( 'joint_states', sensor_msgs.msg.JointState, self._joint_state_cb) self._status_sub = rospy.Subscriber( 'status', oro_barrett_msgs.msg.BarrettStatus, self._status_cb) self._hand_status_sub = rospy.Subscriber( 'hand/status', oro_barrett_msgs.msg.BHandStatus, self._hand_status_cb) self._update_status(-1) self.safety_mode = -1 self.run_mode = 0 self.hand_run_mode = 0 self.hand_min_temperature = 40.0 self.hand_max_temperature = 65.0 self.hand_temperature = 0.0 self.update_timer = QTimer(self) self.update_timer.setInterval(50) self.update_timer.timeout.connect(self._update_widget_values) self.update_timer.start() self.set_home_client = actionlib.SimpleActionClient( 'wam/set_home_action', oro_barrett_msgs.msg.SetHomeAction) self.set_mode_client = actionlib.SimpleActionClient( 'wam/set_mode_action', oro_barrett_msgs.msg.SetModeAction) self._widget.button_set_home.clicked[bool].connect(self._handle_set_home_clicked) self._widget.button_idle_wam.clicked[bool].connect(self._handle_idle_wam_clicked) self._widget.button_run_wam.clicked[bool].connect(self._handle_run_wam_clicked) self.bhand_init_client = actionlib.SimpleActionClient( 'hand/initialize_action', oro_barrett_msgs.msg.BHandInitAction) self.bhand_set_mode_client = actionlib.SimpleActionClient( 'hand/set_mode_action', oro_barrett_msgs.msg.BHandSetModeAction) self.grasp_client = actionlib.SimpleActionClient( 'grasp', BHandGraspAction) self.release_client = actionlib.SimpleActionClient( 'release', BHandReleaseAction) self.spread_client = actionlib.SimpleActionClient( 'spread', BHandSpreadAction) self._widget.button_initialize_hand.clicked[bool].connect(self._handle_initialize_hand_clicked) self._widget.button_idle_hand.clicked[bool].connect(self._handle_idle_hand_clicked) self._widget.button_run_hand.clicked[bool].connect(self._handle_run_hand_clicked) self._widget.button_release_hand.clicked[bool].connect(self._handle_release_hand_clicked) self._widget.button_grasp_hand.clicked[bool].connect(self._handle_grasp_hand_clicked) self._widget.button_set_spread.clicked[bool].connect(self._handle_spread_hand_clicked) self._widget.resizeEvent = self._handle_resize
class BarrettDashboard(Plugin): def __init__(self, context): super(BarrettDashboard, self).__init__(context) self._joint_sub = None # Give QObjects reasonable names self.setObjectName('BarrettDashboard') # 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__)), 'barrett_dashboard.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('BarrettDashboardPluginUi') # 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) self.context = context jp_widgets = [getattr(self._widget,'jp_%d' % i) for i in range(7)] jn_widgets = [getattr(self._widget,'jn_%d' % i) for i in range(7)] self.joint_widgets = zip(jp_widgets,jn_widgets) tp_widgets = [getattr(self._widget,'tp_%d' % i) for i in range(7)] tn_widgets = [getattr(self._widget,'tn_%d' % i) for i in range(7)] self.torque_widgets = zip(tp_widgets,tn_widgets) self.joint_signals = [] self.torque_signals = [] for (tp,tn) in self.torque_widgets: tp.setRange(0.0,1.0,False) tn.setRange(1.0,0.0,False) tp.setValue(0.0) tn.setValue(0.0) # set random values for testing #v = (2.0*random.random()) - 1.0 #tp.setValue(v if v >=0 else 0) #tn.setValue(-v if v <0 else 0) for (jp,jn) in self.joint_widgets: jp.setRange(0.0,1.0,False) jn.setRange(1.0,0.0,False) jp.setValue(0.0) jn.setValue(0.0) # set random values for testing #v = (2.0*random.random()) - 1.0 #jp.setValue(v if v >=0 else 0) #jn.setValue(-v if v <0 else 0) self.barrett_green = QColor(87,186,142) self.barrett_green_dark = self.barrett_green.darker() self.barrett_green_light = self.barrett_green.lighter() self.barrett_blue = QColor(80,148,204) self.barrett_blue_dark = self.barrett_blue.darker() self.barrett_blue_light = self.barrett_blue.lighter() self.barrett_red = QColor(232,47,47) self.barrett_red_dark = self.barrett_red.darker() self.barrett_red_light = self.barrett_red.lighter() self.barrett_orange = QColor(255,103,43) self.barrett_orange_dark = self.barrett_orange.darker() #background_color = p.mid().color() joint_bg_color = self.barrett_blue_dark.darker() joint_fill_color = self.barrett_blue joint_alarm_color = self.barrett_blue #self.barrett_blue_light torque_bg_color = self.barrett_green_dark.darker() torque_fill_color = self.barrett_green torque_alarm_color = self.barrett_orange #self.barrett_green_light temp_bg_color = self.barrett_red_dark.darker() temp_fill_color = self.barrett_orange temp_alarm_color = self.barrett_red for w in jp_widgets + jn_widgets: w.setAlarmLevel(0.80) w.setFillColor(joint_fill_color) w.setAlarmColor(joint_alarm_color) p = w.palette() p.setColor(tp.backgroundRole(), joint_bg_color) w.setPalette(p) for w in tp_widgets + tn_widgets: w.setAlarmLevel(0.66) w.setFillColor(torque_fill_color) w.setAlarmColor(torque_alarm_color) p = w.palette() p.setColor(tp.backgroundRole(), torque_bg_color) w.setPalette(p) self._widget.hand_temp.setAlarmLevel(0.66) self._widget.hand_temp.setFillColor(temp_fill_color) self._widget.hand_temp.setAlarmColor(temp_alarm_color) p = self._widget.hand_temp.palette() p.setColor(self._widget.hand_temp.backgroundRole(), temp_bg_color) self._widget.hand_temp.setPalette(p) self._widget.hand_temp.setOrientation(Qt.Horizontal, QwtThermo.RightScale) self._widget.jf_0.setStyleSheet("QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }" % ( joint_bg_color.red(), joint_bg_color.green(), joint_bg_color.blue(), joint_fill_color.red(), joint_fill_color.green(), joint_fill_color.blue())) self.urdf = rospy.get_param('robot_description') self.robot = URDF() self.robot = self.robot.from_xml_string(self.urdf) self.pos_norm = [0] * 7 self.torque_norm = [0] * 7 self._joint_sub = rospy.Subscriber( 'joint_states', sensor_msgs.msg.JointState, self._joint_state_cb) self._status_sub = rospy.Subscriber( 'status', oro_barrett_msgs.msg.BarrettStatus, self._status_cb) self._hand_status_sub = rospy.Subscriber( 'hand/status', oro_barrett_msgs.msg.BHandStatus, self._hand_status_cb) self._update_status(-1) self.safety_mode = -1 self.run_mode = 0 self.hand_run_mode = 0 self.hand_min_temperature = 40.0 self.hand_max_temperature = 65.0 self.hand_temperature = 0.0 self.update_timer = QTimer(self) self.update_timer.setInterval(50) self.update_timer.timeout.connect(self._update_widget_values) self.update_timer.start() self.set_home_client = actionlib.SimpleActionClient( 'wam/set_home_action', oro_barrett_msgs.msg.SetHomeAction) self.set_mode_client = actionlib.SimpleActionClient( 'wam/set_mode_action', oro_barrett_msgs.msg.SetModeAction) self._widget.button_set_home.clicked[bool].connect(self._handle_set_home_clicked) self._widget.button_idle_wam.clicked[bool].connect(self._handle_idle_wam_clicked) self._widget.button_run_wam.clicked[bool].connect(self._handle_run_wam_clicked) self.bhand_init_client = actionlib.SimpleActionClient( 'hand/initialize_action', oro_barrett_msgs.msg.BHandInitAction) self.bhand_set_mode_client = actionlib.SimpleActionClient( 'hand/set_mode_action', oro_barrett_msgs.msg.BHandSetModeAction) self.grasp_client = actionlib.SimpleActionClient( 'grasp', BHandGraspAction) self.release_client = actionlib.SimpleActionClient( 'release', BHandReleaseAction) self.spread_client = actionlib.SimpleActionClient( 'spread', BHandSpreadAction) self._widget.button_initialize_hand.clicked[bool].connect(self._handle_initialize_hand_clicked) self._widget.button_idle_hand.clicked[bool].connect(self._handle_idle_hand_clicked) self._widget.button_run_hand.clicked[bool].connect(self._handle_run_hand_clicked) self._widget.button_release_hand.clicked[bool].connect(self._handle_release_hand_clicked) self._widget.button_grasp_hand.clicked[bool].connect(self._handle_grasp_hand_clicked) self._widget.button_set_spread.clicked[bool].connect(self._handle_spread_hand_clicked) self._widget.resizeEvent = self._handle_resize def _handle_resize(self, event): for i,((w1,w2),(w3,w4)) in enumerate(zip(self.joint_widgets, self.torque_widgets)): width = 0.8*getattr(self._widget, 'jcc_%d'%i).contentsRect().width() w1.setPipeWidth(width) w2.setPipeWidth(width) w3.setPipeWidth(width) w4.setPipeWidth(width) def _handle_set_home_clicked(self, checked): if checked: self.set_home() def _update_status(self, status): if status == -1: self._widget.safety_mode.setText('UNKNOWN SAFETY MODE') self._widget.safety_mode.setToolTip('The WAM is in an unknown state. Proceed with caution.') color = QColor(200,200,200) self._widget.button_set_home.setEnabled(False) self._widget.button_idle_wam.setEnabled(False) self._widget.button_run_wam.setEnabled(False) elif status == 0: self._widget.safety_mode.setText('E-STOP') self._widget.safety_mode.setToolTip('The WAM is stopped and unpowered. Determine the source of the fault and reset the arm to continue using it.') self._widget.button_set_home.setEnabled(False) self._widget.button_idle_wam.setEnabled(False) self._widget.button_run_wam.setEnabled(False) color = self.barrett_red else: if not self.homed: self._widget.safety_mode.setText('UNCALIBRATED') self._widget.safety_mode.setToolTip('The WAM is not calibrated. Please place it in the calibration posture and click the "Calibrate" button.') self._widget.button_set_home.setEnabled(True) self._widget.button_idle_wam.setEnabled(False) self._widget.button_run_wam.setEnabled(False) color = self.barrett_orange else: if status == 1: self._widget.safety_mode.setText('IDLE') self._widget.safety_mode.setToolTip('The WAM is running but all joints are passive. It is safe to home the arm.') self._widget.button_set_home.setEnabled(True) self._widget.button_idle_wam.setEnabled(True) self._widget.button_run_wam.setEnabled(True) color = self.barrett_blue elif status == 2: self._widget.safety_mode.setText('ACTIVE') self._widget.safety_mode.setToolTip('The WAM is running and all joints are active. Proceed with caution.') self._widget.button_set_home.setEnabled(False) self._widget.button_idle_wam.setEnabled(False) self._widget.button_run_wam.setEnabled(False) color = self.barrett_green darker = color.darker() lighter = color.lighter() self._widget.safety_mode.setStyleSheet("QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }" % ( color.red(), color.green(), color.blue(), lighter.red(), lighter.green(), lighter.blue())) def _update_widget_values(self): if self.safety_mode in (oro_barrett_msgs.msg.SafetyMode.ACTIVE, oro_barrett_msgs.msg.SafetyMode.IDLE): for (v,(tp,tn)) in zip(self.torque_norm,self.torque_widgets): tp.setEnabled(True) tn.setEnabled(True) tp.setValue(v if v >=0 else 0) tn.setValue(-v if v <0 else 0) for (v,(jp,jn)) in zip(self.pos_norm,self.joint_widgets): jp.setEnabled(True) jn.setEnabled(True) jp.setValue(v if v >=0 else 0) jn.setValue(-v if v <0 else 0) else: for (p,n) in self.joint_widgets + self.torque_widgets: p.setEnabled(True) n.setEnabled(True) self._widget.hand_temp.setValue((self.hand_temperature-self.hand_min_temperature)/(self.hand_max_temperature-self.hand_min_temperature)) self._update_status(self.safety_mode) self._update_buttons(self.run_mode, self.hand_run_mode) def _update_buttons(self, run_mode, hand_run_mode): if run_mode == oro_barrett_msgs.msg.RunMode.IDLE: self._widget.button_idle_wam.setChecked(True) self._widget.button_run_wam.setChecked(False) else: self._widget.button_idle_wam.setChecked(False) self._widget.button_run_wam.setChecked(True) if hand_run_mode == oro_barrett_msgs.msg.RunMode.RUN: self._widget.button_idle_hand.setChecked(False) self._widget.button_run_hand.setChecked(True) else: self._widget.button_idle_hand.setChecked(True) self._widget.button_run_hand.setChecked(False) 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 #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 _joint_state_cb(self, msg): joint_pos_map = dict(zip(msg.name, msg.position)) for (name,pos,torque,i) in zip(msg.name,msg.position,msg.effort,range(7)): joint = self.robot.joint_map[name] self.pos_norm[i] = 2.0*((pos-joint.limit.lower)/(joint.limit.upper-joint.limit.lower)) - 1.0 self.torque_norm[i] = torque/joint.limit.effort def _status_cb(self, msg): self.safety_mode = msg.safety_mode.value self.run_mode = msg.run_mode.value self.homed = msg.homed if self.safety_mode == oro_barrett_msgs.msg.SafetyMode.ACTIVE: self._widget.button_initialize_hand.setEnabled(False) elif self.safety_mode == oro_barrett_msgs.msg.SafetyMode.ESTOP: self._widget.button_initialize_hand.setEnabled(False) self._widget.button_idle_hand.setEnabled(False) self._widget.button_run_hand.setEnabled(False) def _hand_status_cb(self, msg): self.hand_initialized = msg.initialized self.hand_temperature = max(msg.temperature) self.hand_run_mode = msg.run_mode.value def _handle_set_home_clicked(self, checked): goal = oro_barrett_msgs.msg.SetHomeGoal() self.set_home_client.send_goal(goal) def _handle_idle_wam_clicked(self, checked): if checked: goal = oro_barrett_msgs.msg.SetModeGoal() goal.mode.value = oro_barrett_msgs.msg.RunMode.IDLE self.set_mode_client.send_goal(goal) def _handle_run_wam_clicked(self, checked): if checked: goal = oro_barrett_msgs.msg.SetModeGoal() goal.mode.value = oro_barrett_msgs.msg.RunMode.RUN self.set_mode_client.send_goal(goal) def _handle_initialize_hand_clicked(self, checked): if self.safety_mode == oro_barrett_msgs.msg.SafetyMode.IDLE: goal = oro_barrett_msgs.msg.BHandInitGoal() self.bhand_init_client.send_goal(goal) def _handle_run_hand_clicked(self, checked): if checked: goal = oro_barrett_msgs.msg.BHandSetModeGoal() goal.run_mode.value = oro_barrett_msgs.msg.RunMode.RUN self.bhand_set_mode_client.send_goal(goal) def _handle_idle_hand_clicked(self, checked): if checked: goal = oro_barrett_msgs.msg.BHandSetModeGoal() goal.run_mode.value = oro_barrett_msgs.msg.RunMode.IDLE self.bhand_set_mode_client.send_goal(goal) def _get_grasp_mask(self): return [ self._widget.button_use_f1.isChecked(), self._widget.button_use_f2.isChecked(), self._widget.button_use_f3.isChecked()] def _handle_release_hand_clicked(self, checked): goal = BHandReleaseGoal() goal.release_mask = self._get_grasp_mask() goal.release_speed = [3.0, 3.0, 3.0] self.release_client.send_goal(goal) def _handle_grasp_hand_clicked(self, checked): goal = BHandGraspGoal() goal.grasp_mask = self._get_grasp_mask() goal.grasp_speed = [1.0, 1.0, 1.0] goal.grasp_effort = [1.0, 1.0, 1.0] goal.min_fingertip_radius = 0.0 self.grasp_client.send_goal(goal) def _handle_spread_hand_clicked(self, checked): goal = BHandSpreadGoal() goal.spread_position = self._widget.spread_slider.value()/1000.0*3.141 self.spread_client.send_goal(goal)