def show_LandingZone(self): '''show/hide the UAV Challenge landing zone around the clicked point''' from MAVProxy.modules.mavproxy_map import mp_slipmap pos = self.click_position 'Create a new layer of two circles - at 30m and 80m radius around the above point' if (self.mpstate.showLandingZone): self.mpstate.map.add_object( mp_slipmap.SlipClearLayer('LandingZone')) self.mpstate.map.add_object( mp_slipmap.SlipCircle('LandingZoneInner', layer='LandingZone', latlon=pos, radius=30, linewidth=2, color=(0, 0, 255))) self.mpstate.map.add_object( mp_slipmap.SlipCircle('LandingZoneOuter', layer='LandingZone', latlon=pos, radius=80, linewidth=2, color=(0, 0, 255))) else: self.mpstate.map.remove_object('LandingZoneInner') self.mpstate.map.remove_object('LandingZoneOuter') self.mpstate.map.remove_object('LandingZone')
def display_waypoints(self): '''display the waypoints''' self.mission_list = self.module('wp').wploader.view_list() polygons = self.module('wp').wploader.polygon_list() self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('Mission')) for i in range(len(polygons)): p = polygons[i] if len(p) > 1: popup = MPMenuSubMenu('Popup', items=[MPMenuItem('Set', returnkey='popupMissionSet'), MPMenuItem('WP Remove', returnkey='popupMissionRemove'), MPMenuItem('WP Move', returnkey='popupMissionMove')]) self.mpstate.map.add_object(mp_slipmap.SlipPolygon('mission %u' % i, p, layer='Mission', linewidth=2, colour=(255,255,255), popup_menu=popup)) loiter_rad = self.get_mav_param('WP_LOITER_RAD') labeled_wps = {} self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('LoiterCircles')) for i in range(len(self.mission_list)): next_list = self.mission_list[i] for j in range(len(next_list)): #label already printed for this wp? if (next_list[j] not in labeled_wps): self.mpstate.map.add_object(mp_slipmap.SlipLabel( 'miss_cmd %u/%u' % (i,j), polygons[i][j], str(next_list[j]), 'Mission', colour=(0,255,255))) if (self.map_settings.loitercircle and self.module('wp').wploader.wp_is_loiter(next_list[j])): self.mpstate.map.add_object(mp_slipmap.SlipCircle('Loiter Circle %u' % (next_list[j] + 1), 'LoiterCircles', polygons[i][j], abs(loiter_rad), (255, 255, 255), 2)) labeled_wps[next_list[j]] = (i,j)
def mavlink_packet(self, m): '''handle an incoming mavlink packet''' if m.get_type() == "ADSB_VEHICLE": id = 'ADSB-' + str(m.ICAO_address) if id not in self.threat_vehicles.keys( ): # check to see if the vehicle is in the dict # if not then add it self.threat_vehicles[id] = ADSBVehicle(id=id, state=m.to_dict()) for mp in self.module_matching('map*'): from MAVProxy.modules.lib import mp_menu from MAVProxy.modules.mavproxy_map import mp_slipmap self.threat_vehicles[id].menu_item = mp_menu.MPMenuItem( name=id, returnkey=None) threat_radius = get_threat_radius(m) selected_icon = get_threat_icon( m, self.threat_vehicles[id].icon) if selected_icon is not None: # draw the vehicle on the map popup = mp_menu.MPMenuSubMenu( 'ADSB', items=[self.threat_vehicles[id].menu_item]) icon = mp.map.icon(selected_icon) mp.map.add_object( mp_slipmap.SlipIcon( id, (m.lat * 1e-7, m.lon * 1e-7), icon, layer=3, rotation=m.heading * 0.01, follow=False, trail=mp_slipmap.SlipTrail(colour=(0, 255, 255)), popup_menu=popup)) if threat_radius > 0: mp.map.add_object( mp_slipmap.SlipCircle(id + ":circle", 3, (m.lat * 1e-7, m.lon * 1e-7), threat_radius, (0, 255, 255), linewidth=1)) else: # the vehicle is in the dict # update the dict entry self.threat_vehicles[id].update(m.to_dict(), self.get_time()) for mp in self.module_matching('map*'): # update the map ground_alt = mp.ElevationMap.GetElevation( m.lat * 1e-7, m.lon * 1e-7) alt_amsl = m.altitude * 0.001 if alt_amsl > 0: alt = int(alt_amsl - ground_alt) label = str(alt) + "m" else: label = None mp.map.set_position(id, (m.lat * 1e-7, m.lon * 1e-7), rotation=m.heading * 0.01, label=label, colour=(0, 250, 250)) mp.map.set_position(id + ":circle", (m.lat * 1e-7, m.lon * 1e-7))
def cmd_obstacle(self, args): '''control behaviour of the module''' if len(args) == 0: print(self.usage()) elif args[0] == "status": print(self.status()) elif args[0] == "set": self.obstacle_settings.command(args[1:]) else: print(self.usage()) self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('Obstacle')) #missions = client.get_missions() #print missions #stationary_obstacles, moving_obstacles = self.client.get_obstacles() #s_o=stationary_obstacles #print m_o #print s_o[0].latitude #print s_o[0].longitude #print s_o[0].cylinder_radius obstaclefile = open("/home/abhinavjava/cells.txt", "r") print "Before slipcircle" lines = obstaclefile.readlines() for i in lines: thisline = i.split(" ") location = (float(thisline[0]), float(thisline[1])) r = float(thisline[2]) if r == 3: self.mpstate.map.add_object( mp_slipmap.SlipCircle('Obstacle Circle' + i, layer='Obstacle', latlon=location, radius=r, linewidth=-1, color=(0, 255, 0))) elif r == 5: self.mpstate.map.add_object( mp_slipmap.SlipCircle('Obstacle Circle' + i, layer='Obstacle', latlon=location, radius=r, linewidth=-1, color=(255, 0, 0))) print "After slipcircle" """while True: #dynamic obstacles
def update_map(self): '''update the map graphics''' for mp in self.module_matching('map*'): for id in self.threat_vehicles.keys(): mstate = self.threat_vehicles[id].state threat_radius = self.threat_vehicles[id].getThreatRadius( self.AIS_settings.threat_radius) # update if existing object on map, else create if self.threat_vehicles[id].onMap: mp.map.set_position(id, (mstate.lat * 1e-7, mstate.lon * 1e-7), 3, rotation=mstate.heading * 0.01, label=str(mstate.callsign)) # Turns out we can't edit the circle's radius, so have to remove and re-add mp.map.remove_object(id + ":circle") mp.map.add_object( mp_slipmap.SlipCircle( id + ":circle", 3, (mstate.lat * 1e-7, mstate.lon * 1e-7), threat_radius, (0, 255, 255), linewidth=1)) else: self.threat_vehicles[id].menu_item = mp_menu.MPMenuItem( name=id, returnkey=None) # draw the vehicle on the map popup = mp_menu.MPMenuSubMenu( 'AIS', items=[self.threat_vehicles[id].menu_item]) icon = mp_slipmap.SlipIcon( id, (mstate.lat * 1e-7, mstate.lon * 1e-7), mp.map.icon(self.threat_vehicles[id].icon), 3, rotation=mstate.heading * 0.01, follow=False, trail=mp_slipmap.SlipTrail(colour=(0, 255, 255)), popup_menu=popup, ) mp.map.add_object(icon) mp.map.add_object( mp_slipmap.SlipCircle( id + ":circle", 3, (mstate.lat * 1e-7, mstate.lon * 1e-7), threat_radius, (0, 255, 255), linewidth=1)) self.threat_vehicles[id].onMap = True
def mavlink_packet_obstacle_distance(self, vehicle, m): heading = self.heading_for_vehicle.get(vehicle, 0) tlayer = "Obstacle Distance for %u.%u type=%u" % ( m.get_srcSystem(), m.get_srcComponent(), m.sensor_type) if m.get_srcSystem() in self.lat_lon_for_vehicle: color = (255, 0, 255) (lat, lon) = self.lat_lon_for_vehicle[m.get_srcSystem()] self.foreach_map(lambda a_map: a_map.add_object( mp_slipmap.SlipClearLayer(tlayer))) increment = m.increment_f if increment == 0: increment = float(m.increment) start_angle = -increment / 2.0 end_angle = increment / 2.0 for i in range(0, 72): slipkey = '%s-POS%u' % (tlayer, i) if m.distances[i] == m.max_distance + 1: # no measurement self.foreach_map( lambda a_map: a_map.remove_object(slipkey)) continue distance = m.distances[i] / 100.0 # cm -> m circle = mp_slipmap.SlipCircle( slipkey, 3, (lat, lon), distance, color, linewidth=3, start_angle=start_angle, end_angle=end_angle, rotation=(-90 + (heading + i * increment)) % 360, ) self.foreach_map(lambda a_map: a_map.add_object(circle)) # add a circle for max-range slipkey = "%s-range" % tlayer self.foreach_map(lambda a_map: a_map.add_object( mp_slipmap.SlipCircle( slipkey, 3, (lat, lon), m.max_distance / 100.0, color, linewidth=1, )))
def display_waypoints(self): '''display the waypoints''' from MAVProxy.modules.mavproxy_map import mp_slipmap self.mission_list = self.module('wp').wploader.view_list() polygons = self.module('wp').wploader.polygon_list() self.map.add_object(mp_slipmap.SlipClearLayer('Mission')) for i in range(len(polygons)): p = polygons[i] if len(p) > 1: items = [MPMenuItem('Set', returnkey='popupMissionSet'), MPMenuItem('WP Remove', returnkey='popupMissionRemove'), MPMenuItem('WP Move', returnkey='popupMissionMove'), MPMenuItem('Remove NoFly', returnkey='popupMissionRemoveNoFly'), ] popup = MPMenuSubMenu('Popup', items) self.map.add_object(mp_slipmap.SlipPolygon('mission %u' % i, p, layer='Mission', linewidth=2, colour=(255,255,255), arrow = self.map_settings.showdirection, popup_menu=popup)) labeled_wps = {} self.map.add_object(mp_slipmap.SlipClearLayer('LoiterCircles')) if not self.map_settings.showwpnum: return for i in range(len(self.mission_list)): next_list = self.mission_list[i] for j in range(len(next_list)): #label already printed for this wp? if (next_list[j] not in labeled_wps): label = self.label_for_waypoint(next_list[j]) colour = self.colour_for_wp(next_list[j]) self.map.add_object(mp_slipmap.SlipLabel( 'miss_cmd %u/%u' % (i,j), polygons[i][j], label, 'Mission', colour=colour)) if (self.map_settings.loitercircle and self.module('wp').wploader.wp_is_loiter(next_list[j])): wp = self.module('wp').wploader.wp(next_list[j]) if wp.command != mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT and wp.param3 != 0: # wp radius and direction is defined by the mission loiter_rad = wp.param3 elif wp.command == mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT and wp.param2 != 0: # wp radius and direction is defined by the mission loiter_rad = wp.param2 else: # wp radius and direction is defined by the parameter loiter_rad = self.get_mav_param('WP_LOITER_RAD') self.map.add_object(mp_slipmap.SlipCircle('Loiter Circle %u' % (next_list[j] + 1), 'LoiterCircles', polygons[i][j], loiter_rad, (255, 255, 255), 2, arrow = self.map_settings.showdirection)) labeled_wps[next_list[j]] = (i,j)
def mavlink_packet_distance_sensor(self, vehicle, m): heading = self.heading_for_vehicle.get(vehicle, 0) tlayer = "Distance sensor for %u.%u id=%u" % ( m.get_srcSystem(), m.get_srcComponent(), m.id) slipkey = '%s-POS%u' % (tlayer, m.orientation) if not m.get_srcSystem() in self.lat_lon_for_vehicle: return if m.current_distance == m.max_distance: self.foreach_map(lambda a_map : a_map.remove_object(slipkey)) return (lat,lon) = self.lat_lon_for_vehicle[m.get_srcSystem()] mav_sensor_rotation_to_degrees = { mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 0, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 45, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 90, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135 : 135, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 180, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225 : 225, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270 : 270, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315 : 315, } if m.orientation in mav_sensor_rotation_to_degrees: degrees = mav_sensor_rotation_to_degrees[m.orientation] else: # print("bad orientation (%u)" % m.orientation) return if m.current_distance >= m.max_distance: return p = mp_util.gps_newpos(lat, lon, heading+degrees, 0) # start angle/end angle are either side of the primary axis, # which is rotated to be North start_angle = -22.5 end_angle = 22.5 self.foreach_map(lambda a_map : a_map.add_object(mp_slipmap.SlipCircle( slipkey, 3, p, m.current_distance/100.0, (255, 127, 0), linewidth=3, start_angle=start_angle, end_angle=end_angle, rotation=(-90+(heading+degrees))%360, )) )
def show_JoeZone(self): '''show the UAV Challenge landing zone around the clicked point''' from MAVProxy.modules.mavproxy_map import mp_slipmap camera = self.module('camera_ground') if camera is None: print("camera_ground module is not loaded") return target = (camera.camera_settings.target_latitude, camera.camera_settings.target_longitude, camera.camera_settings.target_radius) self.target = target for m in self.module_matching('map*'): m.map.add_object(mp_slipmap.SlipClearLayer('JoeZone')) m.map.add_object( mp_slipmap.SlipCircle('JoeZoneCircle', layer='JoeZone', latlon=(target[0], target[1]), radius=target[2], linewidth=2, color=(0, 0, 128)))
def toggle_JoeZone(self): '''show/hide the UAV Challenge landing zone around the clicked point''' from MAVProxy.modules.mavproxy_map import mp_slipmap camera = self.module('camera') if camera is None: print("camera module is not loaded") return if camera.camera_settings.target_radius <= 0: print("camera module target_radius is not set") return target = (camera.camera_settings.target_lattitude, camera.camera_settings.target_longitude, camera.camera_settings.target_radius) self.target = target 'Create a new layer with given radius around the above point' if self.showJoeZone: self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('JoeZone')) self.mpstate.map.add_object(mp_slipmap.SlipCircle('JoeZoneCircle', layer='JoeZone', latlon=(target[0],target[1]), radius=target[2], linewidth=2, color=(0,0,128))) else: self.mpstate.map.remove_object('JoeZoneCircle') self.mpstate.map.remove_object('JoeZone')
def mavlink_packet(self, m): '''handle an incoming mavlink packet''' from MAVProxy.modules.mavproxy_map import mp_slipmap mtype = m.get_type() sysid = m.get_srcSystem() if mtype == "HEARTBEAT": vname = 'plane' if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]: vname = 'plane' elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER]: vname = 'rover' elif m.type in [mavutil.mavlink.MAV_TYPE_SUBMARINE]: vname = 'sub' elif m.type in [mavutil.mavlink.MAV_TYPE_SURFACE_BOAT]: vname = 'boat' elif m.type in [ mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER ]: vname = 'copter' elif m.type in [mavutil.mavlink.MAV_TYPE_COAXIAL]: vname = 'singlecopter' elif m.type in [mavutil.mavlink.MAV_TYPE_HELICOPTER]: vname = 'heli' elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: vname = 'antenna' self.vehicle_type_by_sysid[sysid] = vname if not sysid in self.vehicle_type_by_sysid: self.vehicle_type_by_sysid[sysid] = 'plane' self.vehicle_type_name = self.vehicle_type_by_sysid[sysid] # this is the beginnings of allowing support for multiple vehicles # in the air at the same time vehicle = 'Vehicle%u' % m.get_srcSystem() if mtype == "SIMSTATE" and self.map_settings.showsimpos: self.create_vehicle_icon('Sim' + vehicle, 'green') self.map.set_position('Sim' + vehicle, (m.lat * 1.0e-7, m.lng * 1.0e-7), rotation=math.degrees(m.yaw)) elif mtype == "AHRS2" and self.map_settings.showahrs2pos: self.create_vehicle_icon('AHRS2' + vehicle, 'blue') self.map.set_position('AHRS2' + vehicle, (m.lat * 1.0e-7, m.lng * 1.0e-7), rotation=math.degrees(m.yaw)) elif mtype == "AHRS3" and self.map_settings.showahrs3pos: self.create_vehicle_icon('AHRS3' + vehicle, 'orange') self.map.set_position('AHRS3' + vehicle, (m.lat * 1.0e-7, m.lng * 1.0e-7), rotation=math.degrees(m.yaw)) elif mtype == "GPS_RAW_INT" and self.map_settings.showgpspos: (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7) if lat != 0 or lon != 0: if m.vel > 300 or 'ATTITUDE' not in self.master.messages: cog = m.cog * 0.01 else: cog = math.degrees(self.master.messages['ATTITUDE'].yaw) self.create_vehicle_icon('GPS' + vehicle, 'blue') self.map.set_position('GPS' + vehicle, (lat, lon), rotation=cog) elif mtype == "GPS2_RAW" and self.map_settings.showgps2pos: (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7) if lat != 0 or lon != 0: self.create_vehicle_icon('GPS2' + vehicle, 'green') self.map.set_position('GPS2' + vehicle, (lat, lon), rotation=m.cog * 0.01) elif mtype == 'GLOBAL_POSITION_INT' and self.map_settings.showahrspos: (lat, lon, heading) = (m.lat * 1.0e-7, m.lon * 1.0e-7, m.hdg * 0.01) self.lat_lon[m.get_srcSystem()] = (lat, lon) if abs(lat) > 1.0e-3 or abs(lon) > 1.0e-3: self.have_global_position = True self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True) if len(self.vehicle_type_by_sysid) > 1: label = str(sysid) else: label = None self.map.set_position('Pos' + vehicle, (lat, lon), rotation=heading, label=label, colour=(255, 255, 255)) self.map.set_follow_object('Pos' + vehicle, self.is_primary_vehicle(m)) elif mtype == 'HOME_POSITION': (lat, lon) = (m.latitude * 1.0e-7, m.longitude * 1.0e-7) icon = self.map.icon('home.png') self.map.add_object( mp_slipmap.SlipIcon('HOME_POSITION', (lat, lon), icon, layer=3, rotation=0, follow=False)) elif mtype == "NAV_CONTROLLER_OUTPUT": tlayer = 'Trajectory%u' % m.get_srcSystem() if (self.master.flightmode in [ "AUTO", "GUIDED", "LOITER", "RTL", "QRTL", "QLOITER", "QLAND", "FOLLOW" ] and m.get_srcSystem() in self.lat_lon): (lat, lon) = self.lat_lon[m.get_srcSystem()] trajectory = [(lat, lon), mp_util.gps_newpos(lat, lon, m.target_bearing, m.wp_dist)] self.map.add_object( mp_slipmap.SlipPolygon('trajectory', trajectory, layer=tlayer, linewidth=2, colour=(255, 0, 180))) self.trajectory_layers.add(tlayer) else: if tlayer in self.trajectory_layers: self.map.add_object(mp_slipmap.SlipClearLayer(tlayer)) self.trajectory_layers.remove(tlayer) elif mtype == "POSITION_TARGET_GLOBAL_INT": # FIXME: base this off SYS_STATUS.MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL? if not m.get_srcSystem() in self.lat_lon: return tlayer = 'PostionTarget%u' % m.get_srcSystem() (lat, lon) = self.lat_lon[m.get_srcSystem()] if (self.master.flightmode in [ "AUTO", "GUIDED", "LOITER", "RTL", "QRTL", "QLOITER", "QLAND", "FOLLOW" ]): lat_float = m.lat_int * 1e-7 lon_float = m.lon_int * 1e-7 vec = [(lat_float, lon_float), (lat, lon)] self.map.add_object( mp_slipmap.SlipPolygon('position_target', vec, layer=tlayer, linewidth=2, colour=(0, 255, 0))) else: self.map.add_object(mp_slipmap.SlipClearLayer(tlayer)) if not self.is_primary_vehicle(m): # the rest should only be done for the primary vehicle return # if the waypoints have changed, redisplay last_wp_change = self.module('wp').wploader.last_change if self.wp_change_time != last_wp_change and abs(time.time() - last_wp_change) > 1: self.wp_change_time = last_wp_change self.display_waypoints() #this may have affected the landing lines from the rally points: self.rally_change_time = time.time() # if the fence has changed, redisplay if (self.module('fence') and self.fence_change_time != self.module('fence').fenceloader.last_change): self.display_fence() # if the rallypoints have changed, redisplay if (self.module('rally') and self.rally_change_time != self.module('rally').last_change()): self.rally_change_time = self.module('rally').last_change() icon = self.map.icon('rallypoint.png') self.map.add_object(mp_slipmap.SlipClearLayer('RallyPoints')) for i in range(self.module('rally').rally_count()): rp = self.module('rally').rally_point(i) popup = MPMenuSubMenu( 'Popup', items=[ MPMenuItem('Rally Remove', returnkey='popupRallyRemove'), MPMenuItem('Rally Move', returnkey='popupRallyMove') ]) self.map.add_object( mp_slipmap.SlipIcon('Rally %u' % (i + 1), (rp.lat * 1.0e-7, rp.lng * 1.0e-7), icon, layer='RallyPoints', rotation=0, follow=False, popup_menu=popup)) loiter_rad = self.get_mav_param('WP_LOITER_RAD') if self.map_settings.rallycircle: self.map.add_object( mp_slipmap.SlipCircle( 'Rally Circ %u' % (i + 1), 'RallyPoints', (rp.lat * 1.0e-7, rp.lng * 1.0e-7), loiter_rad, (255, 255, 0), 2, arrow=self.map_settings.showdirection)) #draw a line between rally point and nearest landing point nearest_land_wp = None nearest_distance = 10000000.0 for j in range(self.module('wp').wploader.count()): w = self.module('wp').wploader.wp(j) if (w.command == 21): #if landing waypoint #get distance between rally point and this waypoint dis = mp_util.gps_distance(w.x, w.y, rp.lat * 1.0e-7, rp.lng * 1.0e-7) if (dis < nearest_distance): nearest_land_wp = w nearest_distance = dis if nearest_land_wp is not None: points = [] #tangential approach? if self.get_mav_param('LAND_BREAK_PATH') == 0: theta = math.degrees( math.atan(loiter_rad / nearest_distance)) tan_dis = math.sqrt(nearest_distance * nearest_distance - (loiter_rad * loiter_rad)) ral_bearing = mp_util.gps_bearing( nearest_land_wp.x, nearest_land_wp.y, rp.lat * 1.0e-7, rp.lng * 1.0e-7) points.append( mp_util.gps_newpos(nearest_land_wp.x, nearest_land_wp.y, ral_bearing + theta, tan_dis)) else: #not tangential approach points.append((rp.lat * 1.0e-7, rp.lng * 1.0e-7)) points.append((nearest_land_wp.x, nearest_land_wp.y)) self.map.add_object( mp_slipmap.SlipPolygon('Rally Land %u' % (i + 1), points, 'RallyPoints', (255, 255, 0), 2)) # check for any events from the map self.map.check_events()
def cmd_map(self, args): '''map commands''' from MAVProxy.modules.mavproxy_map import mp_slipmap if len(args) < 1: print("usage: map <icon|set>") elif args[0] == "icon": if len(args) < 3: print("Usage: map icon <lat> <lon> <icon>") else: lat = args[1] lon = args[2] flag = 'flag.png' if len(args) > 3: flag = args[3] + '.png' icon = self.map.icon(flag) self.map.add_object( mp_slipmap.SlipIcon('icon - %s [%u]' % (str(flag), self.icon_counter), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False)) self.icon_counter += 1 elif args[0] == "circle": if len(args) < 4: # map circle -27.70533373 153.23404844 5 red print("Usage: map circle <lat> <lon> <radius> <colour>") else: lat = args[1] lon = args[2] radius = args[3] colour = 'red' if len(args) > 4: colour = args[4] if colour == "red": colour = (255, 0, 0) elif colour == "green": colour = (0, 255, 0) elif colour == "blue": colour = (0, 0, 255) else: colour = eval(colour) circle = mp_slipmap.SlipCircle( "circle %u" % self.circle_counter, 3, (float(lat), float(lon)), float(radius), colour, linewidth=1, ) self.map.add_object(circle) self.circle_counter += 1 elif args[0] == "set": self.map_settings.command(args[1:]) self.map.add_object( mp_slipmap.SlipBrightness(self.map_settings.brightness)) elif args[0] == "sethome": self.cmd_set_home(args) elif args[0] == "sethomepos": self.cmd_set_homepos(args) elif args[0] == "setorigin": self.cmd_set_origin(args) elif args[0] == "setoriginpos": self.cmd_set_originpos(args) elif args[0] == "zoom": self.cmd_zoom(args) elif args[0] == "center": self.cmd_center(args) elif args[0] == "follow": self.cmd_follow(args) elif args[0] == "clear": self.cmd_clear(args) else: print("usage: map <icon|set>")
def mavlink_packet(self, m): '''handle an incoming mavlink packet''' if m.get_type() == "ADSB_VEHICLE": id = 'ADSB-' + str(m.ICAO_address) if id not in self.threat_vehicles.keys( ): # check to see if the vehicle is in the dict # if not then add it self.threat_vehicles[id] = ADSBVehicle(id=id, state=m.to_dict()) if self.mpstate.map: # if the map is loaded... icon = self.mpstate.map.icon(self.threat_vehicles[id].icon) popup = MPMenuSubMenu( 'ADSB', items=[MPMenuItem(name=id, returnkey=None)]) # draw the vehicle on the map self.mpstate.map.add_object( mp_slipmap.SlipIcon( id, (m.lat * 1e-7, m.lon * 1e-7), icon, layer=3, rotation=m.heading * 0.01, follow=False, trail=mp_slipmap.SlipTrail(colour=(0, 255, 255)), popup_menu=popup)) else: # the vehicle is in the dict # update the dict entry self.threat_vehicles[id].update(m.to_dict()) if self.mpstate.map: # if the map is loaded... # update the map self.mpstate.map.set_position(id, (m.lat * 1e-7, m.lon * 1e-7), rotation=m.heading * 0.01) if m.get_type() == "GLOBAL_POSITION_INT": if self.mpstate.map: if len(self.active_threat_ids) > 0: threat_circle_width = 2 else: threat_circle_width = 1 # update the threat circle on the map threat_circle = mp_slipmap.SlipCircle( "threat_circle", 3, (m.lat * 1e-7, m.lon * 1e-7), self.ADSB_settings.threat_radius, (0, 255, 255), linewidth=threat_circle_width) threat_circle.set_hidden( not self.ADSB_settings.show_threat_radius ) # show the circle? self.mpstate.map.add_object(threat_circle) # update the threat clear circle on the map threat_radius_clear = self.ADSB_settings.threat_radius * \ self.ADSB_settings.threat_radius_clear_multiplier threat_clear_circle = mp_slipmap.SlipCircle( "threat_clear_circle", 3, (m.lat * 1e-7, m.lon * 1e-7), threat_radius_clear, (0, 255, 255), linewidth=1) # show the circle? threat_clear_circle.set_hidden( not self.ADSB_settings.show_threat_radius_clear) self.mpstate.map.add_object(threat_clear_circle) # we assume this is handled much more oftern than ADS-B messages # so update the distance between vehicle and threat here self.update_threat_distances( (m.lat * 1e-7, m.lon * 1e-7, m.alt * 1e-3))
def check_commands(self, bsend): '''check for remote commands''' if bsend is None: return buf = bsend.recv(0) if buf is None: return try: obj = cPickle.loads(str(buf)) if obj is None: return except Exception as e: return if isinstance(obj, cuav_command.StampedCommand): bidx = None for i in range(len(self.bsend)): if bsend == self.bsend[i]: bidx = i + 1 if bidx is None and bsend == self.msend: bidx = 0 if bidx is not None: now = time.time() self.mcount[bidx] += 1 self.last_msg_stamp[bidx] = now if obj.timestamp in self.handled_timestamps: # we've seen this packet before, discard return self.handled_timestamps[obj.timestamp] = time.time() if isinstance(obj, cuav_command.HeartBeat): self.image_count = obj.icount self.console.set_status('Images', 'Images %u' % self.image_count, row=6) if isinstance(obj, cuav_command.ThumbPacket): # we've received a set of thumbnails from the plane for a positive hit if obj.frame_time not in self.thumbs_received: self.thumbs_received.add(obj.frame_time) self.thumb_total_bytes += len(buf) # add the thumbnails to the mosaic thumbdec = cv2.imdecode(obj.thumb, 1) if thumbdec is None: pass thumbs = cuav_mosaic.ExtractThumbs(thumbdec, len(obj.regions)) thumbsRGB = [] #colour space conversion for thumb in thumbs: thumbsRGB.append(cv2.cvtColor(thumb, cv2.COLOR_BGR2RGB)) # log the joe positions # note the filename is where the fullsize image would be downloaded # to, if requested filename = os.path.join( self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg" self.log_joe_position(obj.pos, obj.frame_time, obj.regions, filename, None) # update the mosaic and map self.mosaic.add_regions(obj.regions, thumbsRGB, filename, obj.pos) # update console display self.region_count += len(obj.regions) self.thumb_count += 1 self.console.set_status('Regions', 'Regions %u' % self.region_count, row=6) self.console.set_status('Thumbs', 'Thumbs %u' % self.thumb_count, row=7) self.console.set_status( 'ThumbSize', 'ThumbSize %.0f' % (self.thumb_total_bytes / self.thumb_count), row=7) if isinstance(obj, cuav_command.ImagePacket): # we have an image from the plane self.image_total_bytes += len(buf) #save to file imagedec = cv2.imdecode(obj.jpeg, 1) ff = os.path.join(self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg" write_param = [int(cv2.IMWRITE_JPEG_QUALITY), 99] cv2.imwrite(ff, imagedec, write_param) self.mosaic.tag_image(obj.frame_time) if obj.pos is not None: self.mosaic.add_image(obj.frame_time, ff, obj.pos) # update console self.image_count += 1 color = 'black' self.console.set_status('Images', 'Images %u' % self.image_count, row=6, fg=color) if isinstance(obj, cuav_command.CommandPacket): # ground doesn't accept command packets from air pass if isinstance(obj, cuav_command.CommandResponse): # reply to CommandPacket print('REMOTE: %s' % obj.response) if isinstance(obj, cuav_command.CameraMessage): print('CUAV AIR REMOTE: %s' % obj.msg) if isinstance(obj, cuav_landingregion.LandingZoneDisplay): lzresult = obj # display on all maps for m in self.module_matching('map?'): m.map.add_object( mp_slipmap.SlipCircle('LZ', 'LZ', lzresult.latlon, lzresult.maxrange, linewidth=3, color=(0, 255, 0))) m.map.add_object( mp_slipmap.SlipCircle('LZMid', 'LZMid', lzresult.latlon, 2.0, linewidth=3, color=(0, 255, 0))) lztext = 'LZ: %.6f %.6f E:%.1f AS:%.0f N:%u' % ( lzresult.latlon[0], lzresult.latlon[1], lzresult.maxrange, lzresult.avgscore, lzresult.numregions) m.map.add_object(mp_slipmap.SlipInfoText( 'landingzone', lztext)) # assume map2 is the search map map2 = self.module('map2') if map2 is not None: #map2.map.set_zoom(250) map2.map.set_center(lzresult.latlon[0], lzresult.latlon[1]) map2.map.set_follow(0) # assume map3 is the lz map map3 = self.module('map3') if map3 is not None: map3.map.set_zoom(max(50, 2 * lzresult.maxrange)) map3.map.set_center(lzresult.latlon[0], lzresult.latlon[1]) map3.map.set_follow(0) try: cuav = self.module('CUAV') cuav.show_JoeZone() except Exception as ex: print("err: ", ex) return if isinstance(obj, cuav_command.FilePacket): print("got file %s" % obj.filename) try: open(obj.filename, "w").write(obj.contents) except Exception as ex: print("file save failed", ex) return if obj.filename == "newwp.txt": try: wpmod = self.module('wp') wpmod.wploader.load(obj.filename) except Exception as ex: print("wp load failed", ex) if isinstance(obj, cuav_command.PreviewPacket): # we have a preview image from the plane img = cv2.imdecode(obj.jpeg, 1) if self.preview_window is None or not self.preview_window.is_alive( ): self.preview_window = mp_image.MPImage(title='Preview', width=410, height=308, auto_size=True) if self.preview_window is not None: self.preview_window.set_image(img, bgr=True) self.preview_window.poll()
def process(args): '''process a set of files''' global slipmap, mosaic scan_count = 0 files = [] if os.path.isdir(args.directory): files.extend(file_list(args.directory, ['jpg', 'jpeg', 'png'])) else: if args.directory.find('*') != -1: files.extend(glob.glob(args.directory)) else: files.append(args.directory) files.sort() num_files = len(files) print("num_files=%u" % num_files) region_count = 0 slipmap = mp_slipmap.MPSlipMap(service=args.service, elevation=True, title='Map') if args.vehicle_type == "Copter": icon = slipmap.icon('redcopter.png') else: icon = slipmap.icon('redplane.png') slipmap.add_object( mp_slipmap.SlipIcon('plane', (0, 0), icon, layer=3, rotation=0, follow=True, trail=mp_slipmap.SlipTrail())) for flag in args.flag: a = flag.split(',') lat = a[0] lon = a[1] icon = 'flag.png' if len(a) > 2: icon = a[2] + '.png' icon = slipmap.icon(icon) slipmap.add_object( mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False)) if args.mission: from pymavlink import mavwp wp = mavwp.MAVWPLoader() wp.load(args.mission.name) plist = wp.polygon_list() if len(plist) > 0: for i in range(len(plist)): slipmap.add_object( mp_slipmap.SlipPolygon('Mission-%s-%u' % (args.mission.name, i), plist[i], layer='Mission', linewidth=2, colour=(255, 255, 255))) if args.mavlog: mpos = mav_position.MavInterpolator() mpos.set_logfile(args.mavlog) else: mpos = None if args.gammalog is not None: gamma = parse_gamma_log(args.gammalog) else: gamma = None if args.kmzlog: kmzpos = mav_position.KmlPosition(args.kmzlog) else: kmzpos = None if args.triggerlog: triggerpos = mav_position.TriggerPosition(args.triggerlog) else: triggerpos = None # create a simple lens model using the focal length if args.camera_params: C_params = cam_params.CameraParams.fromfile(args.camera_params) else: im_orig = cv2.imread(files[0]) (w, h) = cuav_util.image_shape(im_orig) C_params = cam_params.CameraParams(lens=args.lens, sensorwidth=args.sensorwidth, xresolution=w, yresolution=h) if args.target: target = args.target.split(',') else: target = [0, 0, 0] camera_settings = MPSettings([ MPSetting('roll_stabilised', bool, args.roll_stabilised, 'Roll Stabilised'), MPSetting('pitch_stabilised', bool, args.pitch_stabilised, 'Pitch Stabilised'), MPSetting('roll_offset', float, args.roll_offset, 'Roll Offset'), MPSetting('pitch_offset', float, args.pitch_offset, 'Pitch Offset'), MPSetting('altitude', int, args.altitude, 'Altitude', range=(0, 10000), increment=1), MPSetting( 'minalt', int, 30, 'MinAltitude', range=(0, 10000), increment=1), MPSetting('mpp100', float, 0.0977, 'MPPat100m', range=(0, 10000), increment=0.001), MPSetting('rotate180', bool, False, 'rotate180'), MPSetting('showlz', bool, args.showlz, 'Show calculated landing zone from regions'), MPSetting( 'filter_type', str, 'simple', 'Filter Type', choice=['simple']), MPSetting('target_lattitude', float, float(target[0]), 'target latitude', increment=1.0e-7), MPSetting('target_longitude', float, float(target[1]), 'target longitude', increment=1.0e-7), MPSetting('target_radius', float, float(target[2]), 'target radius', increment=1), MPSetting('quality', int, 75, 'Compression Quality', range=(1, 100), increment=1), MPSetting('thumbsize', int, args.thumbsize, 'Thumbnail Size', range=(10, 200), increment=1), MPSetting('map_thumbsize', int, args.map_thumbsize, 'Map Thumbnail Size', range=(10, 200), increment=1), MPSetting('minscore', int, args.minscore, 'Min Score', range=(0, 1000), increment=1, tab='Scoring'), MPSetting('brightness', float, 1.0, 'Display Brightness', range=(0.1, 10), increment=0.1, digits=2, tab='Display'), ], title='Camera Settings') image_settings = MPSettings([ MPSetting('MinRegionArea', float, 0.15, range=(0, 100), increment=0.05, digits=2, tab='Image Processing'), MPSetting('MaxRegionArea', float, 1.0, range=(0, 100), increment=0.1, digits=1), MPSetting('MinRegionSize', float, 0.2, range=(0, 100), increment=0.05, digits=2), MPSetting('MaxRegionSize', float, 1.0, range=(0, 100), increment=0.1, digits=1), MPSetting('MaxRarityPct', float, 0.02, range=(0, 100), increment=0.01, digits=2), MPSetting('RegionMergeSize', float, 1.0, range=(0, 100), increment=0.1, digits=1), MPSetting('BlueEmphasis', bool, args.blue_emphasis), MPSetting('SaveIntermediate', bool, args.debug) ], title='Image Settings') mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params, camera_settings=camera_settings, image_settings=image_settings, start_menu=True, classify=args.categories, thumb_size=args.mosaic_thumbsize, map_thumb_size=args.map_thumbsize) joelog = cuav_joe.JoeLog(None) lz = cuav_landingregion.LandingZone() if args.view: viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True) start_time = time.time() for f in files: if not mosaic.started(): print("Waiting for startup") if args.start: mosaic.has_started = True while not mosaic.started(): mosaic.check_events() time.sleep(0.01) if mpos: # get the position by interpolating telemetry data from the MAVLink log file # this assumes that the filename contains the timestamp if gamma is not None: frame_time = parse_gamma_time(f, gamma) else: frame_time = cuav_util.parse_frame_time(f) frame_time += args.time_offset if camera_settings.roll_stabilised: roll = 0 else: roll = None if camera_settings.pitch_stabilised: pitch = 0 else: pitch = None try: pos = mpos.position(frame_time, roll=roll, pitch=pitch, roll_offset=camera_settings.roll_offset, pitch_offset=camera_settings.pitch_offset) except Exception: print("No position available for %s" % frame_time) # skip this frame continue elif kmzpos is not None: pos = kmzpos.position(f) elif triggerpos is not None: pos = triggerpos.position(f) else: # get the position using EXIF data pos = mav_position.exif_position(f) pos.time += args.time_offset # update the vehicle icon on the map if pos is not None: slipmap.set_position("plane", (pos.lat, pos.lon), rotation=pos.yaw) if camera_settings.altitude > 0: pos.altitude = camera_settings.altitude # check for any events from the map slipmap.check_events() mosaic.check_events() #im_orig = cuav_util.LoadImage(f, rotate180=camera_settings.rotate180) im_orig = cv2.imread(f) if im_orig is None: continue (w, h) = cuav_util.image_shape(im_orig) if args.downsample: im_full = cv2.resize(im_orig, (0, 0), fx=0.5, fy=0.5) else: im_full = im_orig count = 0 total_time = 0 t0 = time.time() img_scan = im_full scan_parms = {} for name in image_settings.list(): scan_parms[name] = image_settings.get(name) scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate']) scan_parms['BlueEmphasis'] = float(scan_parms['BlueEmphasis']) if pos is not None: (sw, sh) = cuav_util.image_shape(img_scan) altitude = pos.altitude if altitude < camera_settings.minalt: altitude = camera_settings.minalt scan_parms[ 'MetersPerPixel'] = camera_settings.mpp100 * altitude / 100.0 regions = scanner.scan(img_scan, scan_parms) else: regions = scanner.scan(img_scan) regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full)) count += 1 t1 = time.time() frame_time = pos.time if pos: for r in regions: r.latlon = cuav_util.gps_position_from_image_region( r, pos, w, h, altitude=altitude, C=C_params) if camera_settings.target_radius > 0 and pos is not None: regions = cuav_region.filter_radius( regions, (camera_settings.target_lattitude, camera_settings.target_longitude), camera_settings.target_radius) regions = cuav_region.filter_regions( im_full, regions, min_score=camera_settings.minscore, filter_type=camera_settings.filter_type) scan_count += 1 if pos and len(regions) > 0: joelog.add_regions(frame_time, regions, pos, f) mosaic.add_image(pos.time, f, pos) if camera_settings.showlz: for r in regions: if r.score >= camera_settings.minscore and pos is not None: lz.checkaddregion(r, pos) #if lz.landingzoneconfidence and lz.landingzoneconfidence < 20: # print("Refining LZ") lzresult = lz.calclandingzone() if lzresult is not None: slipmap.add_object( mp_slipmap.SlipCircle('LZ', 'LZ', lzresult.latlon, lzresult.maxrange, linewidth=3, color=(0, 255, 0))) slipmap.add_object( mp_slipmap.SlipCircle('LZMid', 'LZMid', lzresult.latlon, 2.0, linewidth=3, color=(0, 255, 0))) region_count += len(regions) if len(regions) > 0: composite = cuav_region.CompositeThumbnail(im_full, regions) thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions)) thumbsRGB = [] #colour space conversion for thumb in thumbs: thumbsRGB.append(cv2.cvtColor(thumb, cv2.COLOR_BGR2RGB)) mosaic.add_regions(regions, thumbsRGB, f, pos) if args.view: img_view = img_scan (wview, hview) = cuav_util.image_shape(img_view) for r in regions: r.draw_rectangle(img_view, (255, 0, 0)) #cv.CvtColor(mat, mat, cv.CV_BGR2RGB) img_view = cv2.cvtColor(img_view, cv2.COLOR_BGR2RGB) viewer.set_image(img_view) viewer.set_title('Image: ' + os.path.basename(f)) total_time += (t1 - t0) if t1 != t0: print('%s scan %.1f fps %u regions [%u/%u]' % (os.path.basename(f), count / total_time, region_count, scan_count, num_files)) #raw_input("hit ENTER when ready") print("All images processed (%u seconds)" % (time.time() - start_time)) while True: # check for any events from the map slipmap.check_events() mosaic.check_events() time.sleep(0.2)
def mavlink_packet(self, m): '''handle and incoming mavlink packet''' self.Update_traffic() if (len(self.traffic_list) > 0): self.kmbMsgCounter = self.kmbMsgCounter + 1 wcv_volume = mp_slipmap.SlipCircle("well_clear_volume", 3,\ (self.module('map').lat,self.module('map').lon),\ self.radius,\ (0, 0, 255), linewidth=2) self.mpstate.map.add_object(wcv_volume) if (self.kmbMsgCounter == 95): self.Bands = [] if m.get_type() == "ICAROUS_KINEMATIC_BANDS": self.kmbMsgCounter = 0 self.oldNumBands = self.numBands self.numBands = m.numBands numBands = 0 numBands = numBands + 1 self.Bands = [] if (numBands <= self.numBands): low = m.min1 high = m.max1 bands = [low, high, m.type1] self.Bands.append(bands) numBands = numBands + 1 if (numBands <= self.numBands): low = m.min2 high = m.max2 bands = [low, high, m.type2] self.Bands.append(bands) numBands = numBands + 1 if (numBands <= self.numBands): low = m.min3 high = m.max3 bands = [low, high, m.type3] self.Bands.append(bands) numBands = numBands + 1 if (numBands <= self.numBands): low = m.min4 high = m.max4 bands = [low, high, m.type4] self.Bands.append(bands) numBands = numBands + 1 if (numBands <= self.numBands): low = m.min5 high = m.max5 bands = [low, high, m.type5] self.Bands.append(bands) numBands = numBands + 1 if (self.oldNumBands > self.numBands) or self.kmbMsgCounter == 100: for i in range(self.oldNumBands): self.mpstate.map.remove_object("band" + str(i)) for i, kmb in enumerate(self.Bands): self.AddBand(i, kmb) if m.get_type() == "TRAFFIC_INFO": print m.breach_status if m.get_type() == "COMMAND_LONG": if self.V2V: self.show_received_traffic(m)
def mavlink_packet(self, m): '''handle an incoming mavlink packet''' if m.get_type() == "HEARTBEAT": from pymavlink import mavutil if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]: self.vehicle_type_name = 'plane' elif m.type in [ mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_TYPE_SURFACE_BOAT, mavutil.mavlink.MAV_TYPE_SUBMARINE ]: self.vehicle_type_name = 'rover' elif m.type in [ mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_COAXIAL, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER, mavutil.mavlink.MAV_TYPE_HELICOPTER ]: self.vehicle_type_name = 'copter' elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: self.vehicle_type_name = 'antenna' # this is the beginnings of allowing support for multiple vehicles # in the air at the same time vehicle = 'Vehicle%u' % m.get_srcSystem() if m.get_type() == "SIMSTATE" and self.map_settings.showsimpos: self.create_vehicle_icon('Sim' + vehicle, 'green') self.mpstate.map.set_position('Sim' + vehicle, (m.lat * 1.0e-7, m.lng * 1.0e-7), rotation=math.degrees(m.yaw)) if m.get_type() == "AHRS2" and self.map_settings.showahrs2pos: self.create_vehicle_icon('AHRS2' + vehicle, 'blue') self.mpstate.map.set_position('AHRS2' + vehicle, (m.lat * 1.0e-7, m.lng * 1.0e-7), rotation=math.degrees(m.yaw)) if m.get_type() == "GPS_RAW_INT" and self.map_settings.showgpspos: (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7) if lat != 0 or lon != 0: self.create_vehicle_icon('GPS' + vehicle, 'blue') self.mpstate.map.set_position('GPS' + vehicle, (lat, lon), rotation=m.cog * 0.01) if m.get_type() == "GPS2_RAW" and self.map_settings.showgps2pos: (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7) if lat != 0 or lon != 0: self.create_vehicle_icon('GPS2' + vehicle, 'green') self.mpstate.map.set_position('GPS2' + vehicle, (lat, lon), rotation=m.cog * 0.01) if m.get_type() == 'GLOBAL_POSITION_INT': (self.lat, self.lon, self.heading) = (m.lat * 1.0e-7, m.lon * 1.0e-7, m.hdg * 0.01) if self.lat != 0 or self.lon != 0: self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True) self.mpstate.map.set_position('Pos' + vehicle, (self.lat, self.lon), rotation=self.heading) if m.get_type() == "NAV_CONTROLLER_OUTPUT": if self.master.flightmode in ["AUTO", "GUIDED", "LOITER", "RTL"]: trajectory = [(self.lat, self.lon), mp_util.gps_newpos(self.lat, self.lon, m.target_bearing, m.wp_dist)] self.mpstate.map.add_object( mp_slipmap.SlipPolygon('trajectory', trajectory, layer='Trajectory', linewidth=2, colour=(255, 0, 180))) else: self.mpstate.map.add_object( mp_slipmap.SlipClearLayer('Trajectory')) # if the waypoints have changed, redisplay last_wp_change = self.module('wp').wploader.last_change if self.wp_change_time != last_wp_change and abs(time.time() - last_wp_change) > 1: self.wp_change_time = last_wp_change self.display_waypoints() #this may have affected the landing lines from the rally points: self.rally_change_time = time.time() # if the fence has changed, redisplay if self.fence_change_time != self.module( 'fence').fenceloader.last_change: self.display_fence() # if the rallypoints have changed, redisplay if self.rally_change_time != self.module( 'rally').rallyloader.last_change: self.rally_change_time = self.module( 'rally').rallyloader.last_change icon = self.mpstate.map.icon('rallypoint.png') self.mpstate.map.add_object( mp_slipmap.SlipClearLayer('RallyPoints')) for i in range(self.module('rally').rallyloader.rally_count()): rp = self.module('rally').rallyloader.rally_point(i) popup = MPMenuSubMenu( 'Popup', items=[ MPMenuItem('Rally Remove', returnkey='popupRallyRemove'), MPMenuItem('Rally Move', returnkey='popupRallyMove') ]) self.mpstate.map.add_object( mp_slipmap.SlipIcon('Rally %u' % (i + 1), (rp.lat * 1.0e-7, rp.lng * 1.0e-7), icon, layer='RallyPoints', rotation=0, follow=False, popup_menu=popup)) loiter_rad = self.get_mav_param('WP_LOITER_RAD') if self.map_settings.rallycircle: self.mpstate.map.add_object( mp_slipmap.SlipCircle( 'Rally Circ %u' % (i + 1), 'RallyPoints', (rp.lat * 1.0e-7, rp.lng * 1.0e-7), abs(loiter_rad), (255, 255, 0), 2)) #draw a line between rally point and nearest landing point nearest_land_wp = None nearest_distance = 10000000.0 for j in range(self.module('wp').wploader.count()): w = self.module('wp').wploader.wp(j) if (w.command == 21): #if landing waypoint #get distance between rally point and this waypoint dis = mp_util.gps_distance(w.x, w.y, rp.lat * 1.0e-7, rp.lng * 1.0e-7) if (dis < nearest_distance): nearest_land_wp = w nearest_distance = dis if nearest_land_wp != None: points = [] #tangential approach? if self.get_mav_param('LAND_BREAK_PATH') == 0: theta = math.degrees( math.atan(loiter_rad / nearest_distance)) tan_dis = math.sqrt(nearest_distance * nearest_distance - (loiter_rad * loiter_rad)) ral_bearing = mp_util.gps_bearing( nearest_land_wp.x, nearest_land_wp.y, rp.lat * 1.0e-7, rp.lng * 1.0e-7) points.append( mp_util.gps_newpos(nearest_land_wp.x, nearest_land_wp.y, ral_bearing + theta, tan_dis)) else: #not tangential approach points.append((rp.lat * 1.0e-7, rp.lng * 1.0e-7)) points.append((nearest_land_wp.x, nearest_land_wp.y)) self.mpstate.map.add_object( mp_slipmap.SlipPolygon('Rally Land %u' % (i + 1), points, 'RallyPoints', (255, 255, 0), 2)) # check for any events from the map self.mpstate.map.check_events()
def mavlink_packet_obstacle_distance(self, vehicle, m): heading = self.heading_for_vehicle.get(vehicle, 0) tlayer = "Obstacle Distance for %u.%u type=%u" % ( m.get_srcSystem(), m.get_srcComponent(), m.sensor_type) if m.get_srcSystem() in self.lat_lon_for_vehicle: color = (255, 0, 255) (lat, lon) = self.lat_lon_for_vehicle[m.get_srcSystem()] self.foreach_map(lambda a_map: a_map.add_object( mp_slipmap.SlipClearLayer(tlayer))) increment = m.increment_f if increment == 0: increment = float(m.increment) measurement_count = 0 for i in range(0, 72): if m.distances[i] != 65535: measurement_count += 1 fov = measurement_count * increment start_angle = -increment / 2.0 end_angle = increment / 2.0 rotation_start = -90 + heading for i in range(0, 72): slipkey = '%s-POS%u' % (tlayer, i) if m.distances[i] == m.max_distance + 1: # no measurement self.foreach_map( lambda a_map: a_map.remove_object(slipkey)) measurement_count += 1 continue distance = m.distances[i] / 100.0 # cm -> m circle = mp_slipmap.SlipCircle( slipkey, 3, (lat, lon), distance, color, linewidth=3, start_angle=start_angle, end_angle=end_angle, rotation=(m.angle_offset + rotation_start + i * increment) % 360, ) if m.distances[i] < m.min_distance: measurement_count += 1 self.foreach_map(lambda a_map: a_map.add_object(circle)) slipkey = "%s-range" % tlayer if fov == 360: # perfect circle # add a circle for max-range self.foreach_map(lambda a_map: a_map.add_object( mp_slipmap.SlipCircle( slipkey, 3, (lat, lon), m.max_distance / 100.0, color, linewidth=1, ))) else: # add an arc for max-range self.foreach_map(lambda a_map: a_map.add_object( mp_slipmap.SlipCircle( slipkey, 3, (lat, lon), m.max_distance / 100.0, color, linewidth=1, start_angle=-fov / 2, end_angle=fov / 2, rotation=(m.angle_offset + -90 + (heading)) % 360, add_radii=True, )))