mavproxy_map: inclusion/exclusion circles and polygons

mavproxy_map: SlipCircle handles clicked callback

Allows circles to be right-clicked and manipulated.

mavproxy_map: mp_slipmap_util.py: add UnclosedSlipPolygon

Variant of the existing SlipPolygon which doesn't assume it can discern
item types and doesn't assume a closing point on the polygon.

mavproxy_map: add support for drawing MissionItemProtocol fences

mavproxy_map: add option to insert polygon points

mavproxy_map: add option to draw polyfences on map

mavproxy_map: hide remove-fence option

MAVProxy/modules/mavproxy_map/__init__.py

@@ -32,6 +32,7 @@ def __init__(self, mpstate):
         self.moving_fencepoint = None
         self.moving_rally = None
         self.mission_list = None
+        self.moving_polygon_point = None
         self.icon_counter = 0
         self.circle_counter = 0
         self.draw_line = None
@@ -259,6 +260,7 @@ def display_waypoints(self):
                 items = [MPMenuItem('WP Set', returnkey='popupMissionSet'),
                          MPMenuItem('WP Remove', returnkey='popupMissionRemove'),
                          MPMenuItem('WP Move', returnkey='popupMissionMove'),
+                         MPMenuItem('WP Split', returnkey='popupMissionSplit'),
                 ]
                 popup = MPMenuSubMenu('Popup', items)
                 self.map.add_object(mp_slipmap.SlipPolygon('mission %u' % i, p,
@@ -296,10 +298,132 @@ def display_waypoints(self):
 
                     labeled_wps[next_list[j]] = (i,j)
 
+    # Start: handling of PolyFence popup menu items
+    def polyfence_remove_circle(self, id):
+        '''called when a fence is right-clicked and remove is selected;
+        removes the circle
+        '''
+        (seq, type) = id.split(":")
+        self.module('fence').removecircle(int(seq))
+
+    def polyfence_remove_polygon(self, id):
+        '''called when a fence is right-clicked and remove is selected;
+        removes the polygon
+        '''
+        (seq, type) = id.split(":")
+        self.module('fence').removepolygon(int(seq))
+
+    def polyfence_remove_polygon_point(self, id, extra):
+        '''called when a fence is right-clicked and remove point is selected;
+        removes the polygon point
+        '''
+        (seq, type) = id.split(":")
+        self.module('fence').removepolygon_point(int(seq), int(extra))
+
+    def polyfence_add_polygon_point(self, id, extra):
+        '''called when a fence is right-clicked and add point is selected;
+        adds a polygon *before* this one in the list
+        '''
+        (seq, type) = id.split(":")
+        self.module('fence').addpolygon_point(int(seq), int(extra))
+
+    def polyfence_move_polygon_point(self, id, extra):
+        '''called when a fence is right-clicked and move point is selected; start
+        moving the polygon point
+        '''
+        (seq, t) = id.split(":")
+        self.moving_polygon_point = (int(seq), extra)
+    # End: handling of PolyFence popup menu items
+
+    def display_polyfences_circles(self, circles, colour):
+        '''draws circles in the PolyFence layer with colour colour'''
+        for circle in circles:
+            lat = circle.x
+            lng = circle.y
+            if circle.get_type() == 'MISSION_ITEM_INT':
+                lat *= 1e-7
+                lng *= 1e-7
+            items = [
+                MPMenuItem('Remove Circle', returnkey='popupPolyFenceRemoveCircle'),
+            ]
+            popup = MPMenuSubMenu('Popup', items)
+            slipcircle = mp_slipmap.SlipCircle(
+                str(circle.seq) + ":circle", # key
+                "PolyFence", # layer
+                (lat, lng), # latlon
+                circle.param1, # radius
+                colour,
+                linewidth=2,
+                popup_menu=popup)
+            self.map.add_object(slipcircle)
+
+    def display_polyfences_inclusion_circles(self):
+        '''draws inclusion circles in the PolyFence layer with colour colour'''
+        inclusions = self.module('fence').inclusion_circles()
+        self.display_polyfences_circles(inclusions, (0, 255, 0))
+
+    def display_polyfences_exclusion_circles(self):
+        '''draws exclusion circles in the PolyFence layer with colour colour'''
+        exclusions = self.module('fence').exclusion_circles()
+        self.display_polyfences_circles(exclusions, (255, 0, 0))
+
+    def display_polyfences_polygons(self, polygons, colour):
+        '''draws polygons in the PolyFence layer with colour colour'''
+        for polygon in polygons:
+            points = []
+            for point in polygon:
+                lat = point.x
+                lng = point.y
+                if point.get_type() == 'MISSION_ITEM_INT':
+                    lat *= 1e-7
+                    lng *= 1e-7
+                points.append((lat, lng))
+            items = [
+                MPMenuItem('Remove Polygon', returnkey='popupPolyFenceRemovePolygon'),
+            ]
+            if len(points) > 3:
+                items.append(MPMenuItem('Remove Polygon Point', returnkey='popupPolyFenceRemovePolygonPoint'))
+            items.append(MPMenuItem('Move Polygon Point', returnkey='popupPolyFenceMovePolygonPoint'))
+            items.append(MPMenuItem('Add Polygon Point', returnkey='popupPolyFenceAddPolygonPoint'))
+
+            popup = MPMenuSubMenu('Popup', items)
+            poly = mp_slipmap.UnclosedSlipPolygon(
+                str(polygon[0].seq) + ":poly", # key,
+                points,
+                layer='PolyFence',
+                linewidth=2,
+                colour=colour,
+                popup_menu=popup)
+            self.map.add_object(poly)
+
+    def display_polyfences_inclusion_polygons(self):
+        '''draws inclusion polygons in the PolyFence layer with colour colour'''
+        inclusions = self.module('fence').inclusion_polygons()
+        self.display_polyfences_polygons(inclusions, (0, 255, 0))
+
+    def display_polyfences_exclusion_polygons(self):
+        '''draws exclusion polygons in the PolyFence layer with colour colour'''
+        exclusions = self.module('fence').exclusion_polygons()
+        self.display_polyfences_polygons(exclusions, (255, 0, 0))
+
+    def display_polyfences(self):
+        '''draws PolyFence items in the PolyFence layer'''
+        self.map.add_object(mp_slipmap.SlipClearLayer('PolyFence'))
+        self.display_polyfences_inclusion_circles()
+        self.display_polyfences_exclusion_circles()
+        self.display_polyfences_inclusion_polygons()
+        self.display_polyfences_exclusion_polygons()
+
     def display_fence(self):
         '''display the fence'''
         from MAVProxy.modules.mavproxy_map import mp_slipmap
-        self.fence_change_time = self.module('fence').fenceloader.last_change
+        if getattr(self.module('fence'), "cmd_addcircle", None) is not None:
+            # we're doing fences via MissionItemProtocol and thus have
+            # much more work to do
+            return self.display_polyfences()
+
+        # traditional module, a single polygon fence transfered using
+        # FENCE_POINT protocol:
         points = self.module('fence').fenceloader.polygon()
         self.map.add_object(mp_slipmap.SlipClearLayer('Fence'))
         if len(points) > 1:
@@ -374,6 +498,11 @@ def remove_mission(self, key, selection_index):
         idx = self.selection_index_to_idx(key, selection_index)
         self.mpstate.functions.process_stdin('wp remove %u' % idx)
 
+    def split_mission_wp(self, key, selection_index):
+        '''add wp before this one'''
+        idx = self.selection_index_to_idx(key, selection_index)
+        self.mpstate.functions.process_stdin('wp split %u' % idx)
+
     def remove_fencepoint(self, key, selection_index):
         '''remove a fence point'''
         self.mpstate.functions.process_stdin('fence remove %u' % (selection_index+1))
@@ -408,10 +537,20 @@ def handle_menu_event(self, obj):
             self.remove_mission(obj.selected[0].objkey, obj.selected[0].extra_info)
         elif menuitem.returnkey == 'popupMissionMove':
             self.move_mission(obj.selected[0].objkey, obj.selected[0].extra_info)
-        elif menuitem.returnkey == 'popupFenceRemove':
-            self.remove_fencepoint(obj.selected[0].objkey, obj.selected[0].extra_info)
+        elif menuitem.returnkey == 'popupMissionSplit':
+            self.split_mission_wp(obj.selected[0].objkey, obj.selected[0].extra_info)
         elif menuitem.returnkey == 'popupFenceMove':
             self.move_fencepoint(obj.selected[0].objkey, obj.selected[0].extra_info)
+        elif menuitem.returnkey == 'popupPolyFenceRemoveCircle':
+            self.polyfence_remove_circle(obj.selected[0].objkey)
+        elif menuitem.returnkey == 'popupPolyFenceRemovePolygon':
+            self.polyfence_remove_polygon(obj.selected[0].objkey)
+        elif menuitem.returnkey == 'popupPolyFenceMovePolygonPoint':
+            self.polyfence_move_polygon_point(obj.selected[0].objkey, obj.selected[0].extra_info)
+        elif menuitem.returnkey == 'popupPolyFenceAddPolygonPoint':
+            self.polyfence_add_polygon_point(obj.selected[0].objkey, obj.selected[0].extra_info)
+        elif menuitem.returnkey == 'popupPolyFenceRemovePolygonPoint':
+            self.polyfence_remove_polygon_point(obj.selected[0].objkey, obj.selected[0].extra_info)
         elif menuitem.returnkey == 'showPosition':
             self.show_position()
         elif menuitem.returnkey == 'printGoogleMapsLink':
@@ -450,6 +589,16 @@ def map_callback(self, obj):
             print("Cancelled wp move")
             self.moving_wp = None
             return
+        if obj.event.leftIsDown and self.moving_polygon_point is not None:
+            self.mpstate.click(obj.latlon)
+            (seq, offset) = self.moving_polygon_point
+            self.mpstate.functions.process_stdin("fence movepolypoint %u %u" % (int(seq), int(offset)))
+            self.moving_polygon_point = None
+            return
+        if obj.event.rightIsDown and self.moving_polygon_point is not None:
+            print("Cancelled polygon point move")
+            self.moving_polygon_point = None
+            return
         if obj.event.rightIsDown and self.moving_fencepoint is not None:
             print("Cancelled fence move")
             self.moving_fencepoint = None
@@ -509,7 +658,7 @@ def drawing_update(self):
         self.draw_line.append(self.mpstate.click_location)
         if len(self.draw_line) > 1:
             self.map.add_object(mp_slipmap.SlipPolygon('drawing', self.draw_line,
-                                                          layer='Drawing', linewidth=2, colour=(128,128,255)))
+                                                          layer='Drawing', linewidth=2, colour=self.draw_colour))
 
     def drawing_end(self):
         '''end line drawing'''
@@ -521,10 +670,11 @@ def drawing_end(self):
         self.map.add_object(mp_slipmap.SlipDefaultPopup(self.default_popup, combine=True))
         self.map.add_object(mp_slipmap.SlipClearLayer('Drawing'))
 
-    def draw_lines(self, callback):
+    def draw_lines(self, callback, colour=(128,128,255)):
         '''draw a series of connected lines on the map, calling callback when done'''
         from MAVProxy.modules.mavproxy_map import mp_slipmap
         self.draw_callback = callback
+        self.draw_colour = colour
         self.draw_line = []
         self.map.add_object(mp_slipmap.SlipDefaultPopup(None))
 
@@ -825,7 +975,8 @@ def mavlink_packet(self, m):
 
         # if the fence has changed, redisplay
         if (self.module('fence') and
-            self.fence_change_time != self.module('fence').fenceloader.last_change):
+            self.fence_change_time != self.module('fence').last_change()):
+            self.fence_change_time = self.module('fence').last_change()
             self.display_fence()
 
         # if the rallypoints have changed, redisplay

MAVProxy/modules/mavproxy_map/mp_slipmap_util.py

@@ -185,13 +185,32 @@ def draw(self, img, pixmapper, bounds):
                       self.color, self.linewidth, 0, reverse = self.reverse).draw(img)
             SlipArrow(self.key, self.layer, (center_px[0]+radius_px, center_px[1]),
                       self.color, self.linewidth, math.pi, reverse = self.reverse).draw(img)
+        # stash some values for determining closest click location
+        self.radius_px = radius_px
+        self.center_px = center_px
 
     def bounds(self):
         '''return bounding box'''
         if self.hidden:
             return None
         return (self.latlon[0], self.latlon[1], 0, 0)
 
+    def clicked(self, px, py):
+        '''check if a click on px,py should be considered a click
+        on the object. Return None if definitely not a click,
+        otherwise return the distance of the click, smaller being nearer
+        '''
+        radius_px = getattr(self, "radius_px", None)
+        if radius_px is None:
+            return
+
+        dx = self.center_px[0] - px
+        dy = self.center_px[1] - py
+        ret = abs(dx*dx+dy*dy - radius_px*radius_px)
+        if ret > 100:  # threshold of within 10 pixels for a click to count
+            return None
+        return ret
+
 class SlipPolygon(SlipObject):
     '''a polygon to display on the map'''
     def __init__(self, key, points, layer, colour, linewidth, arrow = False, popup_menu=None, showlines=True):
@@ -276,6 +295,31 @@ def selection_info(self):
         '''extra selection information sent when object is selected'''
         return self._selected_vertex
 
+class UnclosedSlipPolygon(SlipPolygon):
+    '''a polygon to display on the map - but one with no return point or
+    closing vertex'''
+    def draw(self, img, pixmapper, bounds, colour=(0,0,0)):
+        '''draw a polygon on the image'''
+        if self.hidden:
+            return
+        self._pix_points = []
+        for i in range(len(self.points)):
+            if len(self.points[i]) > 2:
+                colour = self.points[i][2]
+            else:
+                colour = self.colour
+            _from = self.points[i]
+            if i+1 == len(self.points):
+                _to = self.points[0]
+            else:
+                _to = self.points[i+1]
+            self.draw_line(
+                img,
+                pixmapper,
+                _from,
+                _to,
+                colour,
+                self.linewidth)
 
 class SlipGrid(SlipObject):
     '''a map grid'''