diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch index 389a693..6848016 100755 --- a/rb_ws/src/buggy/launch/sim_2d_single.launch +++ b/rb_ws/src/buggy/launch/sim_2d_single.launch @@ -1,6 +1,6 @@ <launch> <arg name="start_pos" default="Hill1_SC" /> - <arg name="autonsystem_args" default="--controller mpc --dist 0.0 --traj frew_test.json --self_name SC" /> + <arg name="autonsystem_args" default="--controller mpc --dist 0.0 --traj buggycourse_safe_1.json --self_name SC" /> <arg name="velocity" default="15.0" /> <arg name="buggy_name" default="SC" /> diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index d2160a7..8ae9fe8 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -4,7 +4,6 @@ from threading import Lock import threading -from rb_ws.src.buggy.scripts.auton.buggystate import BuggyState import rospy # ROS Message Imports @@ -16,11 +15,8 @@ from trajectory import Trajectory from world import World from controller import Controller -from pure_pursuit_controller import PurePursuitController from stanley_controller import StanleyController -from brake_controller import BrakeController from model_predictive_controller import ModelPredictiveController -# from model_predictive_interpolation import ModelPredictiveController from path_planner import PathPlanner from pose import Pose @@ -38,7 +34,6 @@ class AutonSystem: global_trajectory: Trajectory = None local_controller: Controller = None - brake_controller: BrakeController = None lock = None steer_publisher = None ticks = 0 @@ -46,7 +41,6 @@ class AutonSystem: def __init__(self, global_trajectory, local_controller, - brake_controller, self_name, other_name, curb_traj, @@ -58,10 +52,9 @@ def __init__(self, # 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.has_other_buggy = not (other_name is None) self.cur_traj = global_trajectory self.local_controller = local_controller - self.brake_controller = brake_controller left_curb = curb_traj self.path_planner = PathPlanner(global_trajectory, left_curb) @@ -70,13 +63,14 @@ def __init__(self, self.lock = Lock() self.ticks = 0 self.self_odom_msg = None - # self.gps_odom_msg = None + self.gps_odom_msg = None self.other_odom_msg = None - # self.use_gps_pos = False + self.use_gps_pos = False rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_self_odom) rospy.Subscriber(self_name + "/gnss1/odom", Odometry, self.update_self_odom_backup) - # only if the filtered position has separated do we use the antenna position + + # to report if the filter position has separated (so we need to use the antenna position) rospy.Subscriber(self_name + "/debug/filter_gps_seperation_status", Bool, self.update_use_gps) if self.has_other_buggy: @@ -91,9 +85,6 @@ def __init__(self, self.steer_publisher = rospy.Publisher( self_name + "/buggy/input/steering", Float64, queue_size=1 ) - self.brake_debug_publisher = rospy.Publisher( - self_name + "/auton/debug/brake", Float64, queue_size=1 - ) self.heading_publisher = rospy.Publisher( self_name + "/auton/debug/heading", Float32, queue_size=1 ) @@ -101,7 +92,6 @@ def __init__(self, self_name + "/auton/debug/distance", Float64, queue_size=1 ) - self.controller_rate = 100 self.rosrate_controller = rospy.Rate(self.controller_rate) @@ -111,13 +101,15 @@ def __init__(self, self.profile = profile self.tick_caller() + + # functions to read data from ROS nodes def update_use_gps(self, msg): with self.lock: self.use_gps_pos = msg.data - # def update_self_odom_backup(self, msg): - # with self.lock: - # self.gps_odom_msg = msg + def update_self_odom_backup(self, msg): + with self.lock: + self.gps_odom_msg = msg def update_self_odom(self, msg): with self.lock: @@ -131,24 +123,32 @@ def update_other_steering_angle(self, msg): with self.lock: self.other_steering = msg.data + def init_check(self): - # checks that messages are being receieved - # (from both buggies if relevant) - # covariance is less than 1 meter + """ + Checks if it's safe to switch the buggy into autonomous driving mode. + Specifically, it checks: + if we can recieve odometry messages from the buggy + if the covariance is acceptable (less than 1 meter) + if the buggy thinks it is facing in the correct direction wrt the local trajectory (not 180 degrees flipped) + + Returns: + A boolean describing the status of the buggy (safe for auton or unsafe for auton) + """ + # TODO: should we check if we're recieving messages from NAND? if (self.self_odom_msg == None): rospy.logwarn("WARNING: no available position estimate") return False - if (self.sc_state.get_pos_covariance[0] ** 2 + self.sc_state.get_pos_covariance[7] ** 2 > 1**2): + if (self.self_odom_msg.pose.covariance[0] ** 2 + self.self_odom_msg.pose.covariance[7] ** 2 > 1**2): rospy.logwarn("checking position estimate certainty") return False # waits until covariance is acceptable to check heading with self.lock: - self_easting, self_northing, self_yaw = self.sc_state.get_pose() - current_heading = self_yaw - # TODO: make get_closest_index_on_path work with UTM, not world! - closest_heading = self.cur_traj.get_heading_by_index(trajectory.get_closest_index_on_path(self_easting, self_northing)) + self_pose = self.get_world_pose(self.self_odom_msg) + current_heading = self_pose.theta + closest_heading = self.cur_traj.get_heading_by_index(trajectory.get_closest_index_on_path(self_pose.x, self_pose.y)) rospy.loginfo("current heading: " + str(np.rad2deg(current_heading))) self.heading_publisher.publish(Float32(np.rad2deg(current_heading))) @@ -161,14 +161,15 @@ def init_check(self): closest_heading = 2*np.pi + closest_heading if (abs(current_heading - closest_heading) >= np.pi/2): - rospy.logwarn("WARNING: INCORRECT HEADING! restart stack. Current heading [-180, 180]: " + str(np.rad2deg(self_yaw))) + rospy.logwarn("WARNING: INCORRECT HEADING! restart stack. Current heading [-180, 180]: " + str(np.rad2deg(self_pose.theta))) return False return True def tick_caller(self): - - + """ + The main scheduler - starts threads for the pathplanner and the controller. + """ rospy.loginfo("start checking initialization status") while ((not rospy.is_shutdown()) and not self.init_check()): self.init_check_publisher.publish(False) @@ -195,8 +196,8 @@ def tick_caller(self): t_planner.join() def get_world_pose(self, msg): + #TODO: this should be redundant - converting rospose to pose should automatically handle world conversions current_rospose = msg.pose.pose - # Get data from message pose_gps = Pose.rospose_to_pose(current_rospose) return World.gps_to_world_pose(pose_gps) @@ -207,15 +208,20 @@ def local_controller_thread(self): self.rosrate_controller.sleep() def local_controller_tick(self): + if not self.use_gps_pos: + with self.lock: + state_msg = self.self_odom_msg + else: + with self.lock: + state_msg = self.gps_odom_msg # For viz and debugging - sc_easting, sc_northing, sc_yaw, = self.sc_state.get_pose() - self.heading_publisher.publish(Float32(np.rad2deg(sc_yaw))) + pose = self.get_world_pose(state_msg) + self.heading_publisher.publish(Float32(np.rad2deg(pose.theta))) # Compute control output - # TODO: rewrite compute_control to use buggy state, NOT the entire odom object. (why were we passing around an odom object anyway? instead of just subscribing to it?) steering_angle = self.local_controller.compute_control( - self.sc_state, self.cur_traj) + state_msg, self.cur_traj) steering_angle_deg = np.rad2deg(steering_angle) self.steer_publisher.publish(Float64(steering_angle_deg)) @@ -223,35 +229,38 @@ def local_controller_tick(self): def planner_thread(self): while (not rospy.is_shutdown()): self.rosrate_planner.sleep() - if not self.nand_state.get_pose is None: + if not self.other_odom_msg is None: with self.lock: - sc_easting, sc_northing, sc_yaw = self.sc_state.get_pose() - nand_easting, nand_northing, nand_yaw = self.nand_state.get_pose() + self_pose = self.get_world_pose(self.self_odom_msg) + other_pose = self.get_world_pose(self.other_odom_msg) - distance = (sc_easting - nand_easting) ** 2 + (sc_northing - nand_northing) ** 2 + 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)) self.planner_tick() def planner_tick(self): - # NOTE: do we want to check for None? - with self.lock: - # NOTE: these are in UTM (used to be in world) - sc_easting, sc_northing, sc_yaw = self.sc_state.get_pose() + if not self.use_gps_pos: + with self.lock: + self_pose = self.get_world_pose(self.self_odom_msg) + else: + with self.lock: + self_pose = self.get_world_pose(self.gps_odom_msg) with self.lock: - nand_easting, nand_northing, nand_yaw = self.nand_state.get_pose() + other_pose = self.get_world_pose(self.other_odom_msg) # update local trajectory via path planner - # TODO: have compute_traj take in a buggy state instead of poses self.cur_traj, cur_idx = self.path_planner.compute_traj( - self.sc_state, - self.nand_state) + self_pose, + other_pose) self.local_controller.current_traj_index = cur_idx -if __name__ == "__main__": - rospy.init_node("auton_system") +def init_parser (): + """ + Returns a parser to read launch file arguments to AutonSystem. + """ parser = argparse.ArgumentParser() parser.add_argument("--controller", type=str, @@ -293,6 +302,11 @@ def planner_tick(self): "--profile", action='store_true', help="turn on profiling for the path planner") + return parser + +if __name__ == "__main__": + rospy.init_node("auton_system") + parser = init_parser() args, _ = parser.parse_known_args() ctrl = args.controller @@ -321,12 +335,6 @@ def planner_tick(self): local_ctrller = StanleyController( self_name, start_index=start_index) - - elif (ctrl == "pure_pursuit"): - local_ctrller = PurePursuitController( - self_name, - start_index=start_index) - elif (ctrl == "mpc"): local_ctrller = ModelPredictiveController( self_name, @@ -334,10 +342,10 @@ def planner_tick(self): if (local_ctrller == None): raise Exception("Invalid Controller Argument") + auton_system = AutonSystem( trajectory, local_ctrller, - BrakeController(), self_name, other_name, left_curb, @@ -345,4 +353,4 @@ def planner_tick(self): ) while not rospy.is_shutdown(): - rospy.spin() + rospy.spin() \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/auton/world.py b/rb_ws/src/buggy/scripts/auton/world.py new file mode 100644 index 0000000..9f26d6c --- /dev/null +++ b/rb_ws/src/buggy/scripts/auton/world.py @@ -0,0 +1,226 @@ +import utm +import numpy as np + +from pose import Pose + + +class World: + """Abstraction for the world coordinate system + + The real world uses GPS coordinates, aka latitude and longitude. However, + using lat/lon is bad for path planning for several reasons. First, the difference + in numbers would be very tiny for small distances, alluding to roundoff errors. + Additionally, lat/lon follows a North-East-Down coordinate system, with headings + in the clockwise direction. We want to use an East-North-Up coordinate system, so + that the heading is in the counter-clockwise direction. + + We do this by converting GPS coordinates to UTM coordinates, which are in meters. + UTM works by dividing the world into a grid, where each grid cell has a unique + identifier. A UTM coordinate consists of the grid cell identifier and the "easting" + and "northing" within that grid cell. The easting (x) and northing (y) are in meters, + and are relative to the southwest corner of the grid cell. + + Last, we offset the UTM coordinates to be relative to some arbitrary zero point. That + way, the final world coordinates are relatively close to zero, which makes debugging + easier. + + This class provides methods to convert between GPS and world coordinates. There is + a version for single coordinates and a version for numpy arrays. + """ + + # Geolocates to around the southwest corner of Phipps + WORLD_EAST_ZERO = 589106 + WORLD_NORTH_ZERO = 4476929 + + @staticmethod + def gps_to_world(lat, lon): + """Converts GPS coordinates to world coordinates + + Args: + lat (float): latitude + lon (float): longitude + + Returns: + tuple: (x, y) in meters from some arbitrary zero point + """ + utm_coords = utm.from_latlon(lat, lon) + x = utm_coords[0] - World.WORLD_EAST_ZERO + y = utm_coords[1] - World.WORLD_NORTH_ZERO + + return x, y + + @staticmethod + def utm_to_world_pose(pose: Pose) -> Pose: + """Converts UTM coordinates to world coordinates + + Args: + pose (Pose): pose with utm coordinates + + Returns: + Pose: pose with world coordinates + """ + + utm_e = pose.x + utm_n = pose.y + x = utm_e - World.WORLD_EAST_ZERO + y = utm_n - World.WORLD_NORTH_ZERO + return Pose(x, y, pose.theta) + + @staticmethod + def world_to_utm_pose(pose: Pose) -> Pose: + """Converts world coordinates to utm coordinates + + Args: + pose (Pose): pose with world coordinates + + Returns: + Pose: pose with world coordinates + """ + + utm_e = pose.x + World.WORLD_EAST_ZERO + utm_n = pose.y + World.WORLD_NORTH_ZERO + return Pose(utm_e, utm_n, pose.theta) + + @staticmethod + def world_to_utm_numpy(coords): + """Converts world coordinates to utm coordinates + + Args: + coords (numpy.ndarray [size: (N,2)]): array of x, y pairs + + Returns: + numpy.ndarray [size: (N,2)]: array of utm_e, utm_n pairs + """ + + N = np.shape(coords)[0] + utm_offset_e = np.ones((N, )) * World.WORLD_EAST_ZERO + utm_offset_n = np.ones((N, )) * World.WORLD_NORTH_ZERO + utm_offset = np.stack((utm_offset_e, utm_offset_n), axis=1) + + return coords + utm_offset + + @staticmethod + def utm_to_world_numpy(coords): + """Converts utm coordinates to world coordinates + + Args: + coords (numpy.ndarray [size: (N,2)]): array of utm_e, utm_n pairs + + Returns: + numpy.ndarray [size: (N,2)]: array of x, y pairs + """ + + N = np.shape(coords)[0] + utm_offset_e = np.ones((N, )) * World.WORLD_EAST_ZERO + utm_offset_n = np.ones((N, )) * World.WORLD_NORTH_ZERO + utm_offset = np.stack((utm_offset_e, utm_offset_n), axis=1) + + return coords - utm_offset + + + @staticmethod + def world_to_gps(x, y): + """Converts world coordinates to GPS coordinates + + Args: + x (float): x in meters from some arbitrary zero point + y (float): y in meters from some arbitrary zero point + + Returns: + tuple: (lat, lon) + """ + utm_coords = utm.to_latlon( + x + World.WORLD_EAST_ZERO, y + World.WORLD_NORTH_ZERO, 17, "T" + ) + lat = utm_coords[0] + lon = utm_coords[1] + + return lat, lon + + @staticmethod + def utm_to_gps(x, y): + """Converts utm coordinates to GPS coordinates + + Args: + x (float): x in meters, in utm frame + y (float): y in meters, in utm frame + + Returns: + tuple: (lat, lon) + """ + utm_coords = utm.to_latlon( + x, y, 17, "T" + ) + lat = utm_coords[0] + lon = utm_coords[1] + + return lat, lon + + @staticmethod + def gps_to_world_numpy(coords): + """Converts GPS coordinates to world coordinates + + Args: + coords (numpy.ndarray [size: (N,2)]): array of lat, lon pairs + + Returns: + numpy.ndarray [size: (N,2)]: array of x, y pairs + """ + utm_coords = utm.from_latlon(coords[:, 0], coords[:, 1]) + x = utm_coords[0] - World.WORLD_EAST_ZERO + y = utm_coords[1] - World.WORLD_NORTH_ZERO + + return np.stack((x, y), axis=1) + + @staticmethod + def world_to_gps_numpy(coords): + """Converts world coordinates to GPS coordinates + + Args: + coords (numpy.ndarray [size: (N,2)]): array of x, y pairs + + Returns: + numpy.ndarray [size: (N,2)]: array of lat, lon pairs + """ + + # Pittsburgh is in UTM zone 17T. + utm_coords = utm.to_latlon( + coords[:, 0] + World.WORLD_EAST_ZERO, + coords[:, 1] + World.WORLD_NORTH_ZERO, + 17, + "T", + ) + lat = utm_coords[0] + lon = utm_coords[1] + + return np.stack((lat, lon), axis=1) + + @staticmethod + def gps_to_world_pose(pose: Pose): + """Converts GPS coordinates to world coordinates + + Args: + pose (Pose): pose with lat, lon coordinates (x=lon, y=lat) + + Returns: + Pose: pose with x, y coordinates + """ + world_coords = World.gps_to_world(pose.y, pose.x) + new_heading = pose.theta # INS uses ENU frame so no heading conversion needed + + return Pose(world_coords[0], world_coords[1], new_heading) + + @staticmethod + def world_to_gps_pose(pose: Pose): + """Converts world coordinates to GPS coordinates + + Args: + pose (Pose): pose with x, y coordinates + + Returns: + Pose: pose with lat, lon coordinates + """ + gps_coords = World.world_to_gps(pose.x, pose.y) + new_heading = pose.theta + + return Pose(gps_coords[1], gps_coords[0], new_heading) \ No newline at end of file