Work on using missions, not finished yet.

lrs_ardupilot/ardu_manager.py

@@ -30,7 +30,7 @@ from rclpy.action import ActionServer
 from lrs_msgs_common.srv import InAirCheck, GetHomePosition, GetCurrentPosition, SignalGoal, GimbalCheck
 from lrs_msgs_common.action import FlyPath, Land, TakeOff
 
-from dronekit import connect, VehicleMode, LocationGlobal
+from dronekit import connect, VehicleMode, LocationGlobal, Command, mavutil
 from mavsdk import System
 
 from lrs_util.coordtrans import CoordTrans
@@ -79,11 +79,11 @@ class ArduManager(Node):
 
 
     def timer_callback(self):
-        self.vehicle.parameters["SIM_BATT_CAPACITY"]=10000
+        ### self.vehicle.parameters["SIM_BATT_CAPACITY"]=10000
         calt = self.vehicle.location.global_frame.alt
         rcalt = self.vehicle.location.global_relative_frame.alt
-        self.get_logger().info(f'Global frame alt: {calt}')
-        self.get_logger().info(f'Global relative frame alt: {rcalt}')
+        #self.get_logger().info(f'Global frame alt: {calt}')
+        #self.get_logger().info(f'Global relative frame alt: {rcalt}')
         msg = TakeoffInfo()
         msg.latitude = 0.0
         msg.longitude = 0.0
@@ -237,7 +237,70 @@ class ArduManager(Node):
         result.success = True
         result.message = ""
         return result
+
+    def start_mission(self):
+        self.vehicle.commands.download()
+        self.vehicle.commands.wait_ready()
+        print("MISSION DOWNLOADED")
+        self.vehicle.commands.upload()
+        self.vehicle.commands.wait_ready()
+        print("MISSION UPLOADED")
+        res = self.set_mode("AUTO")
+        return res
+
+    def set_mode(self, new_mode: str) -> bool:
+        print(f"{self.vehicle.mode.name} {new_mode}")
+        if self.vehicle.mode.name == new_mode:
+            return True
+        else:
+            loop = 0
+            timeout_count = 5
+
+            while self.vehicle.mode.name != new_mode:
+                self.vehicle.mode = VehicleMode(new_mode)
+                loop += 1
+                if loop > timeout_count:
+                    break
+                time.sleep(1)
+        return self.vehicle.mode.name == new_mode
+    
+    def pause_mission(self):
+        pause_mode = None
+        if self.is_rotary():
+            paus_mode = "BRAKE"
+        elif self.is_fixed_wing():
+            paus_mode = "LOITER"
+        elif self.is_subsurface():
+            paus_mode = "POSHOLD"
+        elif self.is_ground():
+            paus_mode = "LOITER"
+        else:
+            paus_mode = "LOITER"
+        if self.set_mode(paus_mode):
+            return True
+        return False
         
+    def continue_mission(self):
+        res = False
+        if self.is_subsurface():
+            res = self.set_mode("GUIDED")
+        else:
+            res = self.set_mode("AUTO")
+        return res
+
+    def stop_mission(self):
+        self.clear_mission()
+        return self.pause_mission()
+        
+    def clear_mission(self):
+        self.vehicle.commands.download()
+        self.vehicle.commands.wait_ready()
+        self.vehicle.commands.clear()
+        res = self.set_mode("LOITER")        
+        self.vehicle.commands.upload()
+        self.vehicle.commands.wait_ready()
+        print("MISSION CLEARED")
+    
     def fly_path_execute_callback(self, goal_handle):
         self.get_logger().info('Executing fly path...')
         print("SPEED", goal_handle.request.speed)
@@ -251,18 +314,50 @@ class ArduManager(Node):
         else:
             self.vehicle.airspeed = speed
 
-        gp = gps[0]
-
-        lat = gp.latitude
-        lon = gp.longitude
-        alt = gp.altitude - self.coordtrans.get_sea_level_height()
-        (x0, y0, z0) = self.coordtrans.wgs84_to_world(lon, lat, gp.altitude, angle_deg=-90.0)
-        z0 = alt
-
-        print("FLYTO:", alt)        
-        point = LocationGlobal(lat, lon, alt)
-        self.vehicle.simple_goto(point)
+        #gp = gps[0]
+        #print("FLYTO:", alt)        
+        #point = LocationGlobal(lat, lon, alt)
+        #self.vehicle.simple_goto(point)
+
+        self.clear_mission()
+
+        for gp in gps:
+            lat = gp.latitude
+            lon = gp.longitude
+            alt = gp.altitude - self.coordtrans.get_sea_level_height()
+            (x0, y0, z0) = self.coordtrans.wgs84_to_world(lon, lat, gp.altitude, angle_deg=-90.0)
+            z0 = alt
+
+            wp_cmd: Command = Command(
+                0,  # target system
+                0,  # target component
+                0,  # sequence
+                mavutil.mavlink.MAV_FRAME_GLOBAL,  # frame
+                mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,  # command
+                0,  # current
+                0,  # autucontinue
+                0,  # param1: Speed type (0=Airspeed, 1=Ground Speed)
+                0,  # param2: Speed in m/s
+                0,  # param3: Throttle (ignored)
+                0,  # param4: Absolute or relative (ignored)
+                lat,  # 11: latitude in degrees
+                lon,  # 12: longitude in degrees
+                alt,  # 13: altigutde in meters
+            )
+            self.vehicle.commands.add(wp_cmd)
+        
+        res = self.start_mission()
 
+        if not res:
+            goal_handle.succeed()            
+            result = FlyPath.Result()
+            result.success = False
+            result.message = "Start mission failed"
+            result.abort_flag = False
+            result.enough_flag = False
+            return result
+            
+        
         feedback_msg = FlyPath.Feedback()
 
         d2 = 10.0