diff --git a/rb_ws/src/buggy/paths/rosbag_to_pose_csv.py b/rb_ws/src/buggy/paths/rosbag_to_pose_csv.py index f0c9f399..89f3f806 100644 --- a/rb_ws/src/buggy/paths/rosbag_to_pose_csv.py +++ b/rb_ws/src/buggy/paths/rosbag_to_pose_csv.py @@ -1,12 +1,17 @@ #! /usr/bin/env python3 -import rosbag import argparse -import uuid -import json import csv + +import rosbag from tf.transformations import euler_from_quaternion + def main(): + """ + bag_file is an input that reads the path to the bag file + output_file is an argument that reads the path to the output file + subsample is the number of points to be selected to be converted to waypoints + """ # Read in bag path from command line parser = argparse.ArgumentParser() parser.add_argument("bag_file", help="Path to bag file") @@ -26,31 +31,36 @@ def main(): i = 0 # Loop through bag - for topic, msg, t in bag.read_messages(topics="/nav/odom"): + for _, msg, _ in bag.read_messages(topics="/nav/odom"): # Skip waypoints if i % args.subsample != 0: i += 1 continue i += 1 + # TODO: Check Orientation lat = msg.pose.pose.position.x 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] - (roll, pitch, 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)]) # Write to csv file - with open(args.output_file, 'w', newline='') as csvfile: - writer = csv.writer(csvfile, delimiter=',', - quotechar='|', quoting=csv.QUOTE_MINIMAL) + with open(args.output_file, "w", newline="") as csvfile: + writer = csv.writer( + csvfile, delimiter=",", quotechar="|", quoting=csv.QUOTE_MINIMAL + ) for row in waypoints: writer.writerow(row) - if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/auton/brake_controller.py b/rb_ws/src/buggy/scripts/auton/brake_controller.py deleted file mode 100644 index 9fd85d76..00000000 --- a/rb_ws/src/buggy/scripts/auton/brake_controller.py +++ /dev/null @@ -1,95 +0,0 @@ -#!/usr/bin/env python3 -import rospy -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) \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py b/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py deleted file mode 100755 index 81f48a2f..00000000 --- a/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py +++ /dev/null @@ -1,107 +0,0 @@ -from abc import ABC, abstractmethod - -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 diff --git a/rb_ws/src/buggy/scripts/validation/log_battery.py b/rb_ws/src/buggy/scripts/validation/log_battery.py deleted file mode 100755 index 466669ce..00000000 --- a/rb_ws/src/buggy/scripts/validation/log_battery.py +++ /dev/null @@ -1,25 +0,0 @@ -# A script to log battery data to a .txt file - -import rospy -from sensor_msgs.msg import BatteryState # Callback function to print the subscribed data on the terminal -import numpy as np -import csv - -file = open('battery_data.txt', 'w', newline='') -writer = csv.writer(file) -writer.writerow(["Time", "Voltage", "Percentage"]) - -def callback_battery(battery_data): - writer.writerow([battery_data.header.stamp, battery_data.voltage, battery_data.percentage]) - - -def batteryIn_T_subscriber(): - rospy.init_node("batteryLogger", anonymous=False) - rospy.Subscriber("BatteryIn_T", BatteryState, callback_battery) - rospy.spin() - -if __name__ == '__main__': - try: - batteryIn_T_subscriber() - except rospy.ROSInterruptException: - pass \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/validation/log_battery_tester.py b/rb_ws/src/buggy/scripts/validation/log_battery_tester.py deleted file mode 100755 index 5019fe13..00000000 --- a/rb_ws/src/buggy/scripts/validation/log_battery_tester.py +++ /dev/null @@ -1,25 +0,0 @@ -# Script -import rospy -from sensor_msgs.msg import BatteryState # Callback function to print the subscribed data on the terminal -import numpy as np - -def batteryIn_T_publisher(): - message_publisher = rospy.Publisher("BatteryIn_T", BatteryState, queue_size=10) - rospy.init_node("batteryLoggerTester", anonymous=False) - rate = rospy.Rate(2) # publish rate (Hz) - while not rospy.is_shutdown(): - message = BatteryState() - message.voltage = np.random.rand() * 10 - message.percentage = np.random.rand() * 10 - - message.header.stamp = rospy.Time.now() - - message_publisher.publish(message) - rate.sleep() - -if __name__ == "__main__": - try: - batteryIn_T_publisher() - #capture the Interrupt signals - except rospy.ROSInterruptException: - pass \ No newline at end of file