From a8d207f171e938751aa28555452c6ae9383ca024 Mon Sep 17 00:00:00 2001 From: Jackack Date: Mon, 18 Nov 2024 21:51:53 -0500 Subject: [PATCH] path planner refactor --- rb_ws/src/buggy/buggy/auton/path_planner.py | 201 ++++++++++ rb_ws/src/buggy/buggy/auton/trajectory.py | 403 ++++++++++++++++++++ rb_ws/src/buggy/msg/TrajectoryMsg.msg | 3 +- 3 files changed, 605 insertions(+), 2 deletions(-) create mode 100644 rb_ws/src/buggy/buggy/auton/path_planner.py create mode 100644 rb_ws/src/buggy/buggy/auton/trajectory.py diff --git a/rb_ws/src/buggy/buggy/auton/path_planner.py b/rb_ws/src/buggy/buggy/auton/path_planner.py new file mode 100644 index 0000000..2962a7b --- /dev/null +++ b/rb_ws/src/buggy/buggy/auton/path_planner.py @@ -0,0 +1,201 @@ +import numpy as np + +import rospy +from sensor_msgs.msg import NavSatFix +from std_msgs.msg import Float64 +from buggy.msg import TrajectoryMsg + +from pose import Pose + +from trajectory import Trajectory +from world import World + +class PathPlanner(): + """ + Class to generate new trajectory splices for SC autonomous system. + + Takes in a default trajectory and an inner curb trajectory. + + """ + # move the curb towards the center of the course by CURB_MARGIN meters + # for a margin of safety + CURB_MARGIN = 1 #m + + # the offset is calculated as a mirrored sigmoid function of distance + OFFSET_SCALE_CROSS_TRACK = 2 #m + OFFSET_SCALE_ALONG_TRACK = 0.2 + ACTIVATE_OTHER_SCALE_ALONG_TRACK = 0.1 + OFFSET_SHIFT_ALONG_TRACK = 4 #m + + # number of meters ahead of the buggy to generate local trajectory for + LOCAL_TRAJ_LEN = 50#m + + # start generating local trajectory this many meters ahead of current position + LOOKAHEAD = 2#m + + # number of points to sample along the nominal trajectory + RESOLUTION = 150 + + def __init__(self, nominal_traj:Trajectory, left_curb:Trajectory) -> None: + + self.debug_traj_publisher = rospy.Publisher( + "/auton/debug/passing_traj", NavSatFix, queue_size=1000 + ) + + self.other_buggy_xtrack_publisher = rospy.Publisher( + "/auton/debug/other_buggy_xtrack", Float64, queue_size=1 + ) + + self.traj_publisher = rospy.Publisher("/auton/trajectory", TrajectoryMsg, queue_size=1) + + self.nominal_traj = nominal_traj + self.left_curb = left_curb + + def offset_func(self, dist): + """ + Args: + dist: (N, ) numpy array, distances between ego-buggy and obstacle, + along the trajectory + Returns: + (N, ) numpy array, offsets from nominal trajectory required to overtake, + defined by a sigmoid function + """ + + return self.OFFSET_SCALE_CROSS_TRACK / \ + (1 + np.exp(-(-self.OFFSET_SCALE_ALONG_TRACK * dist + + self.OFFSET_SHIFT_ALONG_TRACK))) + + def activate_other_crosstrack_func(self, dist): + """ + Args: + dist: (N, ) numpy array, distances between ego-buggy and obstacle, + along the trajectory + Returns: + (N, ) numpy array, multiplier used to weigh the cross-track distance of + the obstacle into the passing offset calculation. + """ + return 1 / \ + (1 + np.exp(-(-self.ACTIVATE_OTHER_SCALE_ALONG_TRACK * dist + + self.OFFSET_SHIFT_ALONG_TRACK))) + + + def compute_traj( + self, + self_pose: Pose, + other_pose: Pose, #Currently NAND's location -- To be Changed + ) -> None: + """ + draw trajectory starting at the current pose and ending at a fixed distance + ahead. For each trajectory point, calculate the required offset perpendicular to the nominal + trajectory. A sigmoid function of the distance along track to the other buggy is used to + weigh the other buggy's cross-track distance. This calculation produces a line that + allows the ego-buggy's trajectory to go through the other buggy. Since we want to pass + the other buggy at some constant distance to the left, another sigmoid function is multiplied + by that constant distance to produce a smooth trajectory that passes the other buggy. + + Finally, the trajectory is bounded to the left by the left curb (if it exists), and to the right + by the nominal trajectory. (we never pass on the right) + + passing offsets = + activate_other_crosstrack_func(distance to other buggy along track) * + other buggy cross track distance + + offset_func(distance to other buggy along track) + + trajectory = nominal trajectory + + left nominal trajectory unit normal vector * + clamp(passing offsets, 0, distance from nominal trajectory to left curb) + + Args: + other_pose (Pose): Pose containing NAND's easting (x), + northing(y), and heading (theta), in "world" cooridnates, + which is UTM, shifted so that the origin is near the course. + + See world.py + + other_speed (float): speed in m/s of NAND + Publishes: + Trajectory: list of x,y coords for ego-buggy to follow. + """ + # grab slice of nominal trajectory + nominal_idx = self.nominal_traj.get_closest_index_on_path(self_pose.x, self_pose.y) + nominal_dist_along = self.nominal_traj.get_distance_from_index(nominal_idx) + + nominal_slice = np.empty((self.RESOLUTION, 2)) + + # compute the distance along nominal trajectory between samples and the obstacle + nominal_slice_dists = np.linspace( + nominal_dist_along + self.LOOKAHEAD, + nominal_dist_along + self.LOOKAHEAD + self.LOCAL_TRAJ_LEN, + self.RESOLUTION) + + for i in range(self.RESOLUTION): + nominal_slice[i, :] = np.array(self.nominal_traj.get_position_by_distance( + nominal_slice_dists[i] + )) + + # get index of the other buggy along the trajetory and convert to distance + other_idx = self.nominal_traj.get_closest_index_on_path(other_pose.x, other_pose.y) + other_dist = self.nominal_traj.get_distance_from_index(other_idx) + nominal_slice_to_other_dist = np.abs(nominal_slice_dists - other_dist) + + passing_offsets = self.offset_func(nominal_slice_to_other_dist) + + # compute signed cross-track distance between NAND and nominal + nominal_to_other = np.array((other_pose.x, other_pose.y)) - \ + np.array(self.nominal_traj.get_position_by_index(other_idx)) + + # dot product with the unit normal to produce left-positive signed distance + other_normal = self.nominal_traj.get_unit_normal_by_index(np.array(other_idx.ravel())) + other_cross_track_dist = np.sum( + nominal_to_other * other_normal, axis=1) + + self.other_buggy_xtrack_publisher.publish(Float64(other_cross_track_dist)) + + # here, use passing offsets to weight NAND's cross track signed distance: + # if the sample point is far from SC, the cross track distance doesn't matter + # if the sample point is near, we add cross track distance to the offset, + # such that the resulting offset is adjusted by position of NAND + + passing_offsets = passing_offsets + \ + self.activate_other_crosstrack_func(nominal_slice_to_other_dist) * other_cross_track_dist + + # clamp passing offset distances to distance to the curb + if self.left_curb is not None: + # grab slice of curb correponding to slice of nominal trajectory. + curb_idx = self.left_curb.get_closest_index_on_path(self_pose.x, self_pose.y) + curb_dist_along = self.left_curb.get_distance_from_index(curb_idx) + curb_idx_end = self.left_curb.get_closest_index_on_path(nominal_slice[-1, 0], nominal_slice[-1, 1]) + curb_dist_along_end = self.left_curb.get_distance_from_index(curb_idx_end) + curb_dists = np.linspace(curb_dist_along, curb_dist_along_end, self.RESOLUTION) + + curb_slice = np.empty((self.RESOLUTION, 2)) + for i in range(self.RESOLUTION): + curb_slice[i, :] = np.array(self.left_curb.get_position_by_distance( + curb_dists[i] + )) + + # compute distances from the sample points to the curb + nominal_slice_to_curb_dist = np.linalg.norm(curb_slice - nominal_slice, axis=1) + passing_offsets = np.minimum(passing_offsets, nominal_slice_to_curb_dist - self.CURB_MARGIN) + + # clamp negative passing offsets to zero, since we always pass on the left, + # the passing offsets should never pull SC to the right. + passing_offsets = np.maximum(0, passing_offsets) + + # shift the nominal slice by passing offsets + nominal_normals = self.nominal_traj.get_unit_normal_by_index( + self.nominal_traj.get_index_from_distance(nominal_slice_dists) + ) + positions = nominal_slice + (passing_offsets[:, None] * nominal_normals) + + local_traj = Trajectory(json_filepath=None, positions=positions) + self.traj_publisher.publish(local_traj.pack()) + + + # publish passing targets for debugging + for i in range(len(positions)): + reference_navsat = NavSatFix() + ref_gps = World.world_to_gps(positions[i, 0], positions[i, 1]) + reference_navsat.latitude = ref_gps[0] + reference_navsat.longitude = ref_gps[1] + self.debug_traj_publisher.publish(reference_navsat) diff --git a/rb_ws/src/buggy/buggy/auton/trajectory.py b/rb_ws/src/buggy/buggy/auton/trajectory.py new file mode 100644 index 0000000..0aae24c --- /dev/null +++ b/rb_ws/src/buggy/buggy/auton/trajectory.py @@ -0,0 +1,403 @@ +import json +import time +import uuid +import matplotlib.pyplot as plt + +import rospy +from std_msgs.msg import Header +from buggy.msg import TrajectoryMsg + +import numpy as np +from scipy.interpolate import Akima1DInterpolator, CubicSpline + +from world import World + + +class Trajectory: + """A wrapper around a trajectory JSON file that does some under-the-hood math. Will + interpolate the data points and calculate other information such as distance along the trajectory + and instantaneous curvature. + + Currently, the trajectory is assumed to be a straight line between each waypoint. This is + not a good assumption, but it's good enough for now. This means that the curvature is zero + anywhere between waypoints. + + This class also has a method that will find the closest point on the trajectory to a given + point in the world. This is useful for finding where the buggy is on the trajectory. + + Use https://rdudhagra.github.io/eracer-portal/ to make trajectories and save the JSON file + """ + + def __init__(self, json_filepath=None, positions = None, interpolator="CubicSpline") -> None: + """ + Args: + json_filepath (String): file path to the path json file (begins at /rb_ws) + positions [[float, float]]: reference trajectory in world coordinates + current_speed (float): current speed of the buggy + + Returns: + float (desired steering angle) + """ + self.distances = np.zeros((0, 1)) # (N/dt x 1) [d, d, ...] + self.positions = np.zeros((0, 2)) # (N x 2) [(x,y), (x,y), ...] + self.indices = None # (N x 1) [0, 1, 2, ...] + self.interpolation = None # scipy.interpolate.PPoly + + # read positions from file + if positions is None: + positions = [] + # Load the json file + with open(json_filepath, "r") as f: + data = json.load(f) + + # Iterate through the waypoints and extract the positions + num_waypoints = len(data) + for i in range(0, num_waypoints): + + waypoint = data[i] + + lat = waypoint["lat"] + lon = waypoint["lon"] + + # Convert to world coordinates + x, y = World.gps_to_world(lat, lon) + positions.append([x, y]) + + positions = np.array(positions) + + num_indices = positions.shape[0] + + if interpolator == "Akima": + self.positions = positions + self.indices = np.arange(num_indices) + self.interpolation = Akima1DInterpolator(self.indices, self.positions) + self.interpolation.extrapolate = True + elif interpolator == "CubicSpline": + temp_traj = Trajectory(positions=positions, interpolator="Akima") + tot_len = temp_traj.distances[-1] + interp_dists = np.linspace(0, tot_len, num_indices) + + self.indices = np.arange(num_indices) + self.positions = [ + temp_traj.get_position_by_distance(interp_dist) + for interp_dist in interp_dists + ] + self.positions = np.array(self.positions) + + self.interpolation = CubicSpline(self.indices, self.positions) + self.interpolation.extrapolate = True + + # TODO: check units + # Calculate the distances along the trajectory + dt = 0.01 #dt is time step (in seconds (?)) + ts = np.arange(len(self.positions) - 1, step=dt) # times corresponding to each position (?) + drdt = self.interpolation( + ts, nu=1 + ) # Calculate derivatives of interpolated path wrt indices + ds = np.sqrt(drdt[:, 0] ** 2 + drdt[:, 1] ** 2) * dt # distances to each interpolated point + s = np.cumsum(np.hstack([[0], ds[:-1]])) + self.distances = s + self.dt = dt + + def get_num_points(self): + """Gets the number of points along the trajectory + + Returns: + int: number of points + """ + return len(self.positions) + + def get_heading_by_index(self, index): + """Gets the heading at given index along trajectory, + interpolating if necessary + + Args: + index (float): index along the trajectory + + Returns: + float: (theta) in rads + """ + # theta = np.interp(index, self.indices, self.positions[:, 2]) + dxdt, dydt = self.interpolation(index, nu=1).reshape((-1, 2)).T + theta = np.arctan2(dydt, dxdt) + + return theta + + def get_heading_by_distance(self, distance): + """Gets the heading at given distance along trajectory, + interpolating if necessary + + Args: + distance (float): distance along the trajectory in meters + + Returns: + float: (theta) in rads + """ + index = self.get_index_from_distance(distance) + return self.get_heading_by_index(index) + + def get_position_by_index(self, index): + """Gets the position at a given index along the trajectory, + interpolating if necessary + + Args: + index (float): index along the trajectory + + Returns: + tuple: (x, y) + """ + # Interpolate the position + + return self.interpolation(index) + + def get_position_by_distance(self, distance): + """Gets the position at a given distance along the trajectory, + interpolating if necessary + + Args: + distance (float): distance along the trajectory in meters + + Returns: + tuple: (x, y) + """ + # Interpolate the position + index = self.get_index_from_distance(distance) + return self.get_position_by_index(index) + + def get_steering_angle_by_index(self, index, wheelbase): + """Gets the bicycle-model steering angle at a given distance along the trajectory, + interpolating if necessary. Assumes that the origin point of the buggy is at the + center of the rear axle. + + Args: + index (float): index along the trajectory + wheelbase (float): wheelbase of the buggy in meters + + Returns: + float: steering angle in rads + """ + curvature = self.get_curvature_by_index(index) + return np.arctan(wheelbase * curvature) + + def get_steering_angle_by_distance(self, distance, wheelbase): + """Gets the bicycle-model steering angle at a given distance along the trajectory, + interpolating if necessary. Assumes that the origin point of the buggy is at the + center of the rear axle. + + Args: + distance (float): distance along the trajectory in meters + wheelbase (float): wheelbase of the buggy in meters + + Returns: + float: steering angle in rads + """ + index = self.get_index_from_distance(distance) + return self.get_steering_angle_by_index(index, wheelbase) + + def get_index_from_distance(self, distance): + """Gets the index at a given distance along the trajectory, + interpolating if necessary + + Args: + distance (float): distance along the trajectory in meters + + Returns: + int: index along the trajectory + """ + # Interpolate the index + index = np.interp( + distance, + self.distances, + np.linspace(0, len(self.distances), len(self.distances)), + ) + + return index * self.dt + + def get_distance_from_index(self, index): + """Gets the distance at a given index along the trajectory, + interpolating if necessary + + Args: + index (float): index along the trajectory + + Returns: + float: distance along the trajectory in meters + """ + # Interpolate the distance + distance = np.interp( + index / self.dt, np.arange(len(self.distances)), self.distances + ) + + return distance + + def get_curvature_by_index(self, index): + """Gets the curvature at a given index along the trajectory, + interpolating if necessary + + Args: + index (float): index along the trajectory + + Returns: + float: curvature + """ + # Interpolate the curvature + dxdt, dydt = self.interpolation(index, nu=1).reshape((-1, 2)).T + ddxdtt, ddydtt = self.interpolation(index, nu=2).reshape((-1, 2)).T + + curvature = (dxdt * ddydtt - dydt * ddxdtt) / ( + (dxdt**2 + dydt**2) ** (3 / 2) + ) + + return curvature + + def get_curvature_by_distance(self, distance): + """Gets the curvature at a given distance along the trajectory, + interpolating if necessary + + Args: + distance (float): distance along the trajectory in meters + + Returns: + float: curvature + """ + # Interpolate the curvature + index = self.get_index_from_distance(distance) + + return self.get_curvature_by_index(index) + + def get_dynamics_by_index(self, index, wheelbase): + """Gets the dynamics at a given index along the trajectory, + interpolating if necessary. Convenience function that returns + all of the dynamics at once rather than requiring multiple + calls to other helper functions. In this way, we can reuse calls + to the interpolator, improving performance. + + Args: + index (float): index along the trajectory + + Returns: + tuple: (x, y, theta, steering_angle) + """ + # Interpolate the dynamics + x, y = self.interpolation(index).reshape((-1, 2)).T + dxdt, dydt = self.interpolation(index, nu=1).reshape((-1, 2)).T + ddxdtt, ddydtt = self.interpolation(index, nu=2).reshape((-1, 2)).T + + curvature = (dxdt * ddydtt - dydt * ddxdtt) / ( + (dxdt**2 + dydt**2) ** (3 / 2) + ) + theta = np.arctan2(dydt, dxdt) + + 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] + + # (x, y), rotated by 90 deg ccw = (-y, x) + unit_normal = np.vstack((-unit_derivative[:, 1], unit_derivative[:, 0])).T + return unit_normal + + def get_closest_index_on_path( + self, x, y, start_index=0, end_index=None, subsample_resolution=1000 + ): + """Gets the index of the closest point on the trajectory to the given point + + Args: + x (float): x coordinate + y (float): y coordinate + start_index (int, optional): index to start searching from. Defaults to 0. + end_index (int, optional): index to end searching at. Defaults to None (disable). + subsample_resolution: resolution of the resulting interpolation + Returns: + float: index along the trajectory + """ + # If end_index is not specified, use the length of the trajectory + if end_index is None: + end_index = len(self.positions) #sketch, 0-indexing where?? + + # Floor/ceil the start/end indices + start_index = max(0, int(np.floor(start_index))) + end_index = int(np.ceil(end_index)) + + # Calculate the distance from the point to each point on the trajectory + distances = (self.positions[start_index : end_index + 1, 0] - x) ** 2 + ( + self.positions[start_index : end_index + 1, 1] - y + ) ** 2 #Don't need to squareroot as it is a relative distance + + min_ind = np.argmin(distances) + start_index + + start_index = max(0, min_ind - 1) #Protection in case min_ind is too low + end_index = min(len(self.positions), min_ind + 1) #Prtoecting in case min_ind too high + #Theoretically start_index and end_index are just two apart + + # Now interpolate at a higher resolution to get a more accurate result + r_interp = self.interpolation( + np.linspace(start_index, end_index, subsample_resolution + 1)) + x_interp, y_interp = r_interp[:, 0], r_interp[:, 1] #x_interp, y_interp are numpy column vectors + + distances = (x_interp - x) ** 2 + (y_interp - y) ** 2 #Again distances are relative + + # Return the rational index of the closest point + return ( + np.argmin(distances) / subsample_resolution * (end_index - start_index) + + start_index + ) + + def pack(self) -> TrajectoryMsg: + traj = TrajectoryMsg() + traj.easting = self.positions[:, 0] + traj.northing = self.positions[:, 1] + traj.header = Header(stamp=rospy.Time.now()) + return traj + + def unpack(trajMsg : TrajectoryMsg): + pos = np.array([trajMsg.easting, trajMsg.northing]).transpose(1, 0) + cur_idx = trajMsg.cur_idx + return Trajectory(positions=pos), cur_idx + + +if __name__ == "__main__": + # Example usage + # TODO: do we want to keep this example usage? where is it even useful for + trajectory = Trajectory("/rb_ws/src/buggy/paths/quartermiletrack.json") + + + interp_dat = [] + for k in np.linspace(0, trajectory.indices[-1], 500): + x, y = trajectory.get_position_by_index(k) + lat, lon = World.world_to_gps(x, y) + + interp_dat.append( + {"lat": lat, "lon": lon, "key": str(uuid.uuid4()), "active": False} + ) + + + ts = np.linspace(0, trajectory.indices[-1], 500) + kurv = [trajectory.get_curvature_by_index(t) for t in ts] + plt.plot(ts, kurv) + plt.show() + + with open("/rb_ws/src/buggy/paths/traj_spline_interp.json", "w") as f: + json.dump(interp_dat, f, indent=4) + + # knot_point_distances = np.arange(0, 20, 1) + # reference_trajectory = np.hstack( + # [ + # ( + # *trajectory.get_position_by_distance(d), + # trajectory.get_heading_by_distance(d), + # trajectory.get_steering_angle_by_distance(d, 1.3), + # ) + # for d in knot_point_distances + # ] + # ) + # print(reference_trajectory) \ No newline at end of file diff --git a/rb_ws/src/buggy/msg/TrajectoryMsg.msg b/rb_ws/src/buggy/msg/TrajectoryMsg.msg index 9c952cc..c874db4 100644 --- a/rb_ws/src/buggy/msg/TrajectoryMsg.msg +++ b/rb_ws/src/buggy/msg/TrajectoryMsg.msg @@ -1,4 +1,3 @@ -float64 time -float64 cur_idx +std_msgs/Header header float64[] easting float64[] northing \ No newline at end of file