From 26935a251d6661eb1dbae6910b507ac1d99737cc Mon Sep 17 00:00:00 2001 From: Jack He <45720415+Jackack@users.noreply.github.com> Date: Wed, 13 Mar 2024 11:33:14 -0400 Subject: [PATCH] Changes made during mock rolls, debugging sessions that need to be preserved (#47) * updated path planner and autonsystem, added option for building trajectory from __init__ parameter of trajectory.py * [BETA] obstacle avoidance * added halfplane constraints to MPC * Switched path planning to use offsets along the path normal * Cleaned up unused code, added comments * CI (#19) * pylint fixes more pylint fixes pylint fixes for 2d sim and other existing modules pylint fixes yet again pylint fixes yet again #2 * removed ROSbags * added changes requested * Updated main.launch Co-authored-by: Mehul Goel Co-authored-by: TiaSinghania Co-authored-by: PatXue * fixed main.launch typo * Fixed topic remapping * fixed topic remp, added dummy subscriber for debug * Fixed heading gain, added ghost NAND feature * removed dummy node (was used for debug) * Commented out MPC halfplane constraints, Updated sim launch file * Added exception for path run out for MPC, stanley Added more mock roll paths and starting poses expanded MPC traj index search window * Added debug steer routine * updated the cut path * changed controller type * Added debug topic for filter <-> gnss distance * Turned on debug logging for INS * set debug to true in INS_params.yml * removed duplicate publish_rtk_err.py fixed typo in publish_rtk_err.py variable naming * Fixed pylint warnings * removed duplicate publish_rtk_err.py * removed unusued vars in MPC --------- Co-authored-by: Christian Luu <35637788+christianvluu@users.noreply.github.com> Co-authored-by: Mehul Goel Co-authored-by: TiaSinghania Co-authored-by: PatXue Co-authored-by: Buggy --- rb_ws/src/buggy/INS_params.yml | 56 +++++----- rb_ws/src/buggy/launch/debug_filter.launch | 33 ++++++ rb_ws/src/buggy/launch/debug_steer.launch | 14 +++ rb_ws/src/buggy/launch/ghost_auton.launch | 37 +++++++ rb_ws/src/buggy/launch/main.launch | 24 +++- rb_ws/src/buggy/launch/sim_2d_2buggies.launch | 11 +- rb_ws/src/buggy/launch/sim_2d_single.launch | 2 +- rb_ws/src/buggy/paths/UC_to_purnell.json | 26 +++++ rb_ws/src/buggy/paths/the_cut.json | 104 ++++++++++++++++++ rb_ws/src/buggy/scripts/2d_sim/engine.py | 8 +- rb_ws/src/buggy/scripts/auton/autonsystem.py | 6 +- .../auton/model_predictive_controller.py | 44 ++++---- .../buggy/scripts/auton/publish_rtk_err.py | 53 +++++++++ .../buggy/scripts/auton/stanley_controller.py | 2 +- rb_ws/src/buggy/scripts/debug/debug_steer.py | 47 ++++++++ 15 files changed, 403 insertions(+), 64 deletions(-) create mode 100644 rb_ws/src/buggy/launch/debug_filter.launch create mode 100644 rb_ws/src/buggy/launch/debug_steer.launch create mode 100644 rb_ws/src/buggy/launch/ghost_auton.launch create mode 100644 rb_ws/src/buggy/paths/UC_to_purnell.json create mode 100644 rb_ws/src/buggy/paths/the_cut.json create mode 100755 rb_ws/src/buggy/scripts/auton/publish_rtk_err.py mode change 100644 => 100755 rb_ws/src/buggy/scripts/auton/stanley_controller.py create mode 100755 rb_ws/src/buggy/scripts/debug/debug_steer.py diff --git a/rb_ws/src/buggy/INS_params.yml b/rb_ws/src/buggy/INS_params.yml index b5b2e0ac..d9d8c1b5 100644 --- a/rb_ws/src/buggy/INS_params.yml +++ b/rb_ws/src/buggy/INS_params.yml @@ -3,22 +3,22 @@ # Note: Feature support is device-dependent and some of the following settings may have no affect on your device. # Please consult your device's documentation for supported features -# ****************************************************************** +# ****************************************************************** # NOTE: This file is formatted to work with ROS and will not work if specified as the params_file argument in ROS2. # If you want to override parameters for ROS2, start with https://github.com/LORD-MicroStrain/microstrain_inertial/blob/ros2/microstrain_inertial_driver/config/empty.yml -# ****************************************************************** +# ****************************************************************** -# ****************************************************************** -# General Settings -# ****************************************************************** +# ****************************************************************** +# General Settings +# ****************************************************************** # port is the main port that the device will communicate over. For all devices except the GQ7, this is the only available port. # aux_port is only available for the GQ7 and is only needed when streaming RTCM corrections to the device from ROS, or if you want to publish NMEA sentences from this node port : "/dev/ttyACM0" aux_port : "/dev/null" baudrate : 115200 -debug : False -diagnostics : False +debug : True +diagnostics : True # If set to true, this will configure the requested baudrate on the device. # Note that this will be set on both USB and serial, but will only actually affect the baudrate of a serial connection. @@ -83,18 +83,18 @@ raw_file_enable : False # true - Request the additional channels (please see notes below!) # # Notes: **We recommend only enabling this feature when specifically requested by Microstrain.** -# +# # Including this feature increases communication bandwidth requirements significantly... for serial data connections -# please ensure the baudrate is sufficient for the added data channels. +# please ensure the baudrate is sufficient for the added data channels. raw_file_include_support_data : False # The directory to store the raw data file (no trailing '/') raw_file_directory : "/home/your_name" -# ****************************************************************** -# IMU Settings -# ****************************************************************** +# ****************************************************************** +# IMU Settings +# ****************************************************************** imu_data_rate : 100 # The speed at which the individual IMU publishers will publish at. @@ -106,15 +106,15 @@ imu_gps_corr_data_rate : -1 # Rate of gps_corr topic # Note that for philo devices (GX5, CV5, CX5), this will be published at the highest IMU data rate if "use_device_timestamp" is set to true imu_overrange_status_data_rate : -1 # Rate of imu/overrange_status topic -# Static IMU message covariance values (the device does not generate these) -# Since internally these are std::vector we need to use the rosparam tags +# Static IMU message covariance values (the device does not generate these) +# Since internally these are std::vector we need to use the rosparam tags imu_orientation_cov : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01] imu_linear_cov : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01] imu_angular_cov : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01] -# ****************************************************************** -# GNSS Settings (only applicable for devices with GNSS) -# ****************************************************************** +# ****************************************************************** +# GNSS Settings (only applicable for devices with GNSS) +# ****************************************************************** gnss1_data_rate : 100 # The speed at which the individual GNSS1 publishers will publish at. @@ -137,7 +137,7 @@ gnss1_rf_error_detection_data_rate : -1 # Rate of gnss1/rf_error_detection topi # Note: Make this as accurate as possible for good performance gnss1_antenna_offset : [-0.6007, 0.0, 0.2225] -# GNSS2 settings are only applicable for multi-GNSS systems (e.g. GQ7) +# GNSS2 settings are only applicable for multi-GNSS systems (e.g. GQ7) gnss2_data_rate : 100 # The speed at which the individual GNSS2 publishers will publish at. @@ -154,7 +154,7 @@ gnss2_sbas_info_data_rate : -1 # Rate of gnss2/sbas_info topic gnss2_rf_error_detection_data_rate : -1 # Rate of gnss1/rf_error_detection topic # Antenna #2 lever arm offset vector (In the vehicle frame wrt IMU origin (meters) ) -# Note: Make this as accurate as possible for good performance +# Note: Make this as accurate as possible for good performance gnss2_antenna_offset : [0.4968, 0.0, 0.3268] # (GQ7 Only) Enable RTK dongle interface @@ -181,9 +181,9 @@ rtcm_topic : "/rtcm" # this will publish NMEA sentences from both the main and aux port on the same topic. publish_nmea : False -# ****************************************************************** -# Kalman Filter Settings (only applicable for devices with a Kalman Filter) -# ****************************************************************** +# ****************************************************************** +# Kalman Filter Settings (only applicable for devices with a Kalman Filter) +# ****************************************************************** filter_data_rate : 50 # The speed at which the individual Filter publishers will publish at. @@ -221,23 +221,23 @@ filter_reset_after_config : True # Controls if the Kalman filter will auto-init or requires manual initialization filter_auto_init : True -# (All, except -10, and -15 products) Declination Source 1 = None, 2 = magnetic model, 3 = manual +# (All, except -10, and -15 products) Declination Source 1 = None, 2 = magnetic model, 3 = manual # Note: When using a CV7, this MUST be changed to either 1, or 3 or the node will not start filter_declination_source : 2 filter_declination : 0.23 -# (All, except GQ7, CV7, -10, and -15 products) Heading Source 0 = None, 1 = magnetic, 2 = GNSS velocity (note: see manual for limitations) +# (All, except GQ7, CV7, -10, and -15 products) Heading Source 0 = None, 1 = magnetic, 2 = GNSS velocity (note: see manual for limitations) # Note: When using a -10/-AR product. This MUST be set to 0 or the node will not start filter_heading_source : 1 -# (GX5 and previous,-45 models only) Dynamics Mode 1 = Portable (default), 2 = Automotive, 3 = Airborne (<2Gs), 4 = Airborne High G (<4Gs) +# (GX5 and previous,-45 models only) Dynamics Mode 1 = Portable (default), 2 = Automotive, 3 = Airborne (<2Gs), 4 = Airborne High G (<4Gs) filter_dynamics_mode : 1 # Controls what kind of linear acceleration data is used in the Filter IMU message. # If this is set to true, the acceleration will not factor out gravity, if set to false gravity will be filtered out of the linear acceleration. filter_use_compensated_accel : True -# ZUPT control +# ZUPT control filter_velocity_zupt_topic : "/moving_vel" filter_angular_zupt_topic : "/moving_ang" filter_velocity_zupt : True @@ -245,11 +245,11 @@ filter_angular_zupt : True # (GQ7/CV7 full support, GX5-45 limited support) Adaptive filter settings # Adaptive level: 0 - off, 1 - Conservative, 2 = Moderate (default), 3 = agressive -# Time limit: Max duration of measurement rejection prior to recovery, in milliseconds - default = 15000 +# Time limit: Max duration of measurement rejection prior to recovery, in milliseconds - default = 15000 filter_adaptive_level : 2 filter_adaptive_time_limit_ms : 15000 -# (GQ7/CV7 only) Aiding measurement control +# (GQ7/CV7 only) Aiding measurement control filter_enable_gnss_pos_vel_aiding : True filter_enable_gnss_heading_aiding : True filter_enable_altimeter_aiding : False diff --git a/rb_ws/src/buggy/launch/debug_filter.launch b/rb_ws/src/buggy/launch/debug_filter.launch new file mode 100644 index 00000000..c0a8c7be --- /dev/null +++ b/rb_ws/src/buggy/launch/debug_filter.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rb_ws/src/buggy/launch/debug_steer.launch b/rb_ws/src/buggy/launch/debug_steer.launch new file mode 100644 index 00000000..f2a23dba --- /dev/null +++ b/rb_ws/src/buggy/launch/debug_steer.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/rb_ws/src/buggy/launch/ghost_auton.launch b/rb_ws/src/buggy/launch/ghost_auton.launch new file mode 100644 index 00000000..5afe7a2e --- /dev/null +++ b/rb_ws/src/buggy/launch/ghost_auton.launch @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rb_ws/src/buggy/launch/main.launch b/rb_ws/src/buggy/launch/main.launch index 14f4cc7f..a51b212d 100755 --- a/rb_ws/src/buggy/launch/main.launch +++ b/rb_ws/src/buggy/launch/main.launch @@ -1,12 +1,17 @@ - + + + + + + @@ -18,7 +23,20 @@ + + + - - \ No newline at end of file + + + + + + + + + + + + diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch index 5e10b549..2768373f 100755 --- a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch +++ b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch @@ -1,5 +1,8 @@ - + + + + @@ -7,7 +10,7 @@ - + @@ -33,14 +36,14 @@ args="$(arg nand_start_pos) $(arg nand_velocity) NAND"/> - + - + diff --git a/rb_ws/src/buggy/launch/sim_2d_single.launch b/rb_ws/src/buggy/launch/sim_2d_single.launch index d8f94a0a..6f7e6079 100755 --- a/rb_ws/src/buggy/launch/sim_2d_single.launch +++ b/rb_ws/src/buggy/launch/sim_2d_single.launch @@ -6,7 +6,7 @@ - + diff --git a/rb_ws/src/buggy/paths/UC_to_purnell.json b/rb_ws/src/buggy/paths/UC_to_purnell.json new file mode 100644 index 00000000..92e38352 --- /dev/null +++ b/rb_ws/src/buggy/paths/UC_to_purnell.json @@ -0,0 +1,26 @@ +[ + { + "key": "df250f90-3cc4-4eb8-991c-cb88fa03f774", + "lat": 40.44295800415301, + "lon": -79.9426950522631, + "active": false + }, + { + "key": "93e8d2e2-43e1-460f-9ad9-1bf272dd6b4d", + "lat": 40.4430150847491, + "lon": -79.94294473319194, + "active": false + }, + { + "key": "7fd4962e-8c0a-4780-bf19-0d3fcc597e1f", + "lat": 40.443081717291996, + "lon": -79.94324300478857, + "active": false + }, + { + "key": "00677a33-8f23-41f5-8287-222d2d1efd40", + "lat": 40.44311914802749, + "lon": -79.9434202506794, + "active": false + } +] \ No newline at end of file diff --git a/rb_ws/src/buggy/paths/the_cut.json b/rb_ws/src/buggy/paths/the_cut.json new file mode 100644 index 00000000..722826a2 --- /dev/null +++ b/rb_ws/src/buggy/paths/the_cut.json @@ -0,0 +1,104 @@ +[ + { + "key": "70c8c82a-7b81-4260-aa31-ae422f462e7f", + "lat": 40.44281703069411, + "lon": -79.94275251870536, + "active": false + }, + { + "key": "0f9b54c6-87b7-49fa-bde1-0bef79eb0a62", + "lat": 40.442655048005605, + "lon": -79.94285556781654, + "active": false + }, + { + "key": "4b6ddc46-7fbb-46fc-981a-22a69a2e40c9", + "lat": 40.44249154261635, + "lon": -79.94291189150535, + "active": false + }, + { + "key": "df44a8bb-5d0a-4364-b0fb-7a91514d2f1b", + "lat": 40.44238461547783, + "lon": -79.94292637091463, + "active": false + }, + { + "key": "bbf1b156-5788-477f-94c3-a652ec78a811", + "lat": 40.4422802938879, + "lon": -79.94293510571092, + "active": false + }, + { + "key": "ff8dfc91-9dde-475c-af0c-b36a40121301", + "lat": 40.44222805507116, + "lon": -79.94294775427467, + "active": false + }, + { + "key": "769c73e5-669c-425a-a4d2-bd04eba0e4c2", + "lat": 40.442174789405506, + "lon": -79.94295972824834, + "active": false + }, + { + "key": "c77a28a1-7db1-416e-af3f-7891d155ff45", + "lat": 40.44215335477644, + "lon": -79.94299042209632, + "active": false + }, + { + "key": "c00ee626-0cdc-4754-b980-895b5a4b6653", + "lat": 40.442142832597945, + "lon": -79.94303566590472, + "active": false + }, + { + "key": "bf6fc8d3-3c88-48a4-8848-442902eccfae", + "lat": 40.44214678611532, + "lon": -79.94308358180389, + "active": false + }, + { + "key": "0fa532cc-35f6-4d4d-9cb3-3aef1479e5e1", + "lat": 40.44215987992077, + "lon": -79.94312456048715, + "active": false + }, + { + "key": "faf24008-f524-46f8-8f2f-fdbcaa333a93", + "lat": 40.44218815995547, + "lon": -79.94315467450606, + "active": false + }, + { + "key": "f8a45569-92bd-443c-bb0f-c4bab411f332", + "lat": 40.44225097844309, + "lon": -79.94319764375527, + "active": false + }, + { + "key": "60fb4366-352c-46a0-a375-3055ebe35a31", + "lat": 40.44231829636168, + "lon": -79.94324094064025, + "active": false + }, + { + "key": "0460d4db-bcbb-49ee-98ac-b6c8b2bb4396", + "lat": 40.442382766019605, + "lon": -79.94327026791606, + "active": false + }, + { + "key": "47932938-f6e4-45eb-a600-35e7873411de", + "lat": 40.442461446376996, + "lon": -79.94329910916957, + "active": false + }, + { + "key": "97d0785b-f233-4a9a-86de-19cd0082a679", + "lat": 40.442528617720356, + "lon": -79.94332497492582, + "active": false + } +] \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py index 87a8ecad..aa88b6b8 100755 --- a/rb_ws/src/buggy/scripts/2d_sim/engine.py +++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py @@ -38,7 +38,7 @@ def __init__(self, start_pos: str, velocity: float, buggy_name: str): self.pose_publisher = rospy.Publisher(buggy_name + "/nav/odom", Odometry, queue_size=1) self.steering_subscriber = rospy.Subscriber( - buggy_name + "/input/steering", Float64, self.update_steering_angle + buggy_name + "/buggy/input/steering", Float64, self.update_steering_angle ) # To read from velocity self.velocity_subscriber = rospy.Subscriber( @@ -59,6 +59,9 @@ def __init__(self, start_pos: str, velocity: float, buggy_name: str): self.starting_poses = { "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 0, Simulator.UTM_NORTH_ZERO + 0, -110), "Hill1_SC": (Simulator.UTM_EAST_ZERO + 20, Simulator.UTM_NORTH_ZERO + 30, -110), + "WESTINGHOUSE": (589647, 4477143, -150), + "UC_TO_PURNELL": (589635, 4477468, 160), + "UC": (589681, 4477457, 160) } # Start position for End of Hill 2 @@ -238,9 +241,7 @@ def publish(self): # Odometry for using with autonomous code odom = Odometry() - odom_noisy = Odometry() odom.header.stamp = time_stamp - odom_noisy.header.stamp = time_stamp odom_pose = Pose() odom_pose.position.x = long_noisy # may not be noisy depending on Simulator.NOISE flag @@ -299,6 +300,7 @@ def publish(self): 0.01, ] + self.pose_publisher.publish(odom) def loop(self): diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index be2ef1f3..eab4158f 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -74,7 +74,7 @@ def __init__(self, if self.has_other_buggy: rospy.Subscriber(other_name + "/nav/odom", Odometry, self.update_other_odom) self.other_steer_subscriber = rospy.Subscriber( - other_name + "/input/steering", Float64, self.update_other_steering_angle + other_name + "/buggy/input/steering", Float64, self.update_other_steering_angle ) rospy.Subscriber(self_name + "/gnss1/fix_info_republished_int", Int8, self.update_rtk_status) @@ -82,10 +82,10 @@ def __init__(self, self_name + "/debug/init_safety_check", Bool, queue_size=1 ) self.steer_publisher = rospy.Publisher( - self_name + "/input/steering", Float64, queue_size=1 + self_name + "/buggy/input/steering", Float64, queue_size=1 ) self.brake_publisher = rospy.Publisher( - self_name + "/input/brake", Float64, queue_size=1 + self_name + "/buggy/input/brake", Float64, queue_size=1 ) self.brake_debug_publisher = rospy.Publisher( self_name + "/auton/debug/brake", Float64, queue_size=1 diff --git a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py index 6949c266..257bcb23 100644 --- a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py +++ b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py @@ -378,8 +378,8 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current 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 + 20, + start_index=self.current_traj_index -20, + end_index=self.current_traj_index + 50, subsample_resolution=1000, ) self.current_traj_index = max(traj_index, self.current_traj_index) @@ -389,6 +389,9 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current if self.DEBUG: print("Current traj index: ", self.current_traj_index) + if self.current_traj_index >= trajectory.get_num_points() - 1: + raise Exception("[MPC]: Ran out of path to follow!") + # Get reference trajectory from the current traj index for the next [MPC_HORIZON] seconds knot_point_distances = np.linspace( traj_distance, @@ -511,23 +514,23 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current # c = [n 0 0], where n is the normal vector of the halfplane in x-y space # p is the position of NAND in x-y space - n = np.array([100, 100]) - p = np.array([0, 1]) - c = np.concatenate((n, np.zeros((2, )))).reshape(1, self.N_STATES) - - C2 = sparse.kron( - np.eye(self.MPC_HORIZON), - np.hstack( - ( - np.zeros((1, self.N_CONTROLS)), - c - ) - ), - format="csc", - ) - + # n = np.array([100, 100]) + # p = np.array([0, 1]) + # c = np.concatenate((n, np.zeros((2, )))).reshape(1, self.N_STATES) + + # C2 = sparse.kron( + # np.eye(self.MPC_HORIZON), + # np.hstack( + # ( + # np.zeros((1, self.N_CONTROLS)), + # c + # ) + # ), + # format="csc", + # ) - D = sparse.vstack([self.C + C1, self.X, self.U, C2]) + D = sparse.vstack([self.C + C1, self.X, self.U]) + # D = sparse.vstack([self.C + C1, self.X, self.U, C2]) if self.TIME: create_mat_time_D = 1000.0 * (time.time() - t) @@ -542,7 +545,7 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current np.zeros(self.N_STATES * (self.MPC_HORIZON - 1)), np.tile(self.state_lb, self.MPC_HORIZON) + reference_trajectory.ravel(), np.tile(self.control_lb, self.MPC_HORIZON) + reference_control.ravel(), - np.tile(n.T @ p, self.MPC_HORIZON), + # np.tile(n.T @ p, self.MPC_HORIZON), ) ) ub = np.hstack( @@ -552,7 +555,7 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current np.zeros(self.N_STATES * (self.MPC_HORIZON - 1)), np.tile(self.state_ub, self.MPC_HORIZON) + reference_trajectory.ravel(), np.tile(self.control_ub, self.MPC_HORIZON) + reference_control.ravel(), - np.tile(np.inf, self.MPC_HORIZON), + # np.tile(np.inf, self.MPC_HORIZON), ) ) @@ -607,7 +610,6 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current return reference_trajectory state_trajectory += reference_trajectory - # steer_rate_trajectory = solution_trajectory[:, :self.N_CONTROLS] if self.TIME: solve_time = 1000 * (time.time() - t) diff --git a/rb_ws/src/buggy/scripts/auton/publish_rtk_err.py b/rb_ws/src/buggy/scripts/auton/publish_rtk_err.py new file mode 100755 index 00000000..7925a637 --- /dev/null +++ b/rb_ws/src/buggy/scripts/auton/publish_rtk_err.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3 + +from threading import Lock +import numpy as np + +import rospy +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped +from std_msgs.msg import Float64 +from pose import Pose +from world import World + +class RTKErrPublisher(): + + """ + Publish distance between front antenna and filtered output + """ + + def __init__(self) -> None: + self.odom_subscriber = rospy.Subscriber("/nav/odom", Odometry, self.update_odom) + self.gnss_subscriber = rospy.Subscriber("/gnss2/fix_Pose", PoseStamped, self.update_gnss2) + + self.distance_publisher = rospy.Publisher( + "/gnss_odom_distance", Float64, queue_size=1 + ) + + self.rate = 100 + self.odom_msg = None + self.gnss2_msg = None + self.lock = Lock() + + + def update_odom(self, msg): + with self.lock: + self.odom_msg = msg + + def update_gnss2(self, msg): + with self.lock: + self.gnss2_msg = msg + + if not (self.odom_msg is None) and not (self.gnss2_msg is None): + odom_pose = World.gps_to_world_pose(Pose.rospose_to_pose(self.odom_msg.pose.pose)) + gnss2_pose = World.gps_to_world_pose(Pose.rospose_to_pose(self.gnss2_msg.pose)) + + distance = (odom_pose.x - gnss2_pose.x) ** 2 + (odom_pose.y - gnss2_pose.y) ** 2 + distance = np.sqrt(distance) + + self.distance_publisher.publish(Float64(distance)) + +if __name__ == "__main__": + rospy.init_node("publish_rtk_err") + d = RTKErrPublisher() + rospy.spin() \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/auton/stanley_controller.py b/rb_ws/src/buggy/scripts/auton/stanley_controller.py old mode 100644 new mode 100755 index 73e9f76e..d39e3cf6 --- a/rb_ws/src/buggy/scripts/auton/stanley_controller.py +++ b/rb_ws/src/buggy/scripts/auton/stanley_controller.py @@ -47,7 +47,7 @@ def compute_control( float (desired steering angle) """ if self.current_traj_index >= trajectory.get_num_points() - 1: - return 0 + raise Exception("[Stanley]: Ran out of path to follow!") heading = current_pose.theta # in radians x = current_pose.x diff --git a/rb_ws/src/buggy/scripts/debug/debug_steer.py b/rb_ws/src/buggy/scripts/debug/debug_steer.py new file mode 100755 index 00000000..a65e3553 --- /dev/null +++ b/rb_ws/src/buggy/scripts/debug/debug_steer.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +import rospy +from std_msgs.msg import Float64 + + +class DebugController(): + + """ + Debug Controller + Sends oscillating steering command for firmware debug + """ + + def __init__(self) -> None: + self.steer_publisher = rospy.Publisher( + "buggy/input/steering", Float64, queue_size=1) + self.rate = 100 + + def loop(self): + rate = rospy.Rate(self.rate) + tick_count = 0 + steer_cmd = 0 + add = True + + while not rospy.is_shutdown(): + if (add): + steer_cmd += 18 / 100 + else: + steer_cmd -= 18 / 100 + + self.steer_publisher.publish(Float64(steer_cmd)) + + tick_count += 1 + if (tick_count == 200): + tick_count = 0 + + if (steer_cmd >= 18): + add = False + + if (steer_cmd <= -18): + add = True + rate.sleep() + +if __name__ == "__main__": + rospy.init_node("debug_steer") + d = DebugController() + d.loop()