From 728ed5931772229d97ffe353deeaba3941ac3d12 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Tue, 27 Feb 2024 22:25:31 -0500 Subject: [PATCH] Added super class function to uipdate local and global Trajectories. Did some refactroing on stanlye and MPC controllers to move sub functions into the super class. Started type setting function and adding good documentation. --- rb_ws/src/buggy/scripts/auton/autonsystem.py | 2 +- rb_ws/src/buggy/scripts/auton/controller.py | 24 +++++++++++++++++-- .../auton/model_predictive_controller.py | 20 +++++++--------- .../buggy/scripts/auton/stanley_controller.py | 12 +++------- 4 files changed, 35 insertions(+), 23 deletions(-) diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 6545779a..2b898bad 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -198,7 +198,7 @@ def planner_tick(self, curr_pose): # update local trajectory via path planner self.cur_traj, self.local_controller.current_traj_index = self.path_planner.compute_traj( other_pose, - other_speed, self.local_controller.current_traj_index, curr_pose) + other_speed, self.local_controller.current_glob_index, curr_pose) if __name__ == "__main__": rospy.init_node("auton_system") parser = argparse.ArgumentParser() diff --git a/rb_ws/src/buggy/scripts/auton/controller.py b/rb_ws/src/buggy/scripts/auton/controller.py index cf7120f8..699c6d3c 100755 --- a/rb_ws/src/buggy/scripts/auton/controller.py +++ b/rb_ws/src/buggy/scripts/auton/controller.py @@ -4,6 +4,7 @@ from pose import Pose from sensor_msgs.msg import NavSatFix from world import World +from typing import Tuple class Controller(ABC): @@ -22,7 +23,7 @@ class Controller(ABC): WHEELBASE = SC_WHEELBASE current_traj_index = 0 - def __init__(self, start_index, buggy_name) -> None: + def __init__(self, start_index, buggy_name, global_trajectory : Trajectory) -> None: self.buggy_name = buggy_name # self.trajectory_forward_1 = rospy.Publisher( # buggy_name + "/auton/debug/forward1_navsat", NavSatFix, queue_size=1 @@ -40,6 +41,8 @@ def __init__(self, start_index, buggy_name) -> None: # self.forward_publishers = [self.trajectory_forward_1, self.trajectory_forward_2, self.trajectory_forward_3] # self.backward_publishers = [self.trajectory_backward_1] self.current_traj_index = start_index + self.current_glob_index = 0 + self.global_trajectory = global_trajectory @abstractmethod def compute_control( @@ -91,7 +94,24 @@ def plot_trajectory( navsat.longitude = backward_gps[1] self.backward_publishers[i-1].publish(navsat) - + def updateTrajectoryIndexes(self, current_pose : Pose, local_trajectory : Trajectory): #How do I return void type in type casting + traj_index = local_trajectory.get_closest_index_on_path( + current_pose.x, + current_pose.y, + start_index=self.current_traj_index, + end_index=self.current_traj_index + 20, + subsample_resolution=1000, + ) + self.current_traj_index = max(traj_index, self.current_traj_index) + + glob_index = self.global_trajectory.get_closest_index_on_path( + current_pose.x, + current_pose.y, + start_index=self.current_glob_index, + start_index=self.current_glob_index + 2, + subsample_resolution=1000, + ) + self.current_glob_index = max(glob_index, self.current_glob_index) diff --git a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py index 6949c266..06c2ed80 100644 --- a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py +++ b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py @@ -83,9 +83,9 @@ class ModelPredictiveController(Controller): state_cost_diag = np.diag(state_cost) control_cost_diag = np.diag(control_cost) - def __init__(self, buggy_name, start_index=0, ref_trajectory=None, ROS=False) -> None: + def __init__(self, buggy_name, global_traj : Trajectory, start_index=0, ref_trajectory=None, ROS=False) -> None: # instantiate parent - super(ModelPredictiveController, self).__init__(start_index, buggy_name) + super(ModelPredictiveController, self).__init__(start_index, buggy_name, global_traj) # Internal variables self.current_traj_index = 0 # Where in the trajectory we are currently @@ -348,7 +348,7 @@ def transform_trajectory(trajectory, transform_matrix): first_iteration = True - def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current_speed: float): + def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current_speed: float, ): """ Computes the desired control output given the current state and reference trajectory @@ -375,14 +375,12 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current t = time.time() self.current_speed = current_speed - traj_index = trajectory.get_closest_index_on_path( - current_pose.x, - current_pose.y, - start_index=self.current_traj_index, - end_index=self.current_traj_index + 20, - subsample_resolution=1000, - ) - self.current_traj_index = max(traj_index, self.current_traj_index) + + #Finding index on local path + #Do the same on global trajectory + super.updateTrajectoryIndexes(current_pose, trajectory) + + traj_distance = trajectory.get_distance_from_index(self.current_traj_index) traj_distance += self.LOOKAHEAD_TIME * self.current_speed diff --git a/rb_ws/src/buggy/scripts/auton/stanley_controller.py b/rb_ws/src/buggy/scripts/auton/stanley_controller.py index 73e9f76e..ad3e73a9 100644 --- a/rb_ws/src/buggy/scripts/auton/stanley_controller.py +++ b/rb_ws/src/buggy/scripts/auton/stanley_controller.py @@ -23,8 +23,8 @@ class StanleyController(Controller): CROSS_TRACK_GAIN = 1 HEADING_GAIN = 0.3 - def __init__(self, buggy_name, start_index=0) -> None: - super(StanleyController, self).__init__(start_index, buggy_name) + def __init__(self, buggy_name, global_traj : Trajectory, start_index=0) -> None: + super(StanleyController, self).__init__(start_index, buggy_name, global_traj) self.debug_reference_pos_publisher = rospy.Publisher( buggy_name + "/auton/debug/reference_navsat", NavSatFix, queue_size=1 ) @@ -58,13 +58,7 @@ def compute_control( front_x = x + StanleyController.WHEELBASE * np.cos(heading) front_y = y + StanleyController.WHEELBASE * np.sin(heading) - traj_index = trajectory.get_closest_index_on_path( - front_x, - front_y, - start_index=self.current_traj_index - 20, - end_index=self.current_traj_index + 50, - ) - self.current_traj_index = max(traj_index, self.current_traj_index) + super.updateTrajectoryIndexes(current_pose, trajectory) # Calculate heading error