removed trailing white spaces

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

@@ -45,13 +45,13 @@ class AutonSystem:
     ticks = 0
 
     def __init__(self,
-            global_trajectory, 
+            global_trajectory,
             local_controller,
-            brake_controller, 
-            self_name, 
+            brake_controller,
+            self_name,
             other_name,
             profile) -> None:
-        
+
 
         self.global_trajectory = global_trajectory
 
@@ -70,7 +70,7 @@ def __init__(self,
         self.ticks = 0
         self.self_odom_msg = None
         self.other_odom_msg = None
-        
+
         rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_self_odom)
         if self.has_other_buggy:
             rospy.Subscriber(other_name + "/nav/odom", Odometry, self.update_other_odom)
@@ -112,17 +112,17 @@ def update_self_odom(self, msg):
     def update_other_odom(self, msg):
         with self.lock:
             self.other_odom_msg = msg
-    
+
     def update_other_steering_angle(self, msg):
         with self.lock:
             self.other_steering = msg.data
 
     def tick_caller(self):
         while ((not rospy.is_shutdown()) and
-            (self.self_odom_msg == None or 
+            (self.self_odom_msg == None or
             (self.has_other_buggy and self.other_odom_msg == None))): # with no message, we wait
             rospy.sleep(0.001)
-        
+
         # wait for covariance matrix to be better
         while ((not rospy.is_shutdown()) and
                (self.self_odom_msg.pose.covariance[0] ** 2 + self.self_odom_msg.pose.covariance[7] ** 2 > 1**2)):
@@ -141,7 +141,7 @@ def tick_caller(self):
             # run the planner every 10 ticks
             # thr main cycle runs at 100hz, the planner runs at 10hz, but looks 1 second ahead
 
-            if not self.other_odom_msg is None and self.ticks % 10 == 0:         
+            if not self.other_odom_msg is None and self.ticks % 10 == 0:
 
                 # for debugging, publish distance to other buggy
                 with self.lock:
@@ -156,12 +156,12 @@ def tick_caller(self):
                     cProfile.runctx('self.planner_tick()', globals(), locals(), sort="cumtime")
                 else:
                     self.planner_tick()
-            
+
             self.local_controller_tick()
             self.ticks += 1
-            self.rosrate.sleep() 
+            self.rosrate.sleep()
+
 
-
     def get_world_pose_and_speed(self, msg):
         current_rospose = msg.pose.pose
         # Check if the pose covariance is a sane value. Publish a warning if insane
@@ -177,7 +177,7 @@ def get_world_pose_and_speed(self, msg):
         # Get data from message
         pose_gps = Pose.rospose_to_pose(current_rospose)
         return World.gps_to_world_pose(pose_gps), current_speed
-    
+
     def local_controller_tick(self):
         with self.lock:
             self_pose, self_speed = self.get_world_pose_and_speed(self.self_odom_msg)
@@ -198,40 +198,40 @@ def planner_tick(self):
 if __name__ == "__main__":
     rospy.init_node("auton_system")
     parser = argparse.ArgumentParser()
-    parser.add_argument("--controller", 
+    parser.add_argument("--controller",
         type=str,
         help="set controller type",
         required=True)
-    
+
     parser.add_argument(
-        "--dist", 
+        "--dist",
         type=float,
         help="start buggy at meters distance along the path",
         required=True)
-    
+
     parser.add_argument(
-        "--traj", 
-        type=str, 
-        help="path to the trajectory file, relative to /rb_ws/src/buggy/paths/", 
+        "--traj",
+        type=str,
+        help="path to the trajectory file, relative to /rb_ws/src/buggy/paths/",
         required=True)
-    
+
     parser.add_argument(
         "--self_name",
         type=str,
-        help="name of ego-buggy", 
+        help="name of ego-buggy",
         required=True)
-    
+
     parser.add_argument(
-        "--other_name", 
+        "--other_name",
         type=str,
-        help="name of other buggy, if left unspecified, the autonsystem assumes it is the only buggy on the course",         
+        help="name of other buggy, if left unspecified, the autonsystem assumes it is the only buggy on the course",
         required=False)
-    
+
     parser.add_argument(
         "--profile",
         action='store_true',
         help="turn on profiling for the path planner")
-    
+
     args, _ = parser.parse_known_args()
     ctrl = args.controller
     start_dist = args.dist
@@ -253,22 +253,22 @@ def planner_tick(self):
     local_ctrller = None
     if (ctrl == "stanley"):
         local_ctrller = StanleyController(
-            self_name, 
+            self_name,
             start_index=start_index)
-        
+
     elif (ctrl == "pure_pursuit"):
         local_ctrller = PurePursuitController(
-            self_name, 
+            self_name,
             start_index=start_index)
-        
+
     elif (ctrl == "mpc"):
         local_ctrller = ModelPredictiveController(
-            self_name, 
+            self_name,
             start_index=start_index)
 
     if (local_ctrller == None):
         raise Exception("Invalid Controller Argument")
-    
+
     auton_system = AutonSystem(
         trajectory,
         local_ctrller,

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

@@ -59,7 +59,7 @@ def compute_control(
             float (desired steering angle)
         """
         raise NotImplementedError
-    
+
     def plot_trajectory(
         self, current_pose: Pose, trajectory: Trajectory, current_speed: float
     ):

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

@@ -437,7 +437,7 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
         #      0  0  0  A2 B2 ... 0    0    0
         #      0  0  0  0  0  ... 0    0    0
         #      0  0  0  0  0  ... AN-1 BN-1 -I]
-        # 
+        #
         # D = [C; X; U]
         # X selects all the states from z
         # U selects all the controls from z
@@ -601,15 +601,15 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
         if self.TIME:
             t = time.time()
         results = self.solver.solve()
-        
+
         steer_angle = results.x[self.N_CONTROLS + self.N_STATES - 1]
         solution_trajectory = np.reshape(results.x, (self.MPC_HORIZON, self.N_STATES + self.N_CONTROLS))
         state_trajectory = solution_trajectory[:, self.N_CONTROLS:(self.N_CONTROLS + self.N_STATES)]
 
         print("status", results.info.status, results.info.status_val)
         if not (results.info.status == "solved" or results.info.status == "solved inaccurate"):
             return reference_trajectory
-        
+
         state_trajectory += reference_trajectory
         steer_rate_trajectory = solution_trajectory[:, :self.N_CONTROLS]
 

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

@@ -19,7 +19,7 @@
 # in meters, the number of meters behind NAND before we start morphing the trajectory
 REAR_MARGIN = 10
 
-# in meters, the number of meters in front of NAND, 
+# in meters, the number of meters in front of NAND,
 # before the morphed trajectory rejoins the nominal trajectory
 # WARNING: set this value to be greater than 10m/s * lookahead time (10 m/s is the upper limit
 # of NAND speed) Failure to do so can result violent u-turns in the new trajectory.
@@ -49,7 +49,7 @@ def __init__(self, nominal_traj) -> None:
         self.nominal_traj = nominal_traj
 
         # TODO: estimate this value based on the curvature of NAND's recent positions
-        self.other_steering_angle = 0 
+        self.other_steering_angle = 0
 
     def compute_traj(
         self,
@@ -60,29 +60,29 @@ def compute_traj(
         # trajectory near NAND is replace by a new segment:
 
         # 1. the global path, at 10m behind NAND
-        # 2. NAND's projected locations, where each location is 
+        # 2. NAND's projected locations, where each location is
         # shifted to the left along the normal of the trajectory vector
 
         other_idx = self.nominal_traj.get_closest_index_on_path(
-            other_pose.x, 
+            other_pose.x,
             other_pose.y)
         #other is just NAND, for general purposes consider it other
 
         new_segment_start_idx = self.nominal_traj.get_index_from_distance(
                 self.nominal_traj.get_distance_from_index(other_idx) - 10
             ) #Where new path index starts, CONSTANT delta from NAND
-        
+
         new_segment_end_idx = self.nominal_traj.get_index_from_distance(
                 self.nominal_traj.get_distance_from_index(other_idx) + 30
             )
-        
+
         # project other buggy path
         # TODO: put other buggy command
         other_future_poses = self.path_projector.project(
             other_pose,
-            self.other_steering_angle, 
-            other_speed, 
-            LOOKAHEAD_TIME, 
+            self.other_steering_angle,
+            other_speed,
+            LOOKAHEAD_TIME,
             RESOLUTION)
 
         other_future_poses_idxs = np.empty((len(other_future_poses), ))
@@ -93,18 +93,18 @@ def compute_traj(
                     other_future_poses[i][0],
                     other_future_poses[i][1],
                     start_index=other_idx)
-            
+
         future_pose_unit_normal = self.nominal_traj.get_unit_normal_by_index(
             other_future_poses_idxs
         )
 
         # the first passing target is 10 meters backward from NAND's position
         passing_targets = np.array(
             [self.nominal_traj.get_position_by_index(new_segment_start_idx)])
-        
-        passing_targets = np.vstack((passing_targets, 
+
+        passing_targets = np.vstack((passing_targets,
             other_future_poses + PASSING_OFFSET * future_pose_unit_normal))
-        
+
         pre_slice = self.nominal_traj.positions[:int(new_segment_start_idx), :]
         post_slice = self.nominal_traj.positions[int(new_segment_end_idx):, :]
         new_path = np.vstack((pre_slice, passing_targets, post_slice))
@@ -118,7 +118,7 @@ def compute_traj(
             self.debug_passing_traj_publisher.publish(reference_navsat)
 
         # for debugging:
-        # publish the first and last point of the part of the original trajectory 
+        # publish the first and last point of the part of the original trajectory
         # that got spliced out
         reference_navsat = NavSatFix()
         ref_gps = World.world_to_gps(*self.nominal_traj.get_position_by_index(int(new_segment_start_idx)))
@@ -130,7 +130,7 @@ def compute_traj(
         reference_navsat.latitude = ref_gps[0]
         reference_navsat.longitude = ref_gps[1]
         self.debug_splice_pt_publisher.publish(reference_navsat)
-        
-        
+
+
         # generate new path
         return Trajectory(json_filepath=None, positions=new_path)

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

@@ -9,10 +9,10 @@ class Projector:
     def __init__(self, wheelbase: float):
         self.wheelbase = wheelbase
 
-    
+
     def project(self, pose: Pose, command: float, v: float, delta_t: float, resolution: int) -> list:
         """
-        Project buggy motion analytically. Assumes constant velocity and turning angle for the duration. 
+        Project buggy motion analytically. Assumes constant velocity and turning angle for the duration.
 
         Args:
             pose (Pose): Pose containing utm_e, utm_n, and heading, in UTM coords and radians
@@ -33,17 +33,17 @@ def project(self, pose: Pose, command: float, v: float, delta_t: float, resoluti
         theta = t * dtheta + theta_0
 
         if dtheta != 0:
-            # for each t, 
+            # for each t,
             # do the integral of cos(theta(t)) over t for x, between t=0 and t=delta_t
-            # do integral of sin(theta(t)) for y. 
+            # do integral of sin(theta(t)) for y.
             # theta(t) = theta_0 + delta_t * dtheta
             v_over_dtheta = v / dtheta
             x = pose.x + v_over_dtheta * (np.sin(theta) - np.sin(theta_0))
             y = pose.y + v_over_dtheta * (-np.cos(theta) + np.cos(theta_0))
         else:
             x = pose.x + t * v * np.cos(theta)
             y = pose.y + t * v * np.sin(theta)
-        
+
         output = np.vstack((x, y)).T
         t_1 = time.perf_counter_ns()
 
@@ -52,7 +52,7 @@ def project(self, pose: Pose, command: float, v: float, delta_t: float, resoluti
 
     def project_discrete(self, pose: Pose, command: float, v: float, time: float, resolution: int, sim_rate: int) -> list:
         """
-        Project buggy motion discretely, performing kinematics at each sim_ts. Assumes constant velocity and turning angle for the duration. 
+        Project buggy motion discretely, performing kinematics at each sim_ts. Assumes constant velocity and turning angle for the duration.
 
         Args:
             pose (Pose): Pose containing utm_e, utm_n, and heading, in UTM coords and degrees
@@ -84,5 +84,5 @@ def project_discrete(self, pose: Pose, command: float, v: float, time: float, re
             if t >= next_out:
                 output.append((x, y, np.rad2deg(theta)))
                 next_out += ts
-        
+
         return output