From ea4ae46054dae3c00ff363aa8ca1f712c0b1e728 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Thu, 22 Feb 2024 19:55:15 -0500 Subject: [PATCH] Calculating new index from BROKEN last path traj in path planner --- rb_ws/src/buggy/scripts/auton/autonsystem.py | 10 ++++----- rb_ws/src/buggy/scripts/auton/path_planner.py | 21 +++++++++++++++++-- 2 files changed, 24 insertions(+), 7 deletions(-) diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index d40d509c..6545779a 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/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() diff --git a/rb_ws/src/buggy/scripts/auton/path_planner.py b/rb_ws/src/buggy/scripts/auton/path_planner.py index 1d42592e..9ae61f7b 100755 --- a/rb_ws/src/buggy/scripts/auton/path_planner.py +++ b/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) \ No newline at end of file + return new_traj, new_index \ No newline at end of file