From 36d9e1b563a652ba730ae707c795d717c70d3246 Mon Sep 17 00:00:00 2001 From: Daniel Parra Date: Tue, 24 Oct 2023 14:27:59 -0400 Subject: [PATCH] Reduced error by 50 percent --- .../vrx_missions/vrx_wayfinding_2.py | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) 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..2b0eb0b86 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): @@ -37,11 +38,17 @@ async def run(self, parameters): for j in range(array_size): dist_matrix[i][j] = np.linalg.norm(poses[i][0] - poses[j][0]) + #self.send_feedback(f"Distance Matrix:\n{dist_matrix}") + # solve tsp algorithm (ensure start point is where boat is located) & remove current position from pose list path = solve_tsp(dist_matrix, endpoints=(start_pose_index, None)) poses = poses[:start_pose_index] path = path[1:] + #self.send_feedback(f"Poses:\n{poses}") + + #self.send_feedback(f"Path:\n{path}") + # self.send_feedback('Sorted poses' + str(poses)) await self.wait_for_task_such_that(lambda task: task.state in ["running"]) @@ -49,5 +56,22 @@ async def run(self, parameters): for index in path: self.send_feedback(f"Going to {poses[index]}") + #path_msg = await self.get_latching_msg(self.wayfinding_path_sub) + #self.send_feedback(f"Message:\n{path_msg}") # Go to goal + + # Percent Radius of Adjustment + P = 0.85 + part_way_point = [x * P for x in poses[index][0][:-1]] + # Partial Axis Approach + # part_way_point[1] = poses[index][0][1] + + # Constant Radius of Adjustment + # R = 10 + # part_way_point = [(x - R if x > 0 else (x + R if x < 0 else x)) 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])