From b3ca7c49a03fa0b36321e7f69653938a078b7dee Mon Sep 17 00:00:00 2001
From: Daniel Parra <74571960+DaniParr@users.noreply.github.com>
Date: Mon, 6 Nov 2023 14:20:58 -0500
Subject: [PATCH] Wayfinding optimization (#1101)

* Removed IndyAV file

* Reduced error by 50 percent

* reformatted file
---
 .../vrx_missions/vrx_wayfinding_2.py                  | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py
index 60496d0b8..966f38be0 100644
--- a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py
+++ b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py
@@ -5,6 +5,7 @@
 from .vrx import Vrx
 
 ___author___ = "Alex Perez"
+# Optimized by Daniel Parra
 
 
 class VrxWayfinding2(Vrx):
@@ -42,12 +43,18 @@ async def run(self, parameters):
         poses = poses[:start_pose_index]
         path = path[1:]
 
-        # self.send_feedback('Sorted poses' + str(poses))
         await self.wait_for_task_such_that(lambda task: task.state in ["running"])
 
         # do movements
         for index in path:
             self.send_feedback(f"Going to {poses[index]}")
-
             # Go to goal
+            P = 0.85
+            part_way_point = [x * P for x in poses[index][0][:-1]]
+            part_way_point.append(poses[index][0][-1])
+            self.send_feedback(
+                f"\nPartway:\n{part_way_point}\nEndPoint:\n{poses[index][0]}",
+            )
+
+            await self.send_trajectory_without_path([part_way_point, poses[index][1]])
             await self.send_trajectory_without_path(poses[index])