TASKS: Added landing point deconfliction to the task runner (takeoff task)

Reworked some of the swarm-state transition detections to use the
topic published by swarm manager instead of a ros parameter set by
the autopilot node (this makes it easier to add tasks for all state
transitions rather than just between flight ready and preflight).

ap_tasks/src/task_runner.py

@@ -14,18 +14,20 @@ import os
 import subprocess
 from threading import RLock
 import time
+import math
 
 # General ROS imports
 import rospy
 
 # Import ROS message and service types
-import std_msgs
-from autopilot_bridge import msg as autopilot_msg
-from autopilot_bridge import srv as autopilot_srv
-from ap_msgs import msg as payload_msg
-from ap_srvs import srv as payload_srv
-
+import std_msgs.msg as std_msgs
 import ap_lib.ap_enumerations as enums
+import ap_lib.gps_utils as gps
+import autopilot_bridge.msg as autopilot_msg
+import autopilot_bridge.srv as autopilot_srv
+import ap_msgs.msg as payload_msg
+import ap_srvs.srv as payload_srv
+
 
 #-----------------------------------------------------------------------
 # Class to encapsulate a task that may have actions at different events
@@ -84,6 +86,10 @@ class Task(object):
     def on_shutdown(self):
         return None
 
+    # Called when ROS indicates that the UAV has launched
+    def on_takeoff(self):
+        return None
+
 #-----------------------------------------------------------------------
 # Task-running and event-handling class
 
@@ -98,15 +104,19 @@ class TaskRunner(object):
 
         # Default "last" ROS message/param instances
         self.last_status = autopilot_msg.Status()
-        self.last_flightready = False
 
         # Flags telling if one-time events have happened yet
         self.done_on_status = False
+        self.done_on_takeoff = False
+        self.done_on_flight_ready = False
 
         # ROS subscriptions (assumes rospy is initialized)
         self.sub_status = rospy.Subscriber("autopilot/status",
                                            autopilot_msg.Status,
                                            self._cb_status)
+        self.sub_status = rospy.Subscriber("swarm_manager/swarm_state",
+                                           std_msgs.UInt8,
+                                           self._cb_swarm_state)
 
     def _log(self, event, task, start, success, info=''):
         end = time.time()
@@ -152,6 +162,17 @@ class TaskRunner(object):
 
         self.last_status = status
 
+    def _cb_swarm_state(self, state):
+        if state.data == enums.FLIGHT_READY and not self.done_on_flight_ready:
+            self._do_event('on_ready')
+            self.done_on_flight_ready = True
+        elif state.data == enums.PRE_FLIGHT and self.done_on_flight_ready:
+            self._do_event('on_not_ready')
+            self.done_on_flight_ready = False
+        elif state.data == enums.INGRESS:
+            self._do_event('on_takeoff')
+            self.done_on_takeoff = True
+
     # Add a Task() to be run
     def add_task(self, task):
         self.tasks.append(task)
@@ -165,15 +186,6 @@ class TaskRunner(object):
         # Wait until ROS shuts down (callbacks will handle other events)
         r = rospy.Rate(1.0)
         while not rospy.is_shutdown():
-            # Check flight ready status
-            flightready = rospy.has_param("flight_ready") and \
-                          rospy.get_param("flight_ready")
-            if flightready and not self.last_flightready:
-                self._do_event('on_ready')
-            if not flightready and self.last_flightready:
-                self._do_event('on_not_ready')
-            self.last_flightready = flightready
-
             # Sleep for a bit, let subscribers run
             r.sleep()
 
@@ -184,6 +196,63 @@ class TaskRunner(object):
 # Task classes
 # NOTE: This should be the only part to be customized
 
+# Compute a deconflicted landing location
+class DeconflictLandTask(Task):
+
+    def __init__(self):
+        Task.__init__(self, "DeconflictLand")
+        swarm_records = None
+
+    def on_takeoff(self):
+        # Only deconflict copter landing points (and only if called for)
+        if rospy.has_param('aircraft_type') and \
+           (rospy.get_param('aircraft_type') == enums.AC_COPTER):
+            spacing = rospy.get_param('dis_between_land_points')
+            direction = math.radians(rospy.get_param('land_point_lineup_dir'))
+            swarm_records = \
+                rospy.wait_for_message("tracker/swarm_uav_states",
+                                       payload_msg.SwarmStateStamped).swarm
+            # The offset position is determined by launch sequence, so this
+            # vehicle goes after every other one that is currently airborne
+            offset_num = 0
+            for record in swarm_records:
+                if (record.vehicle_id != self._acid) and \
+                   (record.vehicle_type == enums.AC_COPTER) and \
+                   (record.swarm_state != enums.PRE_FLIGHT) and \
+                   (record.swarm_state != enums.FLIGHT_READY):
+                    offset_num += 1
+            offset = spacing * ((offset_num+1) // 2)
+            if (offset_num % 2) == 0:
+                offset *= -1.0
+            self.__setup_new_land_points(direction, offset)
+
+    def __setup_new_land_points(self, direction, offset):
+        # NOTE: TO ENSURE LANDING POINTS DON'T CONTINUE TO DRIFT BETWEEN
+        #       MISSIONS, ARDUPILOT MISSIONS MUST BE RESET BEFORE EVERY TAKOFF      
+        rospy.wait_for_service("autopilot/wp_getall")
+        wp_get_all_proxy = \
+            rospy.ServiceProxy("autopilot/wp_getall", autopilot_srv.WPGetAll)
+        rospy.wait_for_service("autopilot/wp_setall")
+        wp_set_all_proxy = \
+            rospy.ServiceProxy("autopilot/wp_setall", autopilot_srv.WPSetAll)
+        wp_list = wp_get_all_proxy().points
+        new_wp_list = []
+        for wp in wp_list:
+            if wp.command == enums.WP_TYPE_LAND:
+                new_pt = gps.gps_newpos(wp.x, wp.y, direction, offset)
+                wp.x = new_pt[0]
+                wp.y = new_pt[1]
+            new_wp_list.append(wp)
+        success = False
+        while not success:
+            success = wp_set_all_proxy(new_wp_list).ok
+        rospy.loginfo("Deconflicted landing points successfully loaded")
+
+    def __setup_new_rally_points(self, offset):
+        # NOTE: TO ENSURE RALLY POINTS DON'T CONTINUE TO DRIFT BETWEEN MISSIONS
+        #       ARDUPILOT RALLY POINTS MUST BE RESET BEFORE EVERY TAKOFF      
+        pass
+
 # Start and stop rosbagging
 class RosbagTask(Task):
 
@@ -366,10 +435,10 @@ class FetchConfigTask(Task):
             # Make sure waypoint 1 is correctly loaded into autopilot memory
             if rospy.get_param('ok_wp'):
                 pub = rospy.Publisher('autopilot/waypoint_goto',
-                                      std_msgs.msg.UInt16,
+                                      std_msgs.UInt16,
                                       queue_size=1,
                                       latch=True)
-                msg = std_msgs.msg.UInt16()
+                msg = std_msgs.UInt16()
                 msg.data = 1
                 pub.publish(msg)
 
@@ -402,6 +471,10 @@ if __name__ == '__main__':
     if rospy.has_param('rosbag_enable') and rospy.get_param('rosbag_enable'):
         runner.add_task(RosbagTask())
 
+    if rospy.has_param('do_deconflict_land') and \
+       rospy.get_param('do_deconflict_land'):
+        runner.add_task(DeconflictLandTask())
+
     # Run the main loop
     runner.run()