From df4e0e1eb8090b32f92b01418a1b08a811d837fc Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 21 Feb 2024 17:30:11 -0500 Subject: [PATCH] fixed current heading fr --- rb_ws/src/buggy/scripts/auton/autonsystem.py | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 136cba61..d37a00d4 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -129,8 +129,21 @@ def init_check(self): return False # waits until rtk is fixed and covariance is acceptable to check heading - current_heading = self.cur_traj.get_heading_by_index(self.cur_traj.get_closest_index_on_path) - closest_heading = trajectory.get_heading_by_index(trajectory.get_closest_index_on_path) + with self.lock: + self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg) + current_heading = self_pose.theta + + closest_heading = self.cur_traj.get_heading_by_index(trajectory.get_closest_index_on_path) + + # TENTATIVE: + # headings are originally between -pi and pi + # if they are negative, convert them to be between 0 and pi + if current_heading < 0: + current_heading = 2*np.pi + current_heading + + if closest_heading < 0: + closest_heading = 2*np.pi + closest_heading + if (abs(current_heading - closest_heading) >= np.pi/2): print("WARNING: INCORRECT HEADING! restart stack")