Add landing task

param/motion_planner.yaml

@@ -2,3 +2,5 @@ takeoff_velocity        : 2.0
 takeoff_height_tolerance: 0.1
 delay_before_takeoff    : 2.0
 max_takeoff_start_height: 0.5
+land_velocity           : -1.0
+land_height_tolerance   : 0.5

scripts/iarc_task_action_server.py

@@ -3,6 +3,7 @@
 import rospy
 import actionlib
 from takeoff_task import TakeoffTask
+from land_task import LandTask
 from test_task import TestTask
 from iarc7_motion.msg import QuadMoveAction, QuadMoveResult
 
@@ -25,6 +26,7 @@ def __init__(self):
         self._action_server.start()
 
         self._task_dict = {'takeoff': TakeoffTask,
+                           'land': LandTask,
                            'test_task': TestTask}
 
     # Private method

scripts/land_task.py

@@ -0,0 +1,95 @@
+#!/usr/bin/env python
+import rospy
+from abstract_task import AbstractTask
+from task_state import TaskState
+from geometry_msgs.msg import TwistStamped
+
+import rospy
+import tf2_ros
+
+import math
+
+from iarc7_msgs.msg import FlightControllerStatus
+from geometry_msgs.msg import TwistStamped
+
+class LandTaskState:
+    init = 0
+    land = 1
+    disarm = 2
+
+class LandTask(AbstractTask):
+
+    def __init__(self, actionvalues_dict):
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
+        self.canceled = False;
+
+        try:
+            self.LAND_VELOCITY = rospy.get_param('~land_velocity')
+            self.LAND_HEIGHT_TOLERANCE = rospy.get_param('~land_height_tolerance')
+        except KeyError as e:
+            rospy.logerr('Could not lookup a parameter for takeoff task')
+            raise
+
+        self.fc_status = None
+        self.fc_status_sub = rospy.Subscriber('fc_status', FlightControllerStatus, self._receive_fc_status)
+        self.state = LandTaskState.init
+        self.disarm_request_success = False
+
+    def _receive_fc_status(self, data):
+        self.fc_status = data
+
+    def disarm_callback(self, data):
+        self.disarm_request_success = data.success
+
+    def get_desired_command(self):
+        if self.canceled:
+            return (TaskState.canceled)
+
+        if self.state == LandTaskState.init:
+            if self.fc_status != None:
+                # Check that auto pilot is enabled
+                if not self.fc_status.auto_pilot:
+                    return (TaskState.failed, 'flight controller not allowing auto pilot')
+                # Check that the FC is not already armed
+                elif not self.fc_status.armed:
+                    return (TaskState.failed, 'flight controller is not armed, how are we in the air??')
+                # All is good change state to land
+                else:
+                    self.state = LandTaskState.land
+            else:
+                return (TaskState.running, 'nop')
+
+        # Enter the takeoff phase
+        if self.state == LandTaskState.land:
+            try:
+                transStamped = self.tf_buffer.lookup_transform('map', 'quad', rospy.Time(0))
+                # Check if we reached the target height
+                if(transStamped.transform.translation.z < self.LAND_HEIGHT_TOLERANCE):
+                    self.state = LandTaskState.disarm
+
+                velocity = TwistStamped()
+                velocity.header.frame_id = 'level_quad'
+                velocity.header.stamp = rospy.Time.now()
+                velocity.twist.linear.z = self.LAND_VELOCITY
+                return (TaskState.running, 'velocity', velocity)
+
+            except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex:
+                rospy.logerr('Takeofftask: Exception when looking up transform')
+                rospy.logerr(ex.message)
+                return (TaskState.aborted, 'Exception when looking up transform during takeoff')
+
+        # Enter the disarming request stage
+        if self.state == LandTaskState.disarm:
+            # Check if disarm succeeded
+            if self.disarm_request_success:
+                return (TaskState.done, 'velocity', TwistStamped())
+            else:
+                return (TaskState.running, 'disarm', self.disarm_callback)
+
+        # Impossible state reached
+        return (TaskState.aborted, 'Impossible state in takeoff task reached')
+
+    def cancel(self):
+        rospy.loginfo('TakeoffTask canceled')
+        self.canceled = True

scripts/motion_planner.py

@@ -24,19 +24,24 @@ def run(self):
             if(task_command[0] == 'velocity'):
                 self.publish_twist(task_command[1])
             elif(task_command[0] == 'arm'):
-                armed = False
-                try:
-                    armed = self.arm_service(True)
-                except rospy.ServiceException as exc:
-                    print("Could not arm: " + str(exc))
-                task_command[1](armed)
+                task_command[1](self.use_arm_service(True))
+            elif(task_command[0] == 'disarm'):
+                task_command[1](self.use_arm_service(False))
             elif(task_command[0] == 'nop'):
                 self.publish_twist(TwistStamped())
             else:
                 rospy.logerr('Unkown command request: %s, noping', task_command[0])
                 self.publish_twist(TwistStamped())
             rate.sleep()
 
+    def use_arm_service(self, arm):
+        try:
+            armed = self.arm_service(arm)
+        except rospy.ServiceException as exc:
+            rospy.logerr("Could not arm: " + str(exc))
+            armed = False
+        return armed
+
     def publish_twist(self, twist):
         velocity_msg = TwistStampedArrayStamped()
         velocity_msg.header.stamp = rospy.Time.now()

scripts/test_motion_planner.py

@@ -21,7 +21,15 @@ def motion_planner_client():
     client.send_goal(goal)
     # Waits for the server to finish performing the action.
     client.wait_for_result()
-    rospy.loginfo("Request/wait for request success: {}".format(client.get_result()))
+    rospy.logwarn("Takeoff success: {}".format(client.get_result()))
+
+    # Test takeoff
+    goal = QuadMoveGoal(movement_type="land")
+    # Sends the goal to the action server.
+    client.send_goal(goal)
+    # Waits for the server to finish performing the action.
+    client.wait_for_result()
+    rospy.logwarn("Land success: {}".format(client.get_result()))
 
 if __name__ == '__main__':
     try: