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)