From 3cd63fbd395594188362d0b547df166fcc937dac Mon Sep 17 00:00:00 2001
From: Mehul Goel <mehulgoel873@gmail.com>
Date: Thu, 27 Jun 2024 15:38:38 -0700
Subject: [PATCH] Added cur_idx to the trajectory message

---
 rb_ws/src/buggy/msg/TrajectoryMsg.msg         |  1 +
 rb_ws/src/buggy/scripts/auton/autonsystem.py  | 11 +++--------
 rb_ws/src/buggy/scripts/auton/path_planner.py |  2 +-
 rb_ws/src/buggy/scripts/auton/trajectory.py   |  6 ++++--
 4 files changed, 9 insertions(+), 11 deletions(-)

diff --git a/rb_ws/src/buggy/msg/TrajectoryMsg.msg b/rb_ws/src/buggy/msg/TrajectoryMsg.msg
index 6aa4aa9..9c952cc 100644
--- a/rb_ws/src/buggy/msg/TrajectoryMsg.msg
+++ b/rb_ws/src/buggy/msg/TrajectoryMsg.msg
@@ -1,3 +1,4 @@
 float64 time
+float64 cur_idx
 float64[] easting
 float64[] northing
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py
index 57b8fbc..b9ee69d 100755
--- a/rb_ws/src/buggy/scripts/auton/autonsystem.py
+++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py
@@ -127,10 +127,8 @@ def update_other_steering_angle(self, msg):
 
     def update_traj(self, msg):
         with self.lock:
-            # print("New Trajectory:")
-            # print(msg.easting)
-            # print(msg.northing)
-            self.cur_traj = Trajectory.unpack(msg)
+            self.cur_traj, self.local_controller.current_traj_index = Trajectory.unpack(msg)
+            
 
 
     def init_check(self):
@@ -261,10 +259,7 @@ def planner_tick(self):
             other_pose = self.get_world_pose(self.other_odom_msg)
 
         # update local trajectory via path planner
-        _, cur_idx = self.path_planner.compute_traj(
-                                            self_pose,
-                                            other_pose)
-        self.local_controller.current_traj_index = cur_idx
+        self.path_planner.compute_traj(self_pose, other_pose)
 
 def init_parser ():
     """
diff --git a/rb_ws/src/buggy/scripts/auton/path_planner.py b/rb_ws/src/buggy/scripts/auton/path_planner.py
index 36ec83e..4b3a200 100755
--- a/rb_ws/src/buggy/scripts/auton/path_planner.py
+++ b/rb_ws/src/buggy/scripts/auton/path_planner.py
@@ -205,7 +205,7 @@ def compute_traj(
             self.debug_passing_traj_publisher.publish(reference_navsat)
 
         local_traj = Trajectory(json_filepath=None, positions=positions)
-        self.traj_publisher.publish(local_traj.pack())
+        self.traj_publisher.publish(local_traj.pack(self_pose.x, self_pose.y))
         return local_traj, \
                 local_traj.get_closest_index_on_path(
                 self_pose.x,
diff --git a/rb_ws/src/buggy/scripts/auton/trajectory.py b/rb_ws/src/buggy/scripts/auton/trajectory.py
index bcdcea7..adab5c7 100755
--- a/rb_ws/src/buggy/scripts/auton/trajectory.py
+++ b/rb_ws/src/buggy/scripts/auton/trajectory.py
@@ -350,16 +350,18 @@ def get_closest_index_on_path(
             + start_index
         )
 
-    def pack(self) -> TrajectoryMsg:
+    def pack(self, x, y) -> TrajectoryMsg:
         traj = TrajectoryMsg()
         traj.easting = self.positions[:, 0]
         traj.northing = self.positions[:, 1]
         traj.time = time.time()
+        traj.cur_idx = self.get_closest_index_on_path(x,y)
         return traj
 
     def unpack(trajMsg : TrajectoryMsg):
         pos = np.array([trajMsg.easting, trajMsg.northing]).transpose(1, 0)
-        return Trajectory(positions=pos)
+        cur_idx = trajMsg.cur_idx
+        return Trajectory(positions=pos), cur_idx
 
 
 if __name__ == "__main__":