Skip to content

Commit

Permalink
Cleaned up unused code, added comments
Browse files Browse the repository at this point in the history
  • Loading branch information
@Jackack
Jackack committed Feb 3, 2024
1 parent 8ce841d commit bf3438f
Show file tree
Hide file tree
Showing 3 changed files with 99 additions and 108 deletions.
125 changes: 73 additions & 52 deletions rb_ws/src/buggy/scripts/auton/autonsystem.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -40,22 +38,28 @@ 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,
# the local trajectory is never updated.

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

Expand Down Expand Up @@ -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):
Expand All @@ -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
Expand All @@ -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()

Expand Down Expand Up @@ -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")
Expand All @@ -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():
Expand Down
74 changes: 18 additions & 56 deletions rb_ws/src/buggy/scripts/auton/path_planner.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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:
Expand All @@ -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)
Expand All @@ -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
Expand All @@ -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()
Expand All @@ -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)
8 changes: 8 additions & 0 deletions rb_ws/src/buggy/scripts/auton/trajectory.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]

Expand Down

0 comments on commit bf3438f

Please sign in to comment.