def _adjust_html_node_size(self, node):
if node.get_name() in ('graph', 'node', 'empty'):
return node
label = node.get_label()
if not label:
return node
label = label.strip('\"')
if not label.startswith('<') and not label.endswith('>'):
return node
text = QGraphicsTextItem()
text.setHtml(label)
bounding_box = text.boundingRect()
width = bounding_box.width() / POINTS_PER_INCH
height = bounding_box.height() / POINTS_PER_INCH
url = node.get_attributes().get('URL', None)
return self._create_fixed_size_node(node.get_name(), label, str(width),
str(height), url)
def __init__(self,
bounding_box,
shape,
label,
label_pos=None,
url=None,
parent=None,
**kwargs):
super(DmgHtmlNodeItem, self).__init__(parent, **kwargs)
self.url = url
self._incoming_edges = set()
self._outgoing_edges = set()
self._brush = QBrush(self._color)
self._label_pen = QPen()
self._label_pen.setColor(self._color)
self._label_pen.setJoinStyle(Qt.RoundJoin)
self._label_pen.setWidthF(self._label_pen_width)
self._graphics_item_pen = QPen(self._label_pen)
self._graphics_item_pen.setWidthF(self._pen_width)
self._label = QGraphicsTextItem()
self._label.setHtml(label)
label_rectangle = self._label.boundingRect()
if label_pos is None:
label_rectangle.moveCenter(bounding_box.center())
else:
label_rectangle.moveCenter(label_pos)
self._label.setPos(label_rectangle.x(), label_rectangle.y())
self.addToGroup(self._label)
self._graphics_item = ShapeFactory.create(shape, bounding_box)
if ShapeFactory.message is not None:
print ShapeFactory.message
self.addToGroup(self._graphics_item)
self._brush.setColor(self._color)
self._graphics_item_pen.setColor(self._color)
self._label_pen.setColor(self._color)
self._graphics_item.setPen(self._graphics_item_pen)
self._highlight_level = kwargs.get('highlight_level',
self.HIGHLIGHT_LEVEL)
self._hovered_color = kwargs.get('hovered_color', self.HOVERED_COLOR)
self._highlighted_color = kwargs.get('highlighted_color',
self.HIGHLIGHTED_COLOR)
self._highlighted_pen_width = kwargs.get('highlighted_pen_width',
self.HIGHLIGHTED_PEN_WIDTH)
self._highlighted_label_pen_width = kwargs.get(
'highlighted_label_pen_width', self.HIGHLIGHTED_LABEL_PEN_WIDTH)
self.hover_shape = None
self.setAcceptHoverEvents(True)
def reset(self):
# Disable buttons
self.button_setup.setEnabled(False)
self.button_start_sim.setEnabled(False)
self.button_execute_task.setEnabled(False)
# Reinitialize map and clear all item lists
self.current_graphicsScene = MapGraphicsScene()
self.graphicsView_main.setScene(self.current_graphicsScene)
self.scenario = self.world_comboBox.currentText()
rospy.loginfo(
'rqt_simulation : A new world %s has been selected. Reset the chosen FTS.'
% (self.scenario))
self.current_graphicsScene.load_map(self.scenario)
# Scale map
transform = self.current_graphicsScene.scale_map(
self.graphicsView_main, self.scenario)
self.graphicsView_main.setTransform(transform)
# Reset initial pose and plan textboxes
for i in range(0, self.num_robots):
self.tab_list[i].robot_comboBox_init.clear()
self.tab_list[i].initial_pose['start_' + str(i + 1).zfill(
2)]['text_item'] = QGraphicsTextItem('start_' +
str(i + 1).zfill(2))
self.tab_list[i].initial_pose['start_' +
str(i +
1).zfill(2)]['text_item'].setPos(
0.0, 0.0)
self.current_graphicsScene.addItem(
self.tab_list[i].initial_pose['start_' +
str(i +
1).zfill(2)]['text_item'])
self.tab_list[i].robot_sufix_textbox.clear()
self.tab_list[i].robot_prefix_textbox.clear()
self.prefix_string = ''
self.sufix_string = ''
# Reinitialize FTS
self.FTS = FTS()
# Load new map
try:
self.scenario_file = os.path.join(
rospkg.RosPack().get_path('rqt_simulation'), 'config', 'FTS',
'env_GUI.yaml')
except:
print('In file ' + self.filename + ' at line ' +
str(self.cf.f_lineno) +
': Error while loading env_GUI.yaml file')
exit()
# Reinitialize ROI marker msg
self.region_pose_marker_array_msg = MarkerArray()
def add_ROI(self, pixel_coords):
self.regionCounter += 1
markerSize = 25
ellipse_item = QGraphicsEllipseItem(
QRectF(
QPointF(pixel_coords.x() - markerSize / 2,
pixel_coords.y() - markerSize / 2),
QSizeF(markerSize, markerSize)))
ellipse_item.setBrush(QBrush(QColor('red')))
self.addItem(ellipse_item)
label_font = QFont()
label_font.setPointSize(15)
region_string = 'r' + str(self.regionCounter).zfill(2)
ellipse_item_label = QGraphicsTextItem(region_string)
ellipse_item_label.setPos(pixel_coords)
ellipse_item_label.setFont(label_font)
self.addItem(ellipse_item_label)
self.items_dict.update({
region_string: {
'ellipse_item': ellipse_item,
'ellipse_item_label': ellipse_item_label,
'pixel_coords': pixel_coords,
'ap_item_label': {}
}
})
def _adjust_html_node_size(self, node):
if node.get_name() in ('graph', 'node', 'empty'):
return node
label = node.get_label()
if not label:
return node
label = label.strip('\"')
if not label.startswith('<') and not label.endswith('>'):
return node
text = QGraphicsTextItem()
text.setHtml(label)
bounding_box = text.boundingRect()
width = bounding_box.width() / POINTS_PER_INCH
height = bounding_box.height() / POINTS_PER_INCH
url = node.get_attributes().get('URL', None)
return self._create_fixed_size_node(node.get_name(), label, str(width), str(height), url)
def add_ap(self, region, ap):
label_font = QFont()
label_font.setPointSize(15)
ap_item_label = QGraphicsTextItem(ap)
ap_item_label.setPos(
QPointF(self.items_dict[region]['pixel_coords'].x() - 25,
self.items_dict[region]['pixel_coords'].y()))
ap_item_label.setFont(label_font)
self.addItem(ap_item_label)
self.items_dict[region]['ap_item_label'].update({ap: ap_item_label})
def __init__(self, bounding_box, shape, label, label_pos=None, url=None, parent=None, **kwargs):
super(DmgHtmlNodeItem, self).__init__(parent, **kwargs)
self.url = url
self._incoming_edges = set()
self._outgoing_edges = set()
self._brush = QBrush(self._color)
self._label_pen = QPen()
self._label_pen.setColor(self._color)
self._label_pen.setJoinStyle(Qt.RoundJoin)
self._label_pen.setWidthF(self._label_pen_width)
self._graphics_item_pen = QPen(self._label_pen)
self._graphics_item_pen.setWidthF(self._pen_width)
self._label = QGraphicsTextItem()
self._label.setHtml(label)
label_rectangle = self._label.boundingRect()
if label_pos is None:
label_rectangle.moveCenter(bounding_box.center())
else:
label_rectangle.moveCenter(label_pos)
self._label.setPos(label_rectangle.x(), label_rectangle.y())
self.addToGroup(self._label)
self._graphics_item = ShapeFactory.create(shape, bounding_box)
if ShapeFactory.message is not None:
print ShapeFactory.message
self.addToGroup(self._graphics_item)
self._brush.setColor(self._color)
self._graphics_item_pen.setColor(self._color)
self._label_pen.setColor(self._color)
self._graphics_item.setPen(self._graphics_item_pen)
self._highlight_level = kwargs.get('highlight_level', self.HIGHLIGHT_LEVEL)
self._hovered_color = kwargs.get('hovered_color', self.HOVERED_COLOR)
self._highlighted_color = kwargs.get('highlighted_color', self.HIGHLIGHTED_COLOR)
self._highlighted_pen_width = kwargs.get('highlighted_pen_width', self.HIGHLIGHTED_PEN_WIDTH)
self._highlighted_label_pen_width = kwargs.get('highlighted_label_pen_width', self.HIGHLIGHTED_LABEL_PEN_WIDTH)
self.hover_shape = None
self.setAcceptHoverEvents(True)
def __init__(self, num_robots, robots):
super(RobotTab, self).__init__()
# Available robots from gui_config.yaml
self.robots = robots
# Set robot type by default to ground
self.agent_type = 'ground'
# Number of robots
self.num_robots = num_robots
# Set the robot name by default
self.robot_name = 'robot' + str(self.num_robots)
# Set layout for tab
self.layout = QVBoxLayout()
# Robot tab title
self.robot_label_name = QLabel(('Robot ' + str(self.num_robots)))
font = QFont()
font.setPointSize(14)
font.setBold(True)
self.robot_label_name.setFont(font)
self.layout.addWidget(self.robot_label_name)
# Robot name label and input line
self.robot_label = QLabel('Robot name')
self.layout.addWidget(self.robot_label)
self.robot_name_input = QLineEdit('robot' + str(self.num_robots))
#self.robot_name_input.editingFinished.connect(self.robot_name_changed)
self.layout.addWidget(self.robot_name_input)
# Robot model label and comboBox
self.robot_model_label = QLabel('Robot model')
self.layout.addWidget(self.robot_model_label)
self.robot_comboBox = CustomComboBox(self.num_robots - 1)
self.robot_comboBox.addItems(self.robots['Models'].keys())
self.layout.addWidget(self.robot_comboBox)
self.robot_comboBox.signalIndexChanged.connect(self.set_agent_type)
# Initial pose
self.robot_label_init = QLabel('Initial pose')
self.layout.addWidget(self.robot_label_init)
self.robot_comboBox_init = CustomComboBox(self.num_robots)
self.layout.addWidget(self.robot_comboBox_init)
# Add initial pose text item to graphic
initial_pose_textItem = QGraphicsTextItem(
'start_' + str(self.num_robots).zfill(2))
self.initial_pose = {
'start_' + str(self.num_robots).zfill(2): {
'label': 'r01',
'text_item': initial_pose_textItem
}
}
# Use Qualisys
self.robot_localization_label = QLabel('Localization')
self.layout.addWidget(self.robot_localization_label)
self.robot_localization_checkBox = QCheckBox('Use Qualisys')
self.layout.addWidget(self.robot_localization_checkBox)
# Task specifications
self.robot_label_task_title = QLabel('Task robot ' +
str(self.num_robots))
self.robot_label_task_title.setFont(font)
self.layout.addWidget(self.robot_label_task_title)
self.robot_label_hard_task = QLabel('Hard tasks')
self.layout.addWidget(self.robot_label_hard_task)
self.robot_hard_task_input = QLineEdit('([]<> r01) && ([]<> r02)')
self.layout.addWidget(self.robot_hard_task_input)
self.robot_label_soft_task = QLabel('Soft tasks')
self.layout.addWidget(self.robot_label_soft_task)
self.robot_soft_task_input = QLineEdit()
self.layout.addWidget(self.robot_soft_task_input)
# Plan display
self.robot_label_prefix = QLabel('Planner prefix robot ' +
str(self.num_robots))
self.layout.addWidget(self.robot_label_prefix)
self.robot_prefix_textbox = QTextBrowser()
self.layout.addWidget(self.robot_prefix_textbox)
self.robot_label_sufix = QLabel('Planner sufix robot ' +
str(self.num_robots))
self.layout.addWidget(self.robot_label_sufix)
self.robot_sufix_textbox = QTextBrowser()
self.layout.addWidget(self.robot_sufix_textbox)
# Current goal display
self.robot_label_current_goal = QLabel('Current goal robot ' +
str(self.num_robots))
self.layout.addWidget(self.robot_label_current_goal)
self.robot_current_goal_textbox = QTextBrowser()
self.layout.addWidget(self.robot_current_goal_textbox)
# Clear costmap button
self.robot_resend_goal_button = QPushButton('Clear costmap')
self.layout.addWidget(self.robot_resend_goal_button)
# Temporary task button
self.robot_temporary_task_button = QPushButton('Temporary task')
self.layout.addWidget(self.robot_temporary_task_button)
self.robot_temporary_task_button.clicked.connect(
self.temporary_task_button_pressed)
self.setLayout(self.layout)
# Messages for publishing pose, soft-task, hard-task and clear_costmap
self.init_pose_msg = Pose()
self.soft_task_msg = String()
self.hard_task_msg = String()
self.prefix_string = ''
self.sufix_string = ''
# Marker displaying robots name
self.label_marker_msg = Marker()
self.label_marker_msg.pose = self.init_pose_msg
self.label_marker_msg.pose.position.z = 1.0
self.label_marker_msg.text = self.robot_name
self.label_marker_msg.type = self.label_marker_msg.TEXT_VIEW_FACING
self.label_marker_msg.id = self.num_robots
self.label_marker_msg.action = self.label_marker_msg.ADD
self.label_marker_msg.scale.z = 0.5
self.label_marker_msg.color.a = 1.0
self.label_marker_msg.color.r = 0.0
self.label_marker_msg.color.g = 0.0
self.label_marker_msg.color.b = 0.0
self.label_marker_msg.header.frame_id = '/map'
# Pose msg published for planner
self.pose_msg = PoseWithCovarianceStamped()
# Init pose msg for planner
self.pose_msg.pose.pose = self.init_pose_msg
# Clear costmap and resend current goal
self.robot_resend_goal_button.clicked.connect(
self.call_clear_costmap_srvs)
self.robot_current_goal = MoveBaseActionGoal()
class DmgHtmlNodeItem(GraphItem):
HIGHLIGHT_LEVEL = 1
HOVERED_COLOR = QColor(250, 0, 0)
HIGHLIGHTED_COLOR = QColor(100, 100, 100)
HIGHLIGHTED_PEN_WIDTH = 2.0
HIGHLIGHTED_LABEL_PEN_WIDTH = 1.0
def __init__(self, bounding_box, shape, label, label_pos=None, url=None, parent=None, **kwargs):
super(DmgHtmlNodeItem, self).__init__(parent, **kwargs)
self.url = url
self._incoming_edges = set()
self._outgoing_edges = set()
self._brush = QBrush(self._color)
self._label_pen = QPen()
self._label_pen.setColor(self._color)
self._label_pen.setJoinStyle(Qt.RoundJoin)
self._label_pen.setWidthF(self._label_pen_width)
self._graphics_item_pen = QPen(self._label_pen)
self._graphics_item_pen.setWidthF(self._pen_width)
self._label = QGraphicsTextItem()
self._label.setHtml(label)
label_rectangle = self._label.boundingRect()
if label_pos is None:
label_rectangle.moveCenter(bounding_box.center())
else:
label_rectangle.moveCenter(label_pos)
self._label.setPos(label_rectangle.x(), label_rectangle.y())
self.addToGroup(self._label)
self._graphics_item = ShapeFactory.create(shape, bounding_box)
if ShapeFactory.message is not None:
print ShapeFactory.message
self.addToGroup(self._graphics_item)
self._brush.setColor(self._color)
self._graphics_item_pen.setColor(self._color)
self._label_pen.setColor(self._color)
self._graphics_item.setPen(self._graphics_item_pen)
self._highlight_level = kwargs.get('highlight_level', self.HIGHLIGHT_LEVEL)
self._hovered_color = kwargs.get('hovered_color', self.HOVERED_COLOR)
self._highlighted_color = kwargs.get('highlighted_color', self.HIGHLIGHTED_COLOR)
self._highlighted_pen_width = kwargs.get('highlighted_pen_width', self.HIGHLIGHTED_PEN_WIDTH)
self._highlighted_label_pen_width = kwargs.get('highlighted_label_pen_width', self.HIGHLIGHTED_LABEL_PEN_WIDTH)
self.hover_shape = None
self.setAcceptHoverEvents(True)
def add_incoming_edge(self, edge):
self._incoming_edges.add(edge)
def add_outgoing_edge(self, edge):
self._outgoing_edges.add(edge)
def set_color(self, color=None):
if color is None:
color = self._color
self._brush.setColor(color)
self._graphics_item_pen.setColor(color)
self._label.setDefaultTextColor(color)
self._graphics_item.setPen(self._graphics_item_pen)
def set_hover_shape(self, shape):
self.hover_shape = shape
def shape(self):
if self.hover_shape is not None:
path = QPainterPath()
path.addRect(self.hover_shape)
return path
return super(GraphItem, self).shape()
def hoverEnterEvent(self, event):
self.set_color(self._highlighted_color)
self._highlight_connections()
def hoverLeaveEvent(self, event):
self.set_color()
self._highlight_connections(False)
def _highlight_connections(self, highlighted=True):
if highlighted:
if self._highlight_level > 1:
cyclic_edges = self._incoming_edges.intersection(self._outgoing_edges)
# incoming edges in blue
incoming_nodes = set()
for incoming_edge in self._incoming_edges.difference(cyclic_edges):
incoming_edge.set_color(self.COLOR_BLUE)
if incoming_edge.from_node != self:
incoming_nodes.add(incoming_edge.from_node)
# outgoing edges in green
outgoing_nodes = set()
for outgoing_edge in self._outgoing_edges.difference(cyclic_edges):
outgoing_edge.set_color(self.COLOR_GREEN)
if outgoing_edge.to_node != self:
outgoing_nodes.add(outgoing_edge.to_node)
# incoming/outgoing edges in teal
for edge in cyclic_edges:
edge.set_color(self.COLOR_TEAL)
if self._highlight_level > 2:
cyclic_nodes = incoming_nodes.intersection(outgoing_nodes)
# incoming nodes in blue
for incoming_node in incoming_nodes.difference(cyclic_nodes):
incoming_node.set_color(self.COLOR_BLUE)
# outgoing nodes in green
for outgoing_node in outgoing_nodes.difference(cyclic_nodes):
outgoing_node.set_color(self.COLOR_GREEN)
# incoming/outgoing nodes in teal
for node in cyclic_nodes:
node.set_color(self.COLOR_TEAL)
else:
if self._highlight_level > 1:
for incoming_edge in self._incoming_edges:
incoming_edge.set_color()
if self.highlight_level > 2 and incoming_edge.from_node != self:
incoming_edge.from_node.set_color()
for outgoing_edge in self._outgoing_edges:
outgoing_edge.set_color()
if self.highlight_level > 2 and outgoing_edge.to_node != self:
outgoing_edge.to_node.set_color()
def highlight(self, highlighted=True):
if highlighted:
self._graphics_item_pen.setWidthF(self._highlighted_pen_width)
else:
self._graphics_item_pen.setWidthF(self._pen_width)
self._graphics_item.setPen(self._graphics_item_pen)
class DmgHtmlNodeItem(GraphItem):
HIGHLIGHT_LEVEL = 1
HOVERED_COLOR = QColor(250, 0, 0)
HIGHLIGHTED_COLOR = QColor(100, 100, 100)
HIGHLIGHTED_PEN_WIDTH = 2.0
HIGHLIGHTED_LABEL_PEN_WIDTH = 1.0
def __init__(self,
bounding_box,
shape,
label,
label_pos=None,
url=None,
parent=None,
**kwargs):
super(DmgHtmlNodeItem, self).__init__(parent, **kwargs)
self.url = url
self._incoming_edges = set()
self._outgoing_edges = set()
self._brush = QBrush(self._color)
self._label_pen = QPen()
self._label_pen.setColor(self._color)
self._label_pen.setJoinStyle(Qt.RoundJoin)
self._label_pen.setWidthF(self._label_pen_width)
self._graphics_item_pen = QPen(self._label_pen)
self._graphics_item_pen.setWidthF(self._pen_width)
self._label = QGraphicsTextItem()
self._label.setHtml(label)
label_rectangle = self._label.boundingRect()
if label_pos is None:
label_rectangle.moveCenter(bounding_box.center())
else:
label_rectangle.moveCenter(label_pos)
self._label.setPos(label_rectangle.x(), label_rectangle.y())
self.addToGroup(self._label)
self._graphics_item = ShapeFactory.create(shape, bounding_box)
if ShapeFactory.message is not None:
print ShapeFactory.message
self.addToGroup(self._graphics_item)
self._brush.setColor(self._color)
self._graphics_item_pen.setColor(self._color)
self._label_pen.setColor(self._color)
self._graphics_item.setPen(self._graphics_item_pen)
self._highlight_level = kwargs.get('highlight_level',
self.HIGHLIGHT_LEVEL)
self._hovered_color = kwargs.get('hovered_color', self.HOVERED_COLOR)
self._highlighted_color = kwargs.get('highlighted_color',
self.HIGHLIGHTED_COLOR)
self._highlighted_pen_width = kwargs.get('highlighted_pen_width',
self.HIGHLIGHTED_PEN_WIDTH)
self._highlighted_label_pen_width = kwargs.get(
'highlighted_label_pen_width', self.HIGHLIGHTED_LABEL_PEN_WIDTH)
self.hover_shape = None
self.setAcceptHoverEvents(True)
def add_incoming_edge(self, edge):
self._incoming_edges.add(edge)
def add_outgoing_edge(self, edge):
self._outgoing_edges.add(edge)
def set_color(self, color=None):
if color is None:
color = self._color
self._brush.setColor(color)
self._graphics_item_pen.setColor(color)
self._label.setDefaultTextColor(color)
self._graphics_item.setPen(self._graphics_item_pen)
def set_hover_shape(self, shape):
self.hover_shape = shape
def shape(self):
if self.hover_shape is not None:
path = QPainterPath()
path.addRect(self.hover_shape)
return path
return super(GraphItem, self).shape()
def hoverEnterEvent(self, event):
self.set_color(self._highlighted_color)
self._highlight_connections()
def hoverLeaveEvent(self, event):
self.set_color()
self._highlight_connections(False)
def _highlight_connections(self, highlighted=True):
if highlighted:
if self._highlight_level > 1:
cyclic_edges = self._incoming_edges.intersection(
self._outgoing_edges)
# incoming edges in blue
incoming_nodes = set()
for incoming_edge in self._incoming_edges.difference(
cyclic_edges):
incoming_edge.set_color(self.COLOR_BLUE)
if incoming_edge.from_node != self:
incoming_nodes.add(incoming_edge.from_node)
# outgoing edges in green
outgoing_nodes = set()
for outgoing_edge in self._outgoing_edges.difference(
cyclic_edges):
outgoing_edge.set_color(self.COLOR_GREEN)
if outgoing_edge.to_node != self:
outgoing_nodes.add(outgoing_edge.to_node)
# incoming/outgoing edges in teal
for edge in cyclic_edges:
edge.set_color(self.COLOR_TEAL)
if self._highlight_level > 2:
cyclic_nodes = incoming_nodes.intersection(outgoing_nodes)
# incoming nodes in blue
for incoming_node in incoming_nodes.difference(
cyclic_nodes):
incoming_node.set_color(self.COLOR_BLUE)
# outgoing nodes in green
for outgoing_node in outgoing_nodes.difference(
cyclic_nodes):
outgoing_node.set_color(self.COLOR_GREEN)
# incoming/outgoing nodes in teal
for node in cyclic_nodes:
node.set_color(self.COLOR_TEAL)
else:
if self._highlight_level > 1:
for incoming_edge in self._incoming_edges:
incoming_edge.set_color()
if self.highlight_level > 2 and incoming_edge.from_node != self:
incoming_edge.from_node.set_color()
for outgoing_edge in self._outgoing_edges:
outgoing_edge.set_color()
if self.highlight_level > 2 and outgoing_edge.to_node != self:
outgoing_edge.to_node.set_color()
def highlight(self, highlighted=True):
if highlighted:
self._graphics_item_pen.setWidthF(self._highlighted_pen_width)
else:
self._graphics_item_pen.setWidthF(self._pen_width)
self._graphics_item.setPen(self._graphics_item_pen)