auton folder cleanup (documentation + deletion)

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

@@ -15,11 +15,8 @@
 from trajectory import Trajectory
 from world import World
 from controller import Controller
-from pure_pursuit_controller import PurePursuitController
 from stanley_controller import StanleyController
-from brake_controller import BrakeController
 from model_predictive_controller import ModelPredictiveController
-# from model_predictive_interpolation import ModelPredictiveController
 from path_planner import PathPlanner
 from pose import Pose
 
@@ -37,15 +34,13 @@ class AutonSystem:
 
     global_trajectory: Trajectory = None
     local_controller: Controller = None
-    brake_controller: BrakeController = None
     lock = None
     steer_publisher = None
     ticks = 0
 
     def __init__(self,
             global_trajectory,
             local_controller,
-            brake_controller,
             self_name,
             other_name,
             curb_traj,
@@ -57,10 +52,9 @@ def __init__(self,
         # local trajectory is initialized as global trajectory. If there is no other buggy,
         # the local trajectory is never updated.
 
-        self.has_other_buggy = not other_name is None
+        self.has_other_buggy = not (other_name is None)
         self.cur_traj = global_trajectory
         self.local_controller = local_controller
-        self.brake_controller = brake_controller
 
         left_curb = curb_traj
         self.path_planner = PathPlanner(global_trajectory, left_curb)
@@ -75,7 +69,8 @@ def __init__(self,
 
         rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_self_odom)
         rospy.Subscriber(self_name + "/gnss1/odom", Odometry, self.update_self_odom_backup)
-        # only if the filtered position has separated do we use the antenna position
+
+        # to report if the filter position has separated (so we need to use the antenna position)
         rospy.Subscriber(self_name + "/debug/filter_gps_seperation_status", Bool, self.update_use_gps)
 
         if self.has_other_buggy:
@@ -90,17 +85,13 @@ def __init__(self,
         self.steer_publisher = rospy.Publisher(
             self_name + "/buggy/input/steering", Float64, queue_size=1
         )
-        self.brake_debug_publisher = rospy.Publisher(
-            self_name + "/auton/debug/brake", Float64, queue_size=1
-        )
         self.heading_publisher = rospy.Publisher(
             self_name + "/auton/debug/heading", Float32, queue_size=1
         )
         self.distance_publisher = rospy.Publisher(
             self_name + "/auton/debug/distance", Float64, queue_size=1
         )
 
-
         self.controller_rate = 100
         self.rosrate_controller = rospy.Rate(self.controller_rate)
 
@@ -110,6 +101,8 @@ def __init__(self,
         self.profile = profile
         self.tick_caller()
 
+
+    # functions to read data from ROS nodes
     def update_use_gps(self, msg):
         with self.lock:
             self.use_gps_pos = msg.data
@@ -130,10 +123,19 @@ def update_other_steering_angle(self, msg):
         with self.lock:
             self.other_steering = msg.data
 
+
     def init_check(self):
-        # checks that messages are being receieved
-        # (from both buggies if relevant)
-        # covariance is less than 1 meter
+        """
+        Checks if it's safe to switch the buggy into autonomous driving mode.
+        Specifically, it checks:
+            if we can recieve odometry messages from the buggy
+            if the covariance is acceptable (less than 1 meter)
+            if the buggy thinks it is facing in the correct direction wrt the local trajectory (not 180 degrees flipped)
+
+        Returns:
+           A boolean describing the status of the buggy (safe for auton or unsafe for auton)
+        """
+        # TODO: should we check if we're recieving messages from NAND?
         if (self.self_odom_msg == None):
             rospy.logwarn("WARNING: no available position estimate")
             return False
@@ -165,8 +167,9 @@ def init_check(self):
         return True
 
     def tick_caller(self):
-
-
+        """
+        The main scheduler - starts threads for the pathplanner and the controller.
+        """
         rospy.loginfo("start checking initialization status")
         while ((not rospy.is_shutdown()) and not self.init_check()):
             self.init_check_publisher.publish(False)
@@ -193,8 +196,8 @@ def tick_caller(self):
             t_planner.join()
 
     def get_world_pose(self, msg):
+        #TODO: this should be redundant - converting rospose to pose should automatically handle world conversions
         current_rospose = msg.pose.pose
-
         # Get data from message
         pose_gps = Pose.rospose_to_pose(current_rospose)
         return World.gps_to_world_pose(pose_gps)
@@ -254,8 +257,10 @@ def planner_tick(self):
                                             other_pose)
         self.local_controller.current_traj_index = cur_idx
 
-if __name__ == "__main__":
-    rospy.init_node("auton_system")
+def init_parser ():
+    """
+        Returns a parser to read launch file arguments to AutonSystem.
+    """
     parser = argparse.ArgumentParser()
     parser.add_argument("--controller",
         type=str,
@@ -297,6 +302,11 @@ def planner_tick(self):
         "--profile",
         action='store_true',
         help="turn on profiling for the path planner")
+    return parser
+
+if __name__ == "__main__":
+    rospy.init_node("auton_system")
+    parser = init_parser()
 
     args, _ = parser.parse_known_args()
     ctrl = args.controller
@@ -325,23 +335,17 @@ def planner_tick(self):
         local_ctrller = StanleyController(
             self_name,
             start_index=start_index)
-
-    elif (ctrl == "pure_pursuit"):
-        local_ctrller = PurePursuitController(
-            self_name,
-            start_index=start_index)
-
     elif (ctrl == "mpc"):
         local_ctrller = ModelPredictiveController(
             self_name,
             start_index=start_index)
 
     if (local_ctrller == None):
         raise Exception("Invalid Controller Argument")
+
     auton_system = AutonSystem(
         trajectory,
         local_ctrller,
-        BrakeController(),
         self_name,
         other_name,
         left_curb,

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

@@ -18,38 +18,24 @@ class Controller(ABC):
     Example schemes include Pure Pursuit, Stanley, and LQR.
     """
 
+    # TODO: move this to a constants class
     NAND_WHEELBASE = 1.3
     SC_WHEELBASE = 1.104
     current_traj_index = 0
 
-    def __init__(self, start_index, buggy_name) -> None:
+    def __init__(self, start_index: int, buggy_name: str) -> None:
         self.buggy_name = buggy_name
         if buggy_name.upper() == 'NAND':
             Controller.WHEELBASE = self.NAND_WHEELBASE
         else:
             Controller.WHEELBASE = self.SC_WHEELBASE
 
-        # self.trajectory_forward_1 = rospy.Publisher(
-        #     buggy_name + "/auton/debug/forward1_navsat", NavSatFix, queue_size=1
-        # )
-        # self.trajectory_forward_2 = rospy.Publisher(
-        #     buggy_name + "/auton/debug/forward2_navsat", NavSatFix, queue_size=1
-        # )
-        # self.trajectory_forward_3 = rospy.Publisher(
-        #     buggy_name + "/auton/debug/forward3_navsat", NavSatFix, queue_size=1
-        # )
-        # self.trajectory_backward_1 = rospy.Publisher(
-        #     buggy_name + "/auton/debug/backward1_navsat", NavSatFix, queue_size=1
-        # )
-        # # Make lists of publishers for easy iteration
-        # 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
 
     @abstractmethod
     def compute_control(
         self, state_msg: Odometry, trajectory: Trajectory,
-    ):
+    ) -> float:
         """
         Computes the desired control output given the current state and reference trajectory
 
@@ -96,7 +82,6 @@ def plot_trajectory(
             navsat.longitude = backward_gps[1]
             self.backward_publishers[i-1].publish(navsat)
 
-
-
+    # TODO: do we want logging methods to be required (update_speed, update_trajectory)
 
 

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

@@ -36,6 +36,7 @@ class ModelPredictiveController(Controller):
     TIME = False
     ROS = True
 
+    # TODO: constants class
     # MPC Params
     WHEELBASE = 1.17 #m
     MASS = 62 #kg
@@ -85,7 +86,7 @@ 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 : str, start_index=0, ref_trajectory=None, ROS=False) -> None:
         # instantiate parent
         super(ModelPredictiveController, self).__init__(start_index, buggy_name)
 

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

@@ -5,14 +5,19 @@
 from std_msgs.msg import Float64
 from pose import Pose
 
-from occupancy_grid.grid_manager import OccupancyGrid
-from path_projection import Projector
 from trajectory import Trajectory
 from world import World
+
 class PathPlanner():
+    """
+    Class to generate new trajectory splices for SC autonomous system.
+
+    Takes in a default trajectory and an inner curb trajectory.
+
+    """
     # move the curb towards the center of the course by CURB_MARGIN meters
     # for a margin of safety
-    CURB_MARGIN = 1#m
+    CURB_MARGIN = 1 #m
 
     # the offset is calculated as a mirrored sigmoid function of distance
     OFFSET_SCALE_CROSS_TRACK = 2 #m
@@ -30,24 +35,11 @@ class PathPlanner():
     RESOLUTION = 150
 
     def __init__(self, nominal_traj:Trajectory, left_curb:Trajectory) -> None:
-        self.occupancy_grid = OccupancyGrid()
-
-        # TODO: update with NAND wheelbase
-        self.path_projector = Projector(1.3)
 
         self.debug_passing_traj_publisher = rospy.Publisher(
             "/auton/debug/passing_traj", NavSatFix, queue_size=1000
         )
 
-        self.debug_splice_pt_publisher = rospy.Publisher(
-            "/auton/debug/splice_pts", NavSatFix, queue_size=1000
-        )
-
-
-        self.debug_grid_cost_publisher = rospy.Publisher(
-            "/auton/debug/grid_cost", Float64, queue_size=0
-        )
-
         self.other_buggy_xtrack_publisher = rospy.Publisher(
             "/auton/debug/other_buggy_xtrack", Float64, queue_size=1
         )