From 680b83851ed860abb026751a454c78d1722a103e Mon Sep 17 00:00:00 2001
From: Max <Hunt>
Date: Tue, 21 Sep 2021 15:26:10 +0100
Subject: [PATCH] Update

---
 Control/Jetson/dronekit_server_client.py |  5 +++++
 Control/Jetson/main.py                   | 11 +++++++++++
 Control/Pi/main_server.py                | 12 ++++++++++++
 3 files changed, 28 insertions(+)

diff --git a/Control/Jetson/dronekit_server_client.py b/Control/Jetson/dronekit_server_client.py
index e61b4a3..74dfd50 100644
--- a/Control/Jetson/dronekit_server_client.py
+++ b/Control/Jetson/dronekit_server_client.py
@@ -33,6 +33,7 @@ class DroneKitComms:
         self.steering_endpoint = '/set_steering'
         self.speed_steering_endpoint = '/set_speed_steering'
         self.gps_navigation_endpoint = '/go_to_gps'
+        self.goto_heading_endpoint = '/go_to_heading'
         self.set_state_endpoint = '/set_state'
         self.get_state_endpoint = '/get_state'
         self.arm_endpoint = '/arm'
@@ -100,6 +101,10 @@ class DroneKitComms:
         data = {'lat': lat, 'lon': lon}
         _ = self.make_request(self.gps_navigation_endpoint, data)
 
+    def go_to_heading(self, heading):
+        data = {'pickup_heading': heading}
+        _ = self.make_request(self.goto_heading_endpoint, data)
+
     def set_arm(self):
         _ = self.make_request(self.arm_endpoint)
         # print(rsp)
diff --git a/Control/Jetson/main.py b/Control/Jetson/main.py
index 32edc6f..117060d 100755
--- a/Control/Jetson/main.py
+++ b/Control/Jetson/main.py
@@ -25,6 +25,7 @@ from pallet_hole_alignment_c import HoleAligner
 
 DO_ZERO = True
 DO_GPS_THERE = False
+DO_GOTO_TARGET_HEADING = False
 DO_VISION_APPROACH = True
 DO_FORK_ALIGN = True
 DO_PICKUP_CARGO = True
@@ -165,6 +166,16 @@ def queue_executor_thread():
             else:
                 executor_logger.warning("DO_GPS_THERE flag is false, skipping gps navigation to cargo")
 
+            if DO_GOTO_TARGET_HEADING:
+                FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS")
+                heading_set_success = FORKLIFT.go_to_heading(current_job['pickup_heading'])
+                if not heading_set_success:
+                    executor_logger.critical("Go to GPS HEADING was not successful, check logs !")
+                    FORKLIFT.set_state("OTHER_ERROR")
+                    return False
+            else:
+                executor_logger.warning("DO_GOTO_TARGET_HEADING flag is false, skipping GOTO target heading")
+
             if DO_VISION_APPROACH:
                 FORKLIFT.set_state("NAVIGATING_TO_POINT_VISION")
                 vision_to_cargo_succcess = __go_to_cargo_vision(VISION_CARGO_APPROACH)
diff --git a/Control/Pi/main_server.py b/Control/Pi/main_server.py
index 1b73198..3defd8b 100755
--- a/Control/Pi/main_server.py
+++ b/Control/Pi/main_server.py
@@ -239,6 +239,18 @@ def go_to_gps():
     else:
         return jsonify({'Error': 'Not JSON'})
 
+@app.route('/go_to_heading', methods=['POST'])
+def go_to_heading():
+    if request.is_json:
+        content = request.get_json()
+        target_heading = content.get('pickup_heading', -1)
+        assert target_heading >= 0, "Heading missing!"
+        logger.info(f"Navigating to heading: {target_heading}")
+        FORKLIFT.go_to_heading(target_heading)
+        return jsonify({'Success': True})
+    else:
+        return jsonify({'Error': 'Not JSON'})
+
 @app.route('/get_state', methods=['POST', 'GET'])
 def get_state():
     _, state = STATE_MACHINE.get_state()