class RectFromCenterFixedTool(QgsMapTool): msgbar = pyqtSignal(str) rbFinished = pyqtSignal(object) rb_reset_signal = pyqtSignal() def __init__(self, canvas, x_length=0.0, y_length=0.0): QgsMapTool.__init__(self, canvas) self.canvas = canvas self.nbPoints = 0 self.rb = None self.mCtrl = None self.xc, self.yc, self.p2 = None, None, None self.distance = QgsDistanceArea() self.distance.setSourceCrs(QgsCoordinateReferenceSystem(4326), QgsProject.instance().transformContext()) self.distance.setEllipsoid('WGS84') self.x_length = x_length self.y_length = y_length self.diagonal = sqrt(self.x_length / 2 * self.x_length / 2 + self.y_length / 2 * self.y_length / 2) # our own fancy cursor self.cursor = QCursor( QPixmap([ "16 16 3 1", " c None", ". c #FF0000", "+ c #17a51a", " ", " +.+ ", " ++.++ ", " +.....+ ", " +. . .+ ", " +. . .+ ", " +. . .+ ", " ++. . .++", " ... ...+... ...", " ++. . .++", " +. . .+ ", " +. . .+ ", " ++. . .+ ", " ++.....+ ", " ++.++ ", " +.+ " ])) self.curr_geom = None self.last_currpoint = None self.curr_angle = 0.0 self.total_angle = 0.0 self.rectangle = Rectangle() self.angle_ini_rot = 0.0 self.ini_geom = None def keyPressEvent(self, event): if event.key() == Qt.Key_Control: self.mCtrl = True self.point_ini_rot = self.toMapCoordinates( self.canvas.mouseLastXY()) self.angle_ini_rot = self.curr_angle def keyReleaseEvent(self, event): if event.key() == Qt.Key_Control: self.mCtrl = False if event.key() == Qt.Key_Escape: self.nbPoints = 0 self.xc, self.yc, self.p2 = None, None, None if self.rb: self.rb.reset(True) self.rb = None self.canvas.refresh() self.rb_reset_signal.emit() return def changegeomSRID(self, geom): layer = self.canvas.currentLayer() layerCRSSrsid = layer.crs().srsid() projectCRSSrsid = QgsMapSettings().destinationCrs().srsid() if layerCRSSrsid != projectCRSSrsid: g = QgsGeometry.fromPoint(geom) g.transform(QgsCoordinateTransform(projectCRSSrsid, layerCRSSrsid)) retPoint = g.asPoint() else: retPoint = geom return retPoint def canvasPressEvent(self, event): layer = self.canvas.currentLayer() if self.nbPoints == 0: color = QColor(255, 0, 0, 128) if self.rb: self.rb.reset() self.rb = None self.rb = QgsRubberBand(self.canvas, True) self.rb.setColor(color) self.rb.setWidth(1) self.canvas.refresh() self.rb_reset_signal.emit() point = self.toLayerCoordinates(layer, event.pos()) pointMap = self.toMapCoordinates(layer, point) if self.nbPoints == 0: self.xc = pointMap.x() self.yc = pointMap.y() if self.x_length != 0: self.diagonal = sqrt(self.x_length / 2 * self.x_length / 2 + self.y_length / 2 * self.y_length / 2) self.p2 = self.distance.computeSpheroidProject( QgsPointXY(self.xc, self.yc), self.diagonal, atan2(self.y_length / 2, self.x_length / 2)) else: self.x_bearing = pointMap.x() self.y_bearing = pointMap.y() self.bearing = self.distance.bearing(QgsPointXY(self.xc, self.yc), pointMap) self.nbPoints += 1 if self.nbPoints == 2: self.nbPoints = 0 self.xc, self.yc, self.p2 = None, None, None self.last_currpoint = None self.rbFinished.emit(self.curr_geom) self.curr_geom = None self.curr_angle = 0.0 self.total_angle = 0.0 if self.rb: return def canvasMoveEvent(self, event): if not self.rb or not self.xc or not self.yc: return currpoint = self.toMapCoordinates(event.pos()) self.msgbar.emit( "Hold Ctrl to adjust track orientation. Click when finished.") if not self.mCtrl: if self.last_currpoint is None: self.last_currpoint = self.p2 self.curr_geom = self.rectangle.get_rect_from_center( QgsPointXY(self.xc, self.yc), self.last_currpoint, ) self.ini_geom = self.curr_geom self.curr_geom = self.rectangle.get_rect_projection( self.curr_geom, QgsPointXY(self.xc, self.yc), self.x_length, self.y_length) elif self.ini_geom is not None: if self.last_currpoint is None: self.last_currpoint = currpoint self.curr_geom, self.curr_angle = self.rectangle.get_rect_rotated( self.ini_geom, QgsPointXY(self.xc, self.yc), currpoint, self.point_ini_rot, self.angle_ini_rot) self.last_currpoint = currpoint self.curr_geom = self.rectangle.get_rect_projection( self.curr_geom, QgsPointXY(self.xc, self.yc), self.x_length, self.y_length) if self.curr_geom is not None: self.rb.setToGeometry(self.curr_geom, None) def show_settings_warning(self): pass def activate(self): self.canvas.setCursor(self.cursor) def deactivate(self): self.nbPoints = 0 self.xc, self.yc, self.x_p2, self.y_p2 = None, None, None, None if self.rb: self.rb.reset(True) self.rb = None self.canvas.refresh() def is_zoom_tool(self): return False def is_transient(self): return False def is_edit_tool(self): return True
class Rectangle: def __init__(self): self.distance = QgsDistanceArea() self.distance.setSourceCrs(QgsCoordinateReferenceSystem(4326), QgsProject.instance().transformContext()) self.distance.setEllipsoid('WGS84') def get_rect_from_center(self, pc, p2, angle=0.0): """ Creates a rectangle from a center point, a corner point and an angle :param pc: center point of the geometry :param p2: a corner of the geometry :param angle: angle of the geometry, 0 by default. In radians :return: 4 points geometry """ if angle == 0: x_offset = abs(p2.x() - pc.x()) y_offset = abs(p2.y() - pc.y()) pt1 = QgsPointXY(pc.x() - x_offset, pc.y() - y_offset) pt2 = QgsPointXY(pc.x() - x_offset, pc.y() + y_offset) pt3 = QgsPointXY(pc.x() + x_offset, pc.y() + y_offset) pt4 = QgsPointXY(pc.x() + x_offset, pc.y() - y_offset) geom = QgsGeometry.fromPolygonXY([[pt1, pt2, pt3, pt4]]) else: x_offset = (cos(angle) * (p2.x() - pc.x()) + sin(angle) * (p2.y() - pc.y())) y_offset = -(-sin(angle) * (p2.x() - pc.x()) + cos(angle) * (p2.y() - pc.y())) pt1 = QgsPointXY(pc.x() - x_offset, pc.y() - y_offset) pt2 = QgsPointXY(pc.x() - x_offset, pc.y() + y_offset) pt3 = QgsPointXY(pc.x() + x_offset, pc.y() + y_offset) pt4 = QgsPointXY(pc.x() + x_offset, pc.y() - y_offset) geom = QgsGeometry.fromPolygonXY([[pt1, pt2, pt3, pt4]]) return geom def get_rect_rotated(self, geom, cp, ep=QgsPointXY(0, 0), ip=QgsPointXY(0, 0), delta=0): """ Rotates a geometry by some delta + the angle between 2 points and the center point of the geometry :param geom: geometry to be rotated :param cp: center point of the geometry :param ep: point marking the end of the rotation :param ip: point marking the beginning of the rotation :param delta: extra angle of rotation in radians :return: geometry rotated and total angle of rotation """ angle_1 = self.distance.bearing(cp, ep) angle_2 = self.distance.bearing(cp, ip) angle_rotation = delta + (angle_2 - angle_1) coords = [] ring = [] for i in geom.asPolygon(): for k in i: ini_point = QgsPointXY(k.x() - cp.x(), k.y() - cp.y()) end_point = rotate_point(ini_point, angle_rotation) p3 = QgsPointXY(cp.x() + end_point.x(), cp.y() + end_point.y()) ring.append(p3) coords.append(ring) ring = [] geom = QgsGeometry().fromPolygonXY(coords) return geom, angle_rotation def get_rect_by3_points(self, p1, p2, p3, length=0): angle_exist = self.distance.bearing(p1, p2) side = calc_is_collinear(p1, p2, p3) # check if x_p2 > x_p1 and inverse side if side == 0: return None if length == 0: length = self.distance.measureLine(p2, p3) p3 = self.distance.computeSpheroidProject( p2, length, angle_exist + radians(90) * side) p4 = self.distance.computeSpheroidProject( p1, length, angle_exist + radians(90) * side) geom = QgsGeometry.fromPolygonXY([[p1, p2, p3, p4]]) return geom def get_rect_projection(self, rect_geom, cp, x_length=0, y_length=0): """ Transforms the rectangle geometry to its projection on the real world, making all its angles 90º :param rect_geom: 4 point geometry :param cp: central point of the geometry :param x_length: distance between first and second points of the rect_geom :param y_length: distance between second and third points of the rect_geom :return: geometry projected to map """ if x_length == 0 and y_length == 0: proj_geom = self.get_rect_by3_points(rect_geom.asPolygon()[0][0], rect_geom.asPolygon()[0][1], rect_geom.asPolygon()[0][2]) else: point_two = endpoint( rect_geom.asPolygon()[0][0], x_length, degrees( self.distance.bearing(rect_geom.asPolygon()[0][0], rect_geom.asPolygon()[0][1]))) point_three = endpoint( rect_geom.asPolygon()[0][0], y_length, degrees( self.distance.bearing(rect_geom.asPolygon()[0][1], rect_geom.asPolygon()[0][3]))) proj_geom = self.get_rect_by3_points(rect_geom.asPolygon()[0][0], point_two, point_three, y_length) if proj_geom is not None: p1 = proj_geom.asPolygon()[0][0] p2 = proj_geom.asPolygon()[0][1] p3 = proj_geom.asPolygon()[0][2] p4 = proj_geom.asPolygon()[0][3] px = (p1.x() + p3.x()) / 2.0 py = (p1.y() + p3.y()) / 2.0 p1 = QgsPointXY(p1.x() - px + cp.x(), p1.y() - py + cp.y()) p2 = QgsPointXY(p2.x() - px + cp.x(), p2.y() - py + cp.y()) p3 = QgsPointXY(p3.x() - px + cp.x(), p3.y() - py + cp.y()) p4 = QgsPointXY(p4.x() - px + cp.x(), p4.y() - py + cp.y()) proj_geom = QgsGeometry.fromPolygonXY([[p1, p2, p3, p4]]) return proj_geom else: return rect_geom
def updateNavigationInfo(self): if self.gpsConnection is None: self.setMessage(self.tr("Cannot connect to GPS")) return try: gpsinfo = self.gpsConnection.currentGPSInformation() except RuntimeError: # if the GPS is closed in KADAS main interface, stop the navigation self.stopNavigation() return if gpsinfo is None: self.setMessage(self.tr("Cannot connect to GPS")) return layer = self.iface.activeLayer() LOG.debug("Debug: type(layer) = {}".format(type(layer))) point = QgsPointXY(gpsinfo.longitude, gpsinfo.latitude) if gpsinfo.speed > GPS_MIN_SPEED: # if we are moving, it is better for the user experience to # project the current point using the speed vector instead # of using 'point' directly, otherwise we get to feel of being # "behind the current position" qgsdistance = QgsDistanceArea() qgsdistance.setSourceCrs( QgsCoordinateReferenceSystem(4326), QgsProject.instance().transformContext(), ) qgsdistance.setEllipsoid(qgsdistance.sourceCrs().ellipsoidAcronym()) point = qgsdistance.computeSpheroidProject( point, (gpsinfo.speed / SPEED_DIVIDE_BY) * REFRESH_RATE_S, math.radians(gpsinfo.direction), ) origCrs = QgsCoordinateReferenceSystem(4326) canvasCrs = self.iface.mapCanvas().mapSettings().destinationCrs() self.transform = QgsCoordinateTransform( origCrs, canvasCrs, QgsProject.instance() ) if ( isinstance(layer, QgsVectorLayer) and layer.geometryType() == QgsWkbTypes.LineGeometry ): feature = next(layer.getFeatures(), None) if feature: geom = feature.geometry() layer = self.getOptimalRouteLayerForGeometry(geom) if layer is not None: rubbergeom = QgsGeometry(layer.geom) rubbergeom.transform(self.transform) self.rubberband.setToGeometry(rubbergeom) if hasattr(layer, "valhalla") and layer.hasRoute(): try: maneuver = layer.maneuverForPoint(point, gpsinfo.speed) self.refreshCanvas(maneuver["closest_point"], gpsinfo) LOG.debug(maneuver) except NotInRouteException: self.refreshCanvas(point, gpsinfo) self.setMessage(self.tr("You are not on the route")) return self.setWidgetsVisibility(False) html = route_html_template.format(**maneuver) self.textBrowser.setHtml(html) self.textBrowser.setFixedHeight(self.textBrowser.document().size().height()) self.setWarnings(maneuver["raw_distleft"]) # FIXME: we could have some better way of differentiating this... elif not isinstance(layer, type(None)): if layer.name() != "Routes": self.setMessage( self.tr("Select a route or waypoint layer for navigation") ) self.stopNavigation() return waypoints = self.waypointsFromLayer(self.navLayer) if waypoints: if self.waypointLayer is None: self.waypointLayer = self.navLayer self.populateWaypoints(waypoints) else: self.updateWaypoints() waypointItem = ( self.listWaypoints.currentItem() or self.listWaypoints.item(0) ) waypoint = waypointItem.point instructions = getInstructionsToWaypoint(waypoint, gpsinfo) self.setCompass(instructions["heading"], instructions["wpangle"]) html = waypoint_html_template.format(**instructions) self.textBrowser.setHtml(html) self.textBrowser.setFixedHeight( self.textBrowser.document().size().height() ) self.labelWaypointName.setText( waypoint_name_html_template.format(name=waypointItem.name) ) self.setWidgetsVisibility(True) self.setWarnings(instructions["raw_distleft"]) else: self.setMessage(self.tr("The 'Routes' layer has no waypoints.")) self.stopNavigation() return else: self.setMessage(self.tr("Select a route or waypoint layer for navigation")) self.stopNavigation() return
class RectByFixedExtentTool(QgsMapTool): msgbar = pyqtSignal(str) rbFinished = pyqtSignal(object) rb_reset_signal = pyqtSignal() def __init__(self, canvas, x_length, y_length): QgsMapTool.__init__(self, canvas) self.canvas = canvas self.nbPoints = 0 self.rb = None self.x_p1, self.y_p1, self.x_p2, self.y_p2, self.x_p3, self.y_p3, self.x_p4, self.y_p4 = None, None, None, None, None, None, None, None self.distance = QgsDistanceArea() self.distance.setSourceCrs(QgsCoordinateReferenceSystem(4326), QgsProject.instance().transformContext()) self.distance.setEllipsoid('WGS84') self.fixed_p2, self.fixed_p3 = QgsPointXY(0, 0), QgsPointXY(0, 0) self.length = 0 self.mCtrl = None self.x_length = x_length self.y_length = y_length self.bearing = 0.0 # our own fancy cursor self.cursor = QCursor( QPixmap([ "16 16 3 1", " c None", ". c #FF0000", "+ c #1210f3", " ", " +.+ ", " ++.++ ", " +.....+ ", " +. .+ ", " +. . .+ ", " +. . .+ ", " ++. . .++", " ... ...+... ...", " ++. . .++", " +. . .+ ", " +. . .+ ", " ++. .+ ", " ++.....+ ", " ++.++ ", " +.+ " ])) self.rectangle = Rectangle() def keyPressEvent(self, event): if event.key() == Qt.Key_Control: self.mCtrl = True def keyReleaseEvent(self, event): if event.key() == Qt.Key_Control: self.mCtrl = False if event.key() == Qt.Key_Escape: self.nbPoints = 0 self.x_p1, self.y_p1, self.x_p2, self.y_p2, self.x_p3, self.y_p3 = None, None, None, None, None, None self.fixed_p2, self.fixed_p3 = None, None if self.rb: self.rb.reset(True) self.rb = None self.canvas.refresh() self.rb_reset_signal.emit() return def canvasPressEvent(self, event): layer = self.canvas.currentLayer() if self.nbPoints == 0: color = QColor(255, 0, 0, 128) if self.rb: self.rb.reset() self.rb = None self.rb = QgsRubberBand(self.canvas, True) self.rb.setColor(color) self.rb.setWidth(1) self.msgbar.emit("Define bearing along track") self.rb_reset_signal.emit() elif self.nbPoints == 2: self.canvas.refresh() point = self.toLayerCoordinates(layer, event.pos()) point_map = self.toMapCoordinates(layer, point) if self.nbPoints == 0: self.x_p1 = point_map.x() self.y_p1 = point_map.y() elif self.nbPoints == 1: self.x_p2 = point_map.x() self.y_p2 = point_map.y() self.bearing = self.distance.bearing( QgsPointXY(self.x_p1, self.y_p1), QgsPointXY(self.x_p2, self.y_p2)) self.msgbar.emit("Define across track direction") else: self.x_p3 = point_map.x() self.y_p3 = point_map.y() self.nbPoints += 1 if self.nbPoints == 3: geom = self.rectangle.get_rect_by3_points( QgsPointXY(self.x_p1, self.y_p1), self.fixed_p2, self.fixed_p3, self.y_length) self.rb.setToGeometry(geom, None) self.nbPoints = 0 self.x_p1, self.y_p1, self.x_p2, self.y_p2, self.x_p3, self.y_p3 = None, None, None, None, None, None self.fixed_p2, self.fixed_p3 = None, None self.msgbar.emit("") self.rbFinished.emit(geom) if self.rb: return def canvasMoveEvent(self, event): if not self.rb: return currpoint = self.toMapCoordinates(event.pos()) if self.nbPoints == 1: self.bearing = self.distance.bearing( QgsPointXY(self.x_p1, self.y_p1), currpoint) self.fixed_p2 = self.distance.computeSpheroidProject( QgsPointXY(self.x_p1, self.y_p1), self.x_length, self.bearing) self.rb.setToGeometry( QgsGeometry.fromPolyline( [QgsPoint(self.x_p1, self.y_p1), QgsPoint(self.fixed_p2)]), None) curr_bearing = degrees(self.bearing) if curr_bearing < 0.0: curr_bearing = 360 + curr_bearing self.msgbar.emit( "Current distance: {:.3F} m, Current bearing: {:.3F} degrees". format(self.x_length, curr_bearing)) if self.nbPoints >= 2: # test if currpoint is left or right of the line defined by p1 and p2 side = calc_is_collinear(QgsPointXY(self.x_p1, self.y_p1), self.fixed_p2, currpoint) if side == 0: return None self.fixed_p3 = self.distance.computeSpheroidProject( QgsPointXY(self.x_p2, self.y_p2), self.y_length, self.bearing + radians(90) * side) geom = self.rectangle.get_rect_by3_points( QgsPointXY(self.x_p1, self.y_p1), self.fixed_p2, self.fixed_p3, self.y_length) self.rb.setToGeometry(geom, None) curr_bearing = degrees(self.bearing + radians(90) * side) if curr_bearing < 0.0: curr_bearing = 360 + curr_bearing self.msgbar.emit( "Current distance: {:.3F} m, Current bearing: {:.3F} degrees". format(self.y_length, curr_bearing)) def activate(self): self.canvas.setCursor(self.cursor) def deactivate(self): self.nbPoints = 0 self.x_p1, self.y_p1, self.x_p2, self.y_p2, self.x_p3, self.y_p3 = None, None, None, None, None, None self.fixed_p2, self.fixed_p3 = None, None if self.rb: self.rb.reset(True) self.rb.hide() self.rb = None self.canvas.refresh() def is_zoom_tool(self): return False def is_transient(self): return False def is_edit_tool(self): return True
class SpiralLawnMower(object): def __init__(self): self.template_type = 'spiral_lawnmower' self.wp = list() self.mission = None self.distance = QgsDistanceArea() self.distance.setSourceCrs(QgsCoordinateReferenceSystem(4326), QgsProject.instance().transformContext()) self.distance.setEllipsoid('WGS84') def get_mission_type(self): return self.template_type def get_mission(self): return self.mission def compute_tracks(self, area_points, track_spacing, num_across_tracks): """ Compute lawn-mower tracks in spiral pattern :param area_points: points defining the extent of the tracks, they should be in WGS 84 lat/lon. first two points define the along track direction. :param track_spacing: desired space in meters between consecutive along tracks :param num_across_tracks: number of desired across tracks. They will be equally spaced through the area. :return: list of ordered waypoints of the lawn-mower trajectory in spiral pattern """ dist_along_track = self.distance.measureLine(area_points[0], area_points[1]) dist_across_track = self.distance.measureLine(area_points[1], area_points[2]) bearing_along_track = self.distance.bearing(area_points[0], area_points[1]) bearing_across_track = self.distance.bearing(area_points[1], area_points[2]) turn_track_dist = length_one_across_track = track_spacing num_along_tracks = math.ceil(dist_across_track / length_one_across_track) + 1 logger.debug("distAlongTrack: {}".format(dist_along_track)) logger.debug("distAcrossTrack: {}".format(dist_across_track)) logger.debug("bearingAlongTrack {}:".format(bearing_along_track)) logger.debug("bearingAcrossTrack {}:".format(bearing_across_track)) logger.debug("numAlongTrack: {}".format(num_along_tracks)) logger.debug("numAcrossTrack: {}".format(num_across_tracks)) logger.debug("lenghtOneAcrossTrack {}".format(length_one_across_track)) # Initialize with 2 first points current_wp = area_points[0] next_wp = area_points[1] reverse_direction_along = False # For changing direction of alternate along tracks wp_list = [current_wp] # Loop to generate all waypoints of along tracks for i in range(0, int(num_along_tracks / 2)): # The along track next_wp = self.distance.computeSpheroidProject(current_wp, dist_along_track, bearing_along_track) wp_list.append(next_wp) current_wp = next_wp # The across track (dist decremented) curr_dist_across_track = length_one_across_track * (num_along_tracks / 2 - 1) next_wp = self.distance.computeSpheroidProject(current_wp, curr_dist_across_track, bearing_across_track) wp_list.append(next_wp) current_wp = next_wp # The along track reverse direction next_wp = self.distance.computeSpheroidProject(current_wp, dist_along_track, bearing_along_track + math.pi) wp_list.append(next_wp) current_wp = next_wp # The across track reverse direction (dist decremented) curr_dist_across_track = curr_dist_across_track - length_one_across_track next_wp = self.distance.computeSpheroidProject(current_wp, curr_dist_across_track, bearing_across_track + math.pi) wp_list.append(next_wp) current_wp = next_wp if i == int(num_along_tracks / 2 - 1): next_wp = area_points[0] wp_list.append(next_wp) current_wp = next_wp if num_across_tracks > 0: dist_along_track_for_across = dist_along_track / (num_across_tracks + 1) # Distance across track after computing all the along tracks # (might not match with the across track distance defined on the area mission) real_dist_across_track = self.distance.measureLine(wp_list[0], wp_list[2]) next_wp = self.distance.computeSpheroidProject(current_wp, turn_track_dist, bearing_across_track + math.pi) reverse_direction_across = False # For changing direction of alternate across tracks for i in range(num_across_tracks * 2 + 1): wp_list.append(next_wp) current_wp = next_wp if i % 2 == 0: # Compute waypoint along track next_wp = self.distance.computeSpheroidProject(current_wp, dist_along_track_for_across, bearing_along_track + int(reverse_direction_along) * math.pi) else: # Compute waypoint across track next_wp = self.distance.computeSpheroidProject(current_wp, length_one_across_track * ( num_along_tracks - 1) + turn_track_dist, bearing_across_track + int(reverse_direction_across) * math.pi) reverse_direction_across = not reverse_direction_across return wp_list def track_to_mission(self, wp_list, z, altitude_mode,speed, tolerance_x, tolerance_y, tolerance_z): self.mission = Mission() for wp in range(len(wp_list)): if wp == 0: # first step type waypoint step = MissionStep() mwp = MissionWaypoint(MissionPosition(wp_list[wp].y(), wp_list[wp].x(), z, altitude_mode), speed, MissionTolerance(tolerance_x, tolerance_y, tolerance_z)) step.add_maneuver(mwp) self.mission.add_step(step) else: # rest of steps type section step = MissionStep() mwp = MissionSection(MissionPosition(wp_list[wp - 1].y(), wp_list[wp - 1].x(), z, altitude_mode), MissionPosition(wp_list[wp].y(), wp_list[wp].x(), z, altitude_mode), speed, MissionTolerance(tolerance_x, tolerance_y, tolerance_z)) step.add_maneuver(mwp) self.mission.add_step(step)