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_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 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 __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_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, 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()