Calculating new index from BROKEN last path traj in path planner

rb_ws/src/buggy/scripts/auton/autonsystem.py

@@ -142,7 +142,7 @@ def tick_caller(self):
             # than 1 period of the planner when you change the planner frequency.
 
             if not self.other_odom_msg is None and self.ticks == 0:
-                # for debugging, publish distance to other buggy
+                # publish distance to other buggy (not for debugging purposes)
                 with self.lock:
                     self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg)
                     other_pose, _ = self.get_world_pose_and_speed(self.other_odom_msg)
@@ -154,7 +154,7 @@ def tick_caller(self):
                 if self.profile:
                     cProfile.runctx('self.planner_tick()', globals(), locals(), sort="cumtime")
                 else:
-                    self.planner_tick()
+                    self.planner_tick(self_pose)
 
             self.local_controller_tick()
 
@@ -192,13 +192,13 @@ def local_controller_tick(self):
         steering_angle_deg = np.rad2deg(steering_angle)
         self.steer_publisher.publish(Float64(steering_angle_deg))
 
-    def planner_tick(self):
+    def planner_tick(self, curr_pose):
         with self.lock:
             other_pose, other_speed = self.get_world_pose_and_speed(self.other_odom_msg)
         # update local trajectory via path planner
-        self.cur_traj = self.path_planner.compute_traj(
+        self.cur_traj, self.local_controller.current_traj_index = self.path_planner.compute_traj(
                                             other_pose,
-                                            other_speed)
+                                            other_speed, self.local_controller.current_traj_index, curr_pose)
 if __name__ == "__main__":
     rospy.init_node("auton_system")
     parser = argparse.ArgumentParser()

rb_ws/src/buggy/scripts/auton/path_planner.py

@@ -9,6 +9,7 @@
 from path_projection import Projector
 from trajectory import Trajectory
 from world import World
+from typing import Tuple
 class PathPlanner():
     LOOKAHEAD_TIME = 2.0 #s
     RESOLUTION = 30 #samples/s
@@ -48,7 +49,9 @@ def __init__(self, nominal_traj:Trajectory) -> None:
     def compute_traj(
         self,
         other_pose: Pose, #Currently NAND's location -- To be Changed
-        other_speed: float) -> Trajectory:
+        other_speed: float, 
+        curr_index: float,
+        curr_pose: Pose) -> Tuple[Trajectory, float]:
         """
         draw trajectory, such that the section of the
         trajectory near NAND is replaced by a new segment:
@@ -141,6 +144,20 @@ def compute_traj(
         reference_navsat.longitude = ref_gps[1]
         self.debug_splice_pt_publisher.publish(reference_navsat)
 
+        new_traj = Trajectory(json_filepath=None, positions=new_path)
+        #Modifying index
+        new_index = curr_index
+        new_traj_end_index = len(passing_targets) + new_segment_start_idx
+        if curr_index > new_segment_end_idx:
+            new_index = curr_index + (len(passing_targets)) - (new_segment_end_idx - new_segment_start_idx) #Is this right way to calculate length
+        elif curr_index > new_segment_start_idx:
+            new_index = new_traj.get_closest_index_on_path(
+                curr_pose.x, #X Position
+                curr_pose.y, #Y Position
+                start_index=new_segment_start_idx,
+                end_index=new_traj_end_index
+            )
+        else: new_index = curr_index
 
         # generate new path
-        return Trajectory(json_filepath=None, positions=new_path)
+        return new_traj, new_index