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

@@ -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()

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)
 
 
 

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
 

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