reverted all summer work done (now in summer work branch)

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

@@ -0,0 +1,93 @@
+#!/usr/bin/env python3
+import numpy as np
+from controller import Controller
+
+class BrakeController:
+    MAX_LATERAL_ACCEL = 0.9 # in "g" based on regular vehicle
+    BRAKING_GAIN = 2.0
+    def __init__(self, use_binary_braking = True):
+        # NOTE: Add more stuff here to keep track of PID stuff once I and D are implemented.
+        self.binary_braking = use_binary_braking
+
+    @staticmethod
+    def calculate_lateral_accel(linear_speed: float, steering_angle: float) -> float:
+        """Calculate the lateral acceleration given speed and steering angle for the CENTER of the
+        buggy.
+
+        Args:
+            linear_speed (float): m/s
+            steering_angle (float): deg
+
+        Returns:
+            float: lateral accel in "g"
+        """
+        if (steering_angle == 0.0):
+            return 0.0
+        radius_front_wheel = Controller.WHEELBASE / np.sin(np.deg2rad(steering_angle))
+        radius_rear_wheel = Controller.WHEELBASE / np.tan(np.deg2rad(steering_angle))
+
+        lat_accel_front_wheel = np.absolute((linear_speed**2)/radius_front_wheel) / 9.81
+        lat_accel_rear_wheel = np.absolute((linear_speed**2)/radius_rear_wheel) / 9.81
+
+        lat_accel = (lat_accel_front_wheel + lat_accel_rear_wheel) / 2
+
+        return lat_accel
+
+    def compute_braking(self, speed: float, steering_angle: float) -> float:
+        """Wrapper for the type of braking controller we're using
+
+        Args:
+            speed (float): m/s
+            steering_angle (float): degrees
+
+        Returns:
+            float: 0-1 (1 = full brake)
+        """
+        if self.binary_braking:
+            return self._compute_binary_braking(speed, steering_angle)
+        else:
+            return self._compute_modulated_braking(speed, steering_angle)
+
+    def _compute_binary_braking(self, speed: float, steering_angle: float) -> float:
+        """Binary braking - using lateral acceleration
+
+        Args:
+            speed (float): m/s
+            steering_angle (float): degrees
+
+        Returns:
+            float: 1 is brake, 0 is release
+        """
+        lat_accel = BrakeController.calculate_lateral_accel(speed, steering_angle)
+        if (lat_accel > BrakeController.MAX_LATERAL_ACCEL):
+            return 1.0
+        else:
+            return 0.0
+
+    def _compute_modulated_braking(self, speed: float, steering_angle: float) -> float:
+        """Using P controller for braking based on lateral acceleration of buggy. Modulated values
+        from 0-1
+
+        Args:
+            speed (float): _description_
+            steering_angle (float): _description_
+
+        Returns:
+            float: _description_
+        """
+        lat_accel = BrakeController.calculate_lateral_accel(speed, steering_angle)
+        if (lat_accel < BrakeController.MAX_LATERAL_ACCEL):
+            return 0.0
+        else:
+            error = lat_accel - BrakeController.MAX_LATERAL_ACCEL
+            brake_cmd = 1.0 * error * BrakeController.BRAKING_GAIN
+            brake_cmd = np.clip(brake_cmd, 0, 1)
+            return brake_cmd
+
+if __name__ == "__main__":
+    brake_controller = BrakeController()
+    speed = 15
+    steering_angle = 4
+    print("Lateral Accel: ", BrakeController.calculate_lateral_accel(speed, steering_angle))
+    brake_cmd = brake_controller.compute_braking(speed, steering_angle)
+    print("Braking CMD: ", brake_cmd)

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

@@ -0,0 +1,105 @@
+import numpy as np
+
+import rospy
+from sensor_msgs.msg import NavSatFix
+from geometry_msgs.msg import Pose as ROSPose
+
+from pose import Pose
+from trajectory import Trajectory
+from controller import Controller
+from world import World
+
+
+class PurePursuitController(Controller):
+    """
+    Pure Pursuit Controller
+    """
+
+    LOOK_AHEAD_DIST_CONST = 0.5
+    MIN_LOOK_AHEAD_DIST = 0.5
+    MAX_LOOK_AHEAD_DIST = 10
+
+    def __init__(self, buggy_name, start_index=0) -> None:
+        super(PurePursuitController, self).__init__(start_index, buggy_name)
+        self.debug_reference_pos_publisher = rospy.Publisher(
+            buggy_name + "/auton/debug/reference_navsat", NavSatFix, queue_size=1
+        )
+        self.debug_track_pos_publisher = rospy.Publisher(
+            buggy_name + "/auton/debug/track_navsat", NavSatFix, queue_size=1
+        )
+        self.debug_error_publisher = rospy.Publisher(
+            buggy_name + "/auton/debug/error", ROSPose, queue_size=1
+        )
+
+    def compute_control(
+        self, current_pose: Pose, trajectory: Trajectory, current_speed: float
+    ):
+        """
+        Computes the desired control output given the current state and reference trajectory
+
+        Args:
+            current_pose (Pose): current pose (x, y, theta) (UTM coordinates)
+            trajectory (Trajectory): reference trajectory
+            current_speed (float): current speed of the buggy
+
+        Returns:
+            float (desired steering angle)
+        """
+        if self.current_traj_index >= trajectory.get_num_points() - 1:
+            return 0
+
+        # 10 is a good number to search forward along the index
+        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 + 10,
+        )
+        self.current_traj_index = max(traj_index, self.current_traj_index)
+        traj_dist = trajectory.get_distance_from_index(traj_index)
+
+        reference_position = trajectory.get_position_by_index(traj_index)
+        reference_error = current_pose.convert_point_from_global_to_local_frame(
+            reference_position
+        )
+
+        lookahead_dist = np.clip(
+            self.LOOK_AHEAD_DIST_CONST * current_speed,
+            self.MIN_LOOK_AHEAD_DIST,
+            self.MAX_LOOK_AHEAD_DIST,
+        )
+        traj_dist += lookahead_dist
+
+        track_position = trajectory.get_position_by_distance(traj_dist)
+        track_error = current_pose.convert_point_from_global_to_local_frame(
+            track_position
+        )
+
+        bearing = np.arctan2(track_error[1], track_error[0])
+        steering_angle = np.arctan(
+            2.0 * self.WHEELBASE * np.sin(bearing) / lookahead_dist
+        )
+        steering_angle = np.clip(steering_angle, -np.pi / 9, np.pi / 9)
+
+        # Publish track position for debugging
+        track_navsat = NavSatFix()
+        track_gps = World.world_to_gps(*track_position)
+        track_navsat.latitude = track_gps[0]
+        track_navsat.longitude = track_gps[1]
+        self.debug_track_pos_publisher.publish(track_navsat)
+
+        # Publish reference position for debugging
+        reference_navsat = NavSatFix()
+        ref_pos = trajectory.get_position_by_distance(traj_dist - lookahead_dist)
+        ref_gps = World.world_to_gps(*ref_pos)
+        reference_navsat.latitude = ref_gps[0]
+        reference_navsat.longitude = ref_gps[1]
+        self.debug_reference_pos_publisher.publish(reference_navsat)
+
+        # Publish error for debugging
+        error_pose = ROSPose()
+        error_pose.position.x = reference_error[0]
+        error_pose.position.y = reference_error[1]
+        self.debug_error_publisher.publish(error_pose)
+
+        return steering_angle

rb_ws/src/buggy/scripts/util/rosbag_to_pose_csv.py

@@ -1,7 +1,6 @@
 #! /usr/bin/env python3
 import argparse
 import csv
-
 import rosbag
 from tf.transformations import euler_from_quaternion
 
@@ -43,13 +42,9 @@ def main():
         lon = msg.pose.pose.position.y
         orientation_q = msg.pose.pose.orientation
 
-        orientation_list = [
-            orientation_q.x,
-            orientation_q.y,
-            orientation_q.z,
-            orientation_q.w,
-        ]
-        (_, _, yaw) = euler_from_quaternion(orientation_list)
+        orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
+        (_, _, yaw) = euler_from_quaternion (orientation_list)
+
 
         waypoints.append([str(lat), str(lon), str(yaw)])
 

rb_ws/src/buggy/scripts/validation/log_battery.py

@@ -2,9 +2,7 @@
 
 import csv
 import rospy
-from sensor_msgs.msg import (
-    BatteryState,
-)  # Callback function to print the subscribed data on the terminal
+from sensor_msgs.msg import BatteryState # Callback function to print the subscribed data on the terminal
 
 # Should be culled or improved to actually log the battery levels, to a file, when ros bags should contain this if properly published??