PATH_PLANNING: Added area spread behavior (potential field based)

ap_path_planning/src/ap_path_planning/area_spread_plugin.py

+#!/usr/bin/env python
+
+# Swarm behavior that uses a potential field to uniformly spread
+# the swarm throughout a rectangular box specified by its southwest
+# corner (L/L), length, width, and orientation (radians).  Patrol
+# is conducted by generating a series of random waypoints within the box.
+#
+# Duane Davis 2018
+
+# Standard Python libraries
+import math
+
+# ACS imports
+import ap_lib.plugin_behavior as plugin
+import ap_lib.bitmapped_bytes as bytes
+import ap_lib.gps_utils as gps
+import ap_lib.math_utils as mu
+
+# File-specific constants
+Q_STAR = 300.0     # Max relevant distance for potential field calculations
+ETA_AREA = 1.0     # Scaling coefficient for area boundary spacing
+ETA_UAV = 3.0      # Scaling coefficient for spacing between UAVs
+LOOKAHEAD = 500.0  # Forward displacement of computed potential field waypoint
+
+
+class AreaSpread(plugin.PluginBehavior):
+    ''' Swarm behavior that uses a potential field to uniformly spread
+    the swarm throughout a rectangular box specified by its southwest
+    corner (L/L), length, width, and orientation (radians).  Patrol
+    is conducted by generating a series of random waypoints within the box.
+
+    Class member variables:
+      _geo_box: geographic area (rectangular) to be patrolled
+      _center_point: latitude/longitude of the geo_box center point
+      _sw_corner: latitude/longitude of the geo_box's southwest corner
+      _cartesian_corners: geo_box corners as Cartesian coordinates
+
+    Class member functions:
+      parameterize: implementation of the Behavior virtual function
+      compute_command: runs one iteration of the behavior's control loop
+
+    Inherited from PluginBehavior
+      set_manager: sets the manager object for this object
+      get_name: returns the string name of the behavior
+      set_ready: safely sets the ready state to True or False
+      is_ready: returns the behavior's current readiness state
+      set_active: safely activates or deactivates the behavior
+      is_active: returns the behavior's current active state
+      activation_time: returns time at which the behavior was activated
+      time_since_activation: returns elapsed time since activation
+      get_status: returns the behavior's current ready and active status
+    '''
+
+    def __init__(self, behavior_id, behavior_name, manager=None):
+        ''' Class initializer initializes class variables.
+        @param behavior_id: unique identifier for this behavior
+        @param behavior_name: string name of this behavior
+        @param manager: BehaviorManager object to which this behavior belongs
+        '''
+        plugin.PluginBehavior.__init__(self, behavior_id, behavior_name, manager)
+        self._geo_box = None
+        self._center_point = None
+        self._sw_corner = None
+        self._cartesian_corners = None
+
+
+    #-------------------------------------------------
+    # Implementation of parent class virtual functions
+    #-------------------------------------------------
+
+    def parameterize(self, params):
+        ''' Sets behavior parameters based on set service parameters
+        @param params: byte array from the set service request
+        @return True if set with valid parameters
+        '''
+        try:
+            parser = bytes.GeoBoxParser()
+            parser.unpack(params)
+            self._geo_box = \
+                gps.GeoBox(parser.latitude, parser.longitude, \
+                           parser.length, parser.width, parser.orientation)
+            self._center_point = self._geo_box.center_point()            
+            self._sw_corner = (parser.latitude, parser.longitude)
+            self._cartesian_corners = self._geo_box.get_cartesian_corners()
+            self.manager.log_info("Area Spread behavior set")
+            self.set_ready(True)
+        except Exception as ex:
+            self.manager.log_warn("Exception trying to set area_spread behavior: %s" \
+                          %str(ex))
+            self.set_ready(False)
+        return self.is_ready()
+
+
+    def compute_command(self):
+        ''' Executes one iteration of the behavior
+        Uses the position of other UAVs and the area boundaries relative to
+        this UAV to derive a potential field .
+        '''
+        own_posit = self.manager.get_own_state().state.pose.pose.position
+
+        # If we get outside of the area, drive directly back in
+        if not self._geo_box.geography_contains(own_posit.lat, own_posit.lon):
+            self.manager.wp_cmd_msg.lat = self._center_point[0]
+            self.manager.wp_cmd_msg.lon = self._center_point[1]
+            self.manager.wp_cmd_msg.alt = self.manager.ap_wpt.z
+            return self.manager.wp_cmd_msg
+
+        # Accumulators for the potential field values
+        (f_x, f_y) = (0.0, 0.0)
+        ownXY = gps.cartesian_offset(self._sw_corner[0], self._sw_corner[1], \
+                                     own_posit.lat, own_posit.lon)
+
+        # Compute the area boundary potential field values
+        for edge_num in range(0, 4):
+            end1 = self._cartesian_corners[edge_num]
+            end2 = self._cartesian_corners[(edge_num+1)%4]
+            boundaryXY = mu.closest_segment_pt(end1, end2, ownXY)
+            f = self.__repulsive_force(ETA_AREA, boundaryXY, ownXY)
+            f_x += f[0]
+            f_y += f[1]
+
+        # Compute the other UAV potential field values
+        for uav_id in self.manager.subswarm_keys:
+            if uav_id == self.manager.own_id: continue
+            uav_posit = self.manager.get_swarm_vehicle_state(uav_id).state.pose.pose.position
+            uavXY = gps.cartesian_offset(self._sw_corner[0], self._sw_corner[1], \
+                                         uav_posit.lat, uav_posit.lon)
+            f = self.__repulsive_force(ETA_UAV, uavXY, ownXY)
+            f_x += f[0]
+            f_y += f[1]
+
+        (self.manager.wp_cmd_msg.lat, self.manager.wp_cmd_msg.lon) = \
+            gps.gps_newpos(own_posit.lat, own_posit.lon, \
+                           math.atan2(f_y, f_x), LOOKAHEAD)
+        return self.manager.wp_cmd_msg
+
+
+    #-----------------------------------
+    # Behavior-specific member functions
+    #-----------------------------------
+
+    def __repulsive_force(self, eta, obstacle, vehicle):
+        ''' Computes a repulsive potential field value for an obstacle
+        and vehicle, both of which are specified in Cartesian space
+        @param eta: multiple to be applied to the calculation
+        @param obstacle: Cartesian coordinates of the obstacle
+        @param vehicle: Cartesian coordinates of the vehicle
+        @return tuple containing the north/east repulsive force
+        '''
+        d = mu.cartesian_distance(obstacle, vehicle)
+        if d >= Q_STAR:
+            return (0.0, 0.0)
+
+        f_mult = eta * (1.0/Q_STAR - 1.0/d) * (1.0/d**2)
+        (x_diff, y_diff) = ( (obstacle[0] - vehicle[0], \
+                              obstacle[1] - vehicle[1]) )
+        denom = math.sqrt(x_diff*x_diff + y_diff*y_diff)
+        result = ( (f_mult * (x_diff / denom)), (f_mult * (y_diff / denom)) )
+        return result
+