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)
