From dc2eb7129c0e0ecd84c3951422afcbec2c13d08a Mon Sep 17 00:00:00 2001
From: itskalvik <kjakkala@uncc.edu>
Date: Tue, 30 Jul 2024 17:18:31 -0400
Subject: [PATCH] Add trail plan and switch default config to ArduCopter with
 fixed alt

---
 launch/single_robot.launch.py |  13 +----
 launch/trail.plan             | 105 ++++++++++++++++++++++++++++++++++
 scripts/ipp_mission.py        |   2 +-
 3 files changed, 109 insertions(+), 11 deletions(-)
 create mode 100644 launch/trail.plan

diff --git a/launch/single_robot.launch.py b/launch/single_robot.launch.py
index db82364..5a468e6 100644
--- a/launch/single_robot.launch.py
+++ b/launch/single_robot.launch.py
@@ -9,22 +9,15 @@
 
 def generate_launch_description():
     namespace = 'robot_0'
-    data_type = 'SonarData'
+    data_type = 'Altitude'
     num_robots = 1
-    num_waypoints = 30
+    num_waypoints = 10
     sampling_rate = 2
     geofence_plan = PathJoinSubstitution([FindPackageShare('ros_sgp_tools'),
                                           'launch',
-                                          'lake.plan'])
+                                          'trail.plan'])
     
     return LaunchDescription([
-        Node(
-            package='ros_sgp_tools',
-            executable='lake_depth_publisher.py',
-            namespace=namespace,
-            name='lake_depth_publisher'
-        ),
-
         Node(
             package='ros_sgp_tools',
             executable='offline_ipp.py',
diff --git a/launch/trail.plan b/launch/trail.plan
new file mode 100644
index 0000000..1582823
--- /dev/null
+++ b/launch/trail.plan
@@ -0,0 +1,105 @@
+{
+    "fileType": "Plan",
+    "geoFence": {
+        "circles": [
+        ],
+        "polygons": [
+            {
+                "inclusion": true,
+                "polygon": [
+                    [
+                        35.30939176648549,
+                        -80.73818472191542
+                    ],
+                    [
+                        35.3095991283288,
+                        -80.73790612070032
+                    ],
+                    [
+                        35.310125087646526,
+                        -80.73756556377332
+                    ],
+                    [
+                        35.31038492318519,
+                        -80.73741107163788
+                    ],
+                    [
+                        35.310354452730856,
+                        -80.73725312570001
+                    ],
+                    [
+                        35.309791798829394,
+                        -80.73757378437273
+                    ],
+                    [
+                        35.3094913652412,
+                        -80.73768550881958
+                    ],
+                    [
+                        35.30909351039503,
+                        -80.7377566937338
+                    ]
+                ],
+                "version": 1
+            }
+        ],
+        "version": 2
+    },
+    "groundStation": "QGroundControl",
+    "mission": {
+        "cruiseSpeed": 15,
+        "firmwareType": 3,
+        "globalPlanAltitudeMode": 1,
+        "hoverSpeed": 5,
+        "items": [
+            {
+                "AMSLAltAboveTerrain": null,
+                "Altitude": 20,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 22,
+                "doJumpId": 1,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    0,
+                    0,
+                    20
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "autoContinue": true,
+                "command": 20,
+                "doJumpId": 2,
+                "frame": 2,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    0,
+                    0,
+                    0,
+                    0
+                ],
+                "type": "SimpleItem"
+            }
+        ],
+        "plannedHomePosition": [
+            35.3092744698269,
+            -80.7378284186458,
+            183
+        ],
+        "vehicleType": 2,
+        "version": 2
+    },
+    "rallyPoints": {
+        "points": [
+        ],
+        "version": 2
+    },
+    "version": 1
+}
diff --git a/scripts/ipp_mission.py b/scripts/ipp_mission.py
index fc1f47b..f98fcad 100755
--- a/scripts/ipp_mission.py
+++ b/scripts/ipp_mission.py
@@ -90,7 +90,7 @@ def mission(self):
 
 def main(args=None):
     rclpy.init(args=args)
-    mission_planner = IPPMissionPlanner()
+    mission_planner = IPPMissionPlanner(True)
     rclpy.spin_once(mission_planner)
 
 if __name__ == '__main__':