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 _is_robot_clicked(self, field_point):
# ロボットをクリックしたか判定する
# 消えたロボットは対照外
is_clicked = False
replace_id = 0
is_yellow = False
for robot in self._robot_info['blue']:
if robot.disappeared:
continue
robot_point = QPointF(robot.pose.x, robot.pose.y)
is_clicked = self._is_clicked(field_point, robot_point)
if is_clicked:
is_yellow = False
return is_clicked, robot.robot_id, is_yellow
for robot in self._robot_info['yellow']:
if robot.disappeared:
continue
robot_point = QPointF(robot.pose.x, robot.pose.y)
is_clicked = self._is_clicked(field_point, robot_point)
if is_clicked:
is_yellow = True
return is_clicked, robot.robot_id, is_yellow
return is_clicked, replace_id, is_yellow
def __init__(self, parent=None):
super(QMapAnnotation, self).__init__()
self._parent = parent
self.destroyed.connect(self.close)
self._load_ui()
self._scene = self.map_view._scene
self._viz_markers_pose = None
self._viz_marker_items = {}
self._viz_marker_items["annotations"] = []
self._viz_marker_items["new_annotations"] = []
self._viz_marker_polygon = QPolygonF(
[QPointF(-4, 4), QPointF(-4, -4),
QPointF(12, 0)])
self._callback = {}
self._callback['save_annotation'] = None
self._callback['load_world'] = None
self._callback['add_annotation'] = None
self._wc = None
self._init_events()
self._drawing_objects = {}
self._init_variables_for_drawing()
self._init_variableS_for_list()
def c(msg):
if not (msg.header.frame_id == '/map' or msg.header.frame_id == ''):
try:
self._tf.waitForTransform(msg.header.frame_id, '/map', rospy.Time(), rospy.Duration(10))
points_stamped = []
for pt in msg.polygon.points:
ps = PointStamped()
ps.header.frame_id = msg.header.frame_id
ps.point.x = pt.x
ps.point.y = pt.y
points_stamped.append(ps)
trans_pts = [self._tf.transformPoint('/map', pt).point for pt in points_stamped]
except tf.Exception:
rospy.logerr("TF Error")
trans_pts = []
else:
trans_pts = [pt for pt in msg.polygon.points]
if len(trans_pts) > 0:
pts = [QPointF(pt.x / self.resolution, pt.y / self.resolution) for pt in trans_pts]
close = trans_pts[0]
pts.append(QPointF(close.x / self.resolution, close.y / self.resolution))
poly.path = QPolygonF(pts)
self.polygon_changed.emit(name)
def addArrow(self, startPoint=QPointF(), endPoint=QPointF(), pen=QPen()):
alpha = 5 * pi / 6
arrowLength = 10
theta = atan2((endPoint.y() - startPoint.y()),
(endPoint.x() - startPoint.x()))
gamma1 = theta + alpha
gamma2 = theta - alpha
arrowPoint_1 = QPointF(endPoint.x() + arrowLength * cos(gamma1),
endPoint.y() + arrowLength * sin(gamma1))
arrowPoint_2 = QPointF(endPoint.x() + arrowLength * cos(gamma2),
endPoint.y() + arrowLength * sin(gamma2))
line_0 = QLineF(startPoint, endPoint)
line_1 = QLineF(endPoint, arrowPoint_1)
line_2 = QLineF(endPoint, arrowPoint_2)
line_item_0 = QGraphicsLineItem(line_0)
line_item_0.setPen(pen)
line_item_1 = QGraphicsLineItem(line_1)
line_item_1.setPen(pen)
line_item_2 = QGraphicsLineItem(line_2)
line_item_2.setPen(pen)
arrowItems = [line_item_0, line_item_1, line_item_2]
self.addItem(line_item_0)
self.addItem(line_item_1)
self.addItem(line_item_2)
return arrowItems
def _isRobotClicked(self, real_pos):
is_clicked = False
replace_id = 0
is_friend = False
for robot_id in self.friendsIDArray.data:
posX = self.friendOdoms[robot_id].pose.pose.position.x
posY = self.friendOdoms[robot_id].pose.pose.position.y
robot_pos = QPointF(posX, posY)
is_clicked = self._isClicked(real_pos, robot_pos)
if is_clicked:
is_friend = True
return is_clicked, robot_id, is_friend
for robot_id in self.enemyIDArray.data:
posX = self.enemyOdoms[robot_id].pose.pose.position.x
posY = self.enemyOdoms[robot_id].pose.pose.position.y
robot_pos = QPointF(posX, posY)
is_clicked = self._isClicked(real_pos, robot_pos)
if is_clicked:
is_friend = False
return is_clicked, robot_id, is_friend
return is_clicked, replace_id, is_friend
def _replace_robot_angle(self, mouse_pos):
# ロボット角度のReplacement
field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y())
# ロボット角度をradiansからdegreesに変換する
robot_point = QPointF()
if self._replace_is_yellow:
robot_point = QPointF(
self._robot_info['yellow'][self._replace_id].pose.x,
self._robot_info['yellow'][self._replace_id].pose.y)
else:
robot_point = QPointF(
self._robot_info['blue'][self._replace_id].pose.x,
self._robot_info['blue'][self._replace_id].pose.y)
robot = ReplaceRobot()
robot.x = robot_point.x()
robot.y = robot_point.y()
robot.dir = math.degrees(self._to_angle(robot_point, field_point))
robot.id = self._replace_id
robot.yellowteam = self._replace_is_yellow
robot.turnon = True
if self._invert_side:
robot.x *= -1.0
robot.y *= -1.0
robot.dir += 180
replacements = Replacements()
replacements.robots.append(robot)
self._pub_replace.publish(replacements)
self._replacement_target['robot_angle'] = False
def get_cluster_node(self, node):
# let pydot imitate pygraphviz api
attr = {}
for name in node.get_attributes().iterkeys():
value = get_unquoted(node, name)
attr[name] = value
obj_dic = node.__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)
node.attr = attr
name = None
if 'label' in node.attr:
name = node.attr['label']
elif 'name' in node.attr:
name = node.attr['name']
else:
print("Error, no label defined for node with attr: %s" % node.attr)
return None
if name is None:
# happens on Lucid pygraphviz version
print(
"Error, label is None for node %s, pygraphviz version may be too old."
% node)
bb_width = node.attr['width']
bb_height = node.attr['height']
bounding_box = QRectF(0, 0,
POINTS_PER_INCH * float(bb_width) - 1.0,
POINTS_PER_INCH * float(bb_height) - 1.0)
# print bounding_box
pos = (0, 0)
if 'pos' in node.attr:
pos = node.attr['pos'].split(',')
bounding_box.moveCenter(QPointF(float(pos[0]), -float(pos[1])))
label_pos = QPointF(bounding_box.center().x(),
bounding_box.top() + LABEL_HEIGHT / 2)
color = QColor(node.attr['color']) if 'color' in node.attr else None
url = node.attr['URL'] if 'URL' in node.attr else 'N/A'
label = node.attr['label'] if 'label' in node.attr else name
graph_node_item = self._factory.create_node(bounding_box=bounding_box,
shape='box',
label=label,
label_pos=label_pos,
url=url,
cluster=True)
return graph_node_item
def addCoordinateSystem(self, origin=QPointF(), angle=0.0):
XAxis = QPointF(origin.x() + 100, origin.y())
YAxis = QPointF(origin.x(), origin.y() - 100)
self.addArrow(origin, XAxis)
self.addArrow(origin, YAxis)
XLabel = self.addText('X', QFont())
XLabel.setPos(XAxis)
YLabel = self.addText('Y', QFont())
YLabel.setPos(YAxis)
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 itemChange(self, change, value):
if change == QGraphicsItem.ItemPositionChange:
min_starttime = self._track.starttime
max_starttime = 1000000000.0
new_starttime = min(max(min_starttime, round(value.x(), self._precision)), max_starttime)
# check for track change
new_track = self.scene().track_at(value.y() + self._track.height / 2)
if new_track is None or new_track.track_type() != self._track.track_type():
new_track = self._track
# prevent overlap with the next clip
next_clip = new_track.next_clip(new_starttime)
if next_clip is not None and next_clip is not self:
max_starttime = next_clip.starttime() - self.duration() # move only up to the next clip
new_starttime = min(new_starttime, max_starttime)
# prevent overlap with the previous clip
previous_clip = new_track.previous_clip(new_starttime)
if previous_clip is not None and previous_clip is not self:
min_starttime = previous_clip.endtime() # move only up to the previous clip
new_starttime = max(new_starttime, min_starttime)
if min_starttime <= new_starttime <= max_starttime:
self._track = new_track
return QPointF(new_starttime, self._track._y)
return self.pos()
return super(TimelineClip, self).itemChange(change, value)
def drawAngleReplacement(self, painter):
robot_pos = QPointF()
if self._replace_is_friend:
robot_pos.setX(
self.friendOdoms[self._replace_id].pose.pose.position.x)
robot_pos.setY(
self.friendOdoms[self._replace_id].pose.pose.position.y)
else:
robot_pos.setX(
self.enemyOdoms[self._replace_id].pose.pose.position.x)
robot_pos.setY(
self.enemyOdoms[self._replace_id].pose.pose.position.y)
currentPos = self.convertToRealWorld(self._current_mouse_pos.x(),
self._current_mouse_pos.y())
angle = math.degrees(self._to_angle(robot_pos, currentPos))
robotPoint = self.convertToDrawWorld(robot_pos.x(), robot_pos.y())
currentPoint = self.convertToDrawWorld(currentPos.x(), currentPos.y())
painter.setPen(QPen(Qt.blue, 2))
painter.drawLine(robotPoint, currentPoint)
text = "[" + str(round(angle, 2)) + "]"
painter.setPen(Qt.black)
painter.drawText(currentPoint, text)
def paint(self, painter, option, widget):
painter.setFont(self._time_font)
painter.fillRect(0, 0, SandtrayItem.length * SandtrayItem.scale,
SandtrayItem.width * SandtrayItem.scale,
painter.background())
for color, polys in self._zones.items():
painter.setPen(QPen(color, 2))
for poly in polys:
painter.drawPolygon(QPolygonF(poly))
painter.setBrush(QBrush(self._fg_color))
painter.setPen(QPen(self._border_color, 1))
painter.drawRect(0, 0, SandtrayItem.length * SandtrayItem.scale,
SandtrayItem.width * SandtrayItem.scale)
for label, pos in self._items.items():
x, y = pos[0] * SandtrayItem.scale, pos[1] * SandtrayItem.scale
painter.drawText(x - 10, y - 10, label)
if "cube" in label:
painter.setBrush(QBrush(self._cube_color))
painter.drawRect(x - 5, y - 5, 10, 10)
else:
painter.setBrush(QBrush(self._item_color))
painter.drawEllipse(QPointF(x, y), 10, 10)
def addEdgeItem(self,
edge,
nodes,
edges,
highlight_level,
same_label_siblings=False):
"""
adds EdgeItem by data in edge to edges
:param same_label_siblings: if true, edges with same label will be considered siblings (collective highlighting)
"""
# let pydot imitate pygraphviz api
attr = {}
for name in edge.get_attributes().iterkeys():
value = get_unquoted(edge, name)
attr[name] = value
edge.attr = attr
if 'style' in edge.attr:
if edge.attr['style'] == 'invis':
return
label = edge.attr.get('label', None)
label_pos = edge.attr.get('lp', None)
label_center = None
if label_pos is not None:
label_pos = label_pos.split(',')
label_center = QPointF(float(label_pos[0]), -float(label_pos[1]))
# try pydot, fallback for pygraphviz
source_node = edge.get_source() if hasattr(edge,
'get_source') else edge[0]
destination_node = edge.get_destination() if hasattr(
edge, 'get_destination') else edge[1]
# create edge with from-node and to-node
edge_pos = (0, 0)
if 'pos' in edge.attr:
edge_pos = edge.attr['pos']
if label is not None:
label = label.decode('string_escape')
edge_item = EdgeItem(highlight_level=highlight_level,
spline=edge_pos,
label_center=label_center,
label=label,
from_node=nodes[source_node],
to_node=nodes[destination_node])
if same_label_siblings:
if label is None:
# for sibling detection
label = "%s_%s" % (source_node, destination_node)
# symmetrically add all sibling edges with same label
if label in edges:
for sibling in edges[label]:
edge_item.add_sibling_edge(sibling)
sibling.add_sibling_edge(edge_item)
if label not in edges:
edges[label] = []
edges[label].append(edge_item)
def _replaceRobotAngle(self, mouse_pos):
real_pos = self.convertToRealWorld(mouse_pos.x(), mouse_pos.y())
robot_pos = QPointF()
if self._replace_is_friend:
robot_pos.setX(self.friendOdoms[self._replace_id].pose.pose.position.x)
robot_pos.setY(self.friendOdoms[self._replace_id].pose.pose.position.y)
else:
robot_pos.setX(self.enemyOdoms[self._replace_id].pose.pose.position.x)
robot_pos.setY(self.enemyOdoms[self._replace_id].pose.pose.position.y)
is_yellow = True
if self._replace_is_friend and self.friend_color == 'blue':
is_yellow = False
elif not self._replace_is_friend and self.friend_color == 'yellow':
is_yellow = False
replace = ReplaceRobot()
replace.robot_id = self._replace_id
replace.is_yellow = is_yellow
replace.pos_x = robot_pos.x()
replace.pos_y = robot_pos.y()
replace.dir = math.degrees(self._to_angle(robot_pos, real_pos))
replace.turn_on = True
self._pub_replace_robot.publish(replace)
self._is_robotangle_replacement = False
def slot_redraw(self):
"""
Gets called either when new msg comes in or when marker is moved by user.
"""
self._parent._scene.clear()
is_enabled = self.isEnabled()
qsize = self.size()
width_tl = qsize.width()
height_tl = qsize.height()
length_tl = ((self._max_num_seconds + 1) - self._min_num_seconds)
len_queue = length_tl
#w = float(width_tl / len_queue) # This returns always 0 in
# the 1st decimal point, which causes
# timeline not fit nicely.
w = width_tl / float(len_queue)
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 For adding gradation to the cell color. Not used yet.
end_color = QColor(0.5 * QColor('red').value(),
0.5 * QColor('green').value(),
0.5 * QColor('blue').value())
rect = self._parent._scene.addRect(w * i, 0, w, h, QColor('white'),
qcolor)
rospy.logdebug(
'TimelineView.slot_redraw #%d th loop w=%s width_tl=%s', i, w,
width_tl)
# Setting marker.
#xpos_marker = ((self._xpos_marker - 1) * width_cell +
#(width_cell / 2.0) -
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)
# self._timeline_marker.paint(painter, qrect)
rospy.logdebug(' slot_redraw xpos_marker(int)=%s length_tl=%s',
int(xpos_marker), length_tl)
def transformPointToPixelCoordinates(pt, map, image_size):
map_point_f = ((pt - QPointF(map.map.info.origin.position.x,
map.map.info.origin.position.y)) *
(1.0 / map.map.info.resolution))
map_point = QPoint(int(map_point_f.x()), int(map_point_f.y()))
map_size = QSize(map.map.info.width, map.map.info.height)
scaled_point = scalePoint(map_point, map_size, image_size)
return QPoint(scaled_point.x(), image_size.height() - scaled_point.y() - 1)
def getNodeItemForNode(self, node, highlight_level):
"""
returns a pyqt NodeItem object, or None in case of error or invisible style
"""
# let pydot imitate pygraphviz api
# attr = {}
# for name in node.get_attributes().iterkeys():
# value = get_unquoted(node, name)
# attr[name] = value
# obj_dic = node.__getattribute__("obj_dict")
# for name in obj_dic:
# if name not in ['attributes', 'parent_graph'] and obj_dic[name] is not None:
# attr[name] = get_unquoted(obj_dic, name)
# node.attr = attr
if 'style' in node.attr:
if node.attr['style'] == 'invis':
return None
color = QColor(node.attr['color']) if 'color' in node.attr else None
name = None
if 'label' in node.attr:
name = node.attr['label']
elif 'name' in node.attr:
name = node.attr['name']
else:
print("Error, no label defined for node with attr: %s" % node.attr)
return None
if name is None:
# happens on Lucid pygraphviz version
print("Error, label is None for node %s, pygraphviz version may be too old." % node)
else:
name = name.decode('string_escape')
# decrease rect by one so that edges do not reach inside
bb_width = len(name) / 5
if 'width' in node.attr:
bb_width = node.attr['width']
bb_height = 1.0
if 'width' in node.attr:
bb_height = node.attr['height']
bounding_box = QRectF(0, 0, POINTS_PER_INCH * float(bb_width) - 1.0, POINTS_PER_INCH * float(bb_height) - 1.0)
pos = (0, 0)
if 'pos' in node.attr:
pos = node.attr['pos'].split(',')
bounding_box.moveCenter(QPointF(float(pos[0]), -float(pos[1])))
node_item = NodeItem(highlight_level=highlight_level,
bounding_box=bounding_box,
label=name,
shape=node.attr.get('shape', 'ellipse'),
color=color,
tooltip=node.attr.get('tooltip', None)
# parent=None,
# label_pos=None
)
# node_item.setToolTip(self._generate_tool_tip(node.attr.get('URL', None)))
return node_item
def mouseReleaseEvent(self, event):
if self._can_replace:
self._can_replace = False
self._replace_func(event.posF())
else:
self.trans += self.mouseTrans
self.mouseTrans = QPointF(0.0, 0.0)
self.update()
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_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 mouseReleaseEvent(self, event):
# マウスのドラッグ操作で描画領域を移動する
# Replacementを行うときは描画領域を移動しない
if self._do_replacement:
self._do_replacement = False
self._replace_func(event.localPos())
else:
self._trans += self._mouse_trans
self._mouse_trans = QPointF(0.0, 0.0)
self.update()
def _get_zones(self, msg):
zones = {}
for marker in msg.markers:
polygon = []
color = QColor(255*marker.color.r, 255*marker.color.g, 255*marker.color.b, 255*marker.color.a)
for p in marker.points:
polygon.append(QPointF(p.x * SandtrayItem.scale, -p.y * SandtrayItem.scale))
zones.setdefault(color, []).append(polygon)
return zones
def __init__(self, parent=None):
super(QSlamWidget, self).__init__()
self._parent = parent
self._slam_widget_interface = None
self._load_ui()
self._init_events()
self.destroyed.connect(self.close)
self._scene = self.map_view._scene
self._robot_pose = None
self._robot_pose_item = None
self._robot_polygon = QPolygonF(
[QPointF(-4, 4), QPointF(-4, -4),
QPointF(12, 0)])
self._scan = None
self._scan_items = []
self._callback = {}
self._callback['save_map'] = None
def _is_ball_clicked(self, field_point):
# ボールをクリックしたか判定する
# ボールが消えていれば判定しない
if self._ball_info.disappeared:
return False
pos_x = self._ball_info.pose.x
pos_y = self._ball_info.pose.y
ball_pos = QPointF(pos_x, pos_y)
return self._is_clicked(field_point, ball_pos)
def eventFilter(self, _, event):
if event.type() == QEvent.MouseButtonRelease:
x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x())
y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y())
self._last_click_coordinates = QPointF(x, y)
self.limits_changed.emit()
elif event.type() == QEvent.MouseMove:
x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x())
y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y())
coords = QPointF(x, y)
if self._picker.isActive() and self._last_click_coordinates is not None:
toolTip = 'origin x: %.5f, y: %.5f' % (
self._last_click_coordinates.x(), self._last_click_coordinates.y())
delta = coords - self._last_click_coordinates
toolTip += '\ndelta x: %.5f, y: %.5f\nlength: %.5f' % (
delta.x(), delta.y(), QVector2D(delta).length())
else:
toolTip = 'buttons\nleft: measure\nmiddle: move\nright: zoom x/y\nwheel: zoom y'
self.setToolTip(toolTip)
self.mouseCoordinatesChanged.emit(coords)
return False
def _draw_playhead(self, painter):
"""
Draw a line and 2 triangles to denote the current position being viewed
:param painter: ,''QPainter''
"""
px = self.map_stamp_to_x(self.playhead.to_sec())
pw, ph = self._playhead_pointer_size
# Line
painter.setPen(QPen(self._playhead_color))
painter.setBrush(QBrush(self._playhead_color))
painter.drawLine(px, self._history_top - 1, px,
self._history_bottom + 2)
# Upper triangle
py = self._history_top - ph
painter.drawPolygon(
QPolygonF([
QPointF(px, py + ph),
QPointF(px + pw, py),
QPointF(px - pw, py)
]))
# Lower triangle
py = self._history_bottom + 1
painter.drawPolygon(
QPolygonF([
QPointF(px, py),
QPointF(px + pw, py + ph),
QPointF(px - pw, py + ph)
]))
painter.setBrush(self._default_brush)
painter.setPen(self._default_pen)
def _calcuReplaceVelocity(self, from_pos, to_pos):
diff = to_pos - from_pos
diff_size = math.hypot(diff.x(), diff.y())
velocity_size = diff_size * self._VEL_GAIN
if velocity_size > self._VEL_MAX:
velocity_size = self._VEL_MAX
angle = math.atan2(diff.y(), diff.x())
velocity = QPointF(velocity_size * math.cos(angle),
velocity_size * math.sin(angle))
return velocity
def _replacement_velocity(self, from_point, to_point):
# Replacementのボール速度を計算する
diff_point = to_point - from_point
diff_norm = math.hypot(diff_point.x(), diff_point.y())
velocity_norm = diff_norm * self._REPLACE_BALL_VELOCITY_GAIN
if velocity_norm > self._REPLACE_MAX_BALL_VELOCITY:
velocity_norm = self._REPLACE_MAX_BALL_VELOCITY
angle = math.atan2(diff_point.y(), diff_point.x())
velocity = QPointF(velocity_norm * math.cos(angle),
velocity_norm * math.sin(angle))
return velocity
def paintEvent(self, event):
painter = QPainter(self)
#painter.begin(self)
# puts the arrow in the middle
painter.translate(self.width() / 2, self.height() / 2)
painter.rotate(self.angleRobo + self.angleVel)
line = QLineF(0, 0, self.width() / 2 - 3, 0)
headSize = min(self.width() / 20, 4)
points = QPolygonF()
points.append(QPointF(self.width() / 2 - headSize * 2, headSize))
points.append(QPointF(self.width() / 2 - headSize * 2, -headSize))
points.append(QPointF(self.width() / 2 - 3, 0))
pen = QPen(self.color, 2)
painter.setPen(pen)
brush = QBrush(self.color)
painter.setBrush(brush)
painter.drawLine(line)
painter.drawConvexPolygon(points)
