From bf3438f80e7a7fbb340c27c674192204c7f606c4 Mon Sep 17 00:00:00 2001 From: Jackack Date: Sat, 3 Feb 2024 10:33:23 -0500 Subject: [PATCH] Cleaned up unused code, added comments --- rb_ws/src/buggy/scripts/auton/autonsystem.py | 125 ++++++++++-------- rb_ws/src/buggy/scripts/auton/path_planner.py | 74 +++-------- rb_ws/src/buggy/scripts/auton/trajectory.py | 8 ++ 3 files changed, 99 insertions(+), 108 deletions(-) diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 65ec2a80..5ef5969b 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -20,12 +20,10 @@ # from model_predictive_interpolation import ModelPredictiveController from path_planner import PathPlanner from pose import Pose + import argparse -import copy import cProfile -import time - class AutonSystem: """ Top-level class for the RoboBuggy autonomous system @@ -40,14 +38,21 @@ class AutonSystem: global_trajectory: Trajectory = None local_controller: Controller = None - nominal_controller: Controller = None brake_controller: BrakeController = None lock = None steer_publisher = None ticks = 0 - def __init__(self, global_trajectory, local_controller, nominal_controller, brake_controller, self_name, other_name) -> None: + def __init__(self, + global_trajectory, + local_controller, + brake_controller, + self_name, + other_name, + profile) -> None: + + self.global_trajectory = global_trajectory # local trajectory is initialized as global trajectory. If there is no other buggy, @@ -55,7 +60,6 @@ def __init__(self, global_trajectory, local_controller, nominal_controller, brak self.has_other_buggy = not other_name is None self.cur_traj = global_trajectory - self.nominal_controller = nominal_controller self.local_controller = local_controller self.brake_controller = brake_controller @@ -97,6 +101,8 @@ def __init__(self, global_trajectory, local_controller, nominal_controller, brak self.auton_rate = 100 self.rosrate = rospy.Rate(self.auton_rate) + + self.profile = profile self.tick_caller() def update_self_odom(self, msg): @@ -111,9 +117,6 @@ def update_other_steering_angle(self, msg): with self.lock: self.other_steering = msg.data - def x(self): - self.planner_tick() - def tick_caller(self): while ((not rospy.is_shutdown()) and (self.self_odom_msg == None or @@ -133,23 +136,28 @@ def tick_caller(self): with self.lock: e, _ = self.get_world_pose_and_speed(self.self_odom_msg) - while (not rospy.is_shutdown()): # start the actual control loop + while (not rospy.is_shutdown()): + # start the actual control loop # 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 % 5 == 0: - # for debugging obstacle avoidance + + if not self.other_odom_msg is None and self.ticks % 10 == 0: + + # for debugging, publish distance to other buggy with self.lock: - self_pose, self_speed = self.get_world_pose_and_speed(self.self_odom_msg) - other_pose, other_speed = self.get_world_pose_and_speed(self.other_odom_msg) + self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg) + other_pose, _ = self.get_world_pose_and_speed(self.other_odom_msg) distance = (self_pose.x - other_pose.x) ** 2 + (self_pose.y - other_pose.y) ** 2 distance = np.sqrt(distance) self.distance_publisher.publish(Float64(distance)) - cProfile.runctx('self.x()', globals(), locals(), sort="cumtime") - # reset current index of local controller, since local trajectory is updated - else: - self.local_controller_tick() - + # profiling + if self.profile: + cProfile.runctx('self.planner_tick()', globals(), locals(), sort="cumtime") + else: + self.planner_tick() + + self.local_controller_tick() self.ticks += 1 self.rosrate.sleep() @@ -180,47 +188,57 @@ def local_controller_tick(self): steering_angle_deg = np.rad2deg(steering_angle) self.steer_publisher.publish(Float64(steering_angle_deg)) - def nominal_controller_tick(self): - with self.lock: - self_pose, self_speed = self.get_world_pose_and_speed(self.self_odom_msg) - - # Compute control output from global static trajectory - steering_angle = self.nominal_controller.compute_control( - self_pose, self.global_trajectory, self_speed) - steering_angle_deg = np.rad2deg(steering_angle) - return float(steering_angle_deg) - def planner_tick(self): with self.lock: - self_pose, self_speed = self.get_world_pose_and_speed(self.self_odom_msg) other_pose, other_speed = self.get_world_pose_and_speed(self.other_odom_msg) # update local trajectory via path planner - self.cur_traj = self.path_planner.compute_traj(self_pose, + self.cur_traj = self.path_planner.compute_traj( other_pose, - self_speed, - other_speed, - 0, - self.other_steering) + other_speed) if __name__ == "__main__": rospy.init_node("auton_system") parser = argparse.ArgumentParser() - parser.add_argument("--controller", type=str, help="set controller type", required=True) - parser.add_argument("--dist", type=float, - help="start buggy at meters distance along the path", required=True) - parser.add_argument("--traj", type=str, + parser.add_argument("--controller", + type=str, + help="set controller type", + required=True) + + parser.add_argument( + "--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/", required=True) - parser.add_argument("--self_name", type=str, help="name of ego-buggy", + + parser.add_argument( + "--self_name", + type=str, + help="name of ego-buggy", required=True) - parser.add_argument("--other_name", type=str, help="name of other buggy, if left unspecified, the autonsystem assumes it is the only buggy on the course", + + parser.add_argument( + "--other_name", + type=str, + 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 traj = args.traj self_name = args.self_name other_name = args.other_name + profile = args.profile print("\n\nStarting Controller: " + str(ctrl) + "\n\n") print("\n\nUsing path: /rb_ws/src/buggy/paths/" + str(traj) + "\n\n") @@ -233,28 +251,31 @@ def planner_tick(self): # Add Controllers Here local_ctrller = None - nominal_ctrller = None if (ctrl == "stanley"): - local_ctrller = StanleyController(self_name + "_local", start_index) - nominal_ctrller = StanleyController(self_name + "_nominal", start_index) + local_ctrller = StanleyController( + self_name, + start_index=start_index) + elif (ctrl == "pure_pursuit"): - local_ctrller = PurePursuitController(self_name + "_local", start_index) - nominal_ctrller = PurePursuitController(self_name + "_nominal", start_index) + local_ctrller = PurePursuitController( + self_name, + start_index=start_index) + elif (ctrl == "mpc"): - print(start_index) - print(self_name + "local") - local_ctrller = ModelPredictiveController(self_name + "_local") - nominal_ctrller = ModelPredictiveController(self_name + "_nominal") + local_ctrller = ModelPredictiveController( + self_name, + start_index=start_index) + if (local_ctrller == None): raise Exception("Invalid Controller Argument") auton_system = AutonSystem( trajectory, local_ctrller, - nominal_ctrller, BrakeController(), self_name, - other_name + other_name, + profile ) while not rospy.is_shutdown(): diff --git a/rb_ws/src/buggy/scripts/auton/path_planner.py b/rb_ws/src/buggy/scripts/auton/path_planner.py index 86da27b9..5dabef71 100755 --- a/rb_ws/src/buggy/scripts/auton/path_planner.py +++ b/rb_ws/src/buggy/scripts/auton/path_planner.py @@ -16,21 +16,22 @@ LOOKAHEAD_TIME = 2.0 #s RESOLUTION = 30 #samples/s PASSING_OFFSET = 2 #m -NOMINAL_STEERING_ERR_GAIN = 0.001 -MAX_STEERING_ANGLE = 20 -OTHER_BUGGY_COST = 1 -other_steering_angle = 0 # TODO: update this variable with NAND steering angle topic +# 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, +# 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. + +FRONT_MARGIN = 30 class PathPlanner(): def __init__(self, nominal_traj) -> None: self.occupancy_grid = OccupancyGrid() - self.path_projector = Projector(1.3) - - # in degrees - self.candidate_steering_angles = np.linspace(-5, 5, num=20) - # range of possible change in steering angle of other buggy - self.other_steering_angles = np.linspace(-1, 1, num=2) + # 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 @@ -47,16 +48,13 @@ def __init__(self, nominal_traj) -> None: self.last_cmd = 0 self.nominal_traj = nominal_traj - def compute_steering_angle_max_estimate(self, velocity): - return np.min(MAX_STEERING_ANGLE / velocity) + # TODO: estimate this value based on the curvature of NAND's recent positions + self.other_steering_angle = 0 def compute_traj( - self, current_pose: Pose, + self, other_pose: Pose, #Currently NAND's location -- To be Changed - current_speed: float, - other_speed: float, - nominal_steering_angle: float, - other_steering_angle: float): + other_speed: float): # draw trajectory, such that the section of the # trajectory near NAND is replace by a new segment: @@ -82,7 +80,7 @@ def compute_traj( # TODO: put other buggy command other_future_poses = self.path_projector.project( other_pose, - 0, + self.other_steering_angle, other_speed, LOOKAHEAD_TIME, RESOLUTION) @@ -107,10 +105,8 @@ def compute_traj( 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)) # publish passing targets for debugging @@ -121,6 +117,7 @@ def compute_traj( reference_navsat.longitude = ref_gps[1] self.debug_passing_traj_publisher.publish(reference_navsat) + # for debugging: # publish the first and last point of the part of the original trajectory # that got spliced out reference_navsat = NavSatFix() @@ -136,39 +133,4 @@ def compute_traj( # generate new path - return Trajectory(json_filepath=None, positions=new_path) - - # for i in range(len(best_traj)): - # reference_navsat = NavSatFix() - # ref_gps = World.world_to_gps(*best_traj[i]) - # reference_navsat.latitude = ref_gps[0] - # reference_navsat.longitude = ref_gps[1] - # self.debug_local_traj_publisher.publish(reference_navsat) - - # for i in range(len(side_padding_l)): - # reference_navsat = NavSatFix() - # ref_gps = World.utm_to_gps(*side_padding_l[i]) - # reference_navsat.latitude = ref_gps[0] - # reference_navsat.longitude = ref_gps[1] - # self.debug_local_traj_publisher.publish(reference_navsat) - - # for i in range(len(side_padding_r)): - # reference_navsat = NavSatFix() - # ref_gps = World.utm_to_gps(*side_padding_r[i]) - # reference_navsat.latitude = ref_gps[0] - # reference_navsat.longitude = ref_gps[1] - # self.debug_local_traj_publisher.publish(reference_navsat) - - # for i in range(len(other_future_poses)): - # reference_navsat = NavSatFix() - # ref_gps = World.utm_to_gps(*other_future_poses[i]) - # reference_navsat.latitude = ref_gps[0] - # reference_navsat.longitude = ref_gps[1] - # self.debug_local_traj_publisher.publish(reference_navsat) - - # self.occupancy_grid.reset_grid() - - # self.last_cmd = best_cmd - - # return best_cmd - + return Trajectory(json_filepath=None, positions=new_path) \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/auton/trajectory.py b/rb_ws/src/buggy/scripts/auton/trajectory.py index 21ae113f..b11e27c0 100755 --- a/rb_ws/src/buggy/scripts/auton/trajectory.py +++ b/rb_ws/src/buggy/scripts/auton/trajectory.py @@ -284,6 +284,14 @@ def get_dynamics_by_index(self, index, wheelbase): return np.stack((x, y, theta, np.arctan(wheelbase * curvature)), axis=-1) def get_unit_normal_by_index(self, index): + """Gets the index of the closest point on the trajectory to the given point + + Args: + index: Nx1 numpy array: indexes along trajectory + Returns: + Nx2 numpy array: unit normal of the trajectory at index + """ + derivative = self.interpolation(index, nu=1) unit_derivative = derivative / np.linalg.norm(derivative, axis=1)[:, None]