Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

during rolls sanity check #52

Merged
merged 24 commits into from
Mar 19, 2024
Merged
Changes from 1 commit
Commits
Show all changes
24 commits
Select commit Hold shift + click to select a range
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
updated names and heading check
TiaSinghania committed Feb 21, 2024
commit a77c6b870287d059e3ab44e82ce19163408ad5a9
11 changes: 7 additions & 4 deletions rb_ws/src/buggy/scripts/auton/autonsystem.py
Original file line number Diff line number Diff line change
@@ -79,7 +79,7 @@ def __init__(self,
rospy.Subscriber(self_name + "/gnss1/fix_info_republished_int", Int8, self.update_rtk_status)

self.init_check_publisher = rospy.Publisher(
self.name + "/debug/init_check", Bool, queue_size=1
self.name + "/debug/init_safety_check", Bool, queue_size=1
)
self.steer_publisher = rospy.Publisher(
self_name + "/input/steering", Float64, queue_size=1
@@ -125,11 +125,14 @@ def init_check(self):
# (from both buggies if relevant),
# RTK status is fixed
# covariance is less than 1 meter
if (self.self_odom_msg == None) or (self.has_other_buggy and self.other_odom_msg == None) or (self.rtk_status <= 5) or (self.self_odom_msg.pose.covariance[0] ** 2 + self.self_odom_msg.pose.covariance[7] ** 2 > 100):
if (self.self_odom_msg == None) or (self.has_other_buggy and self.other_odom_msg == None) or (self.rtk_status <= 5) or (self.self_odom_msg.pose.covariance[0] ** 2 + self.self_odom_msg.pose.covariance[7] ** 2 > 1**2):
return False

# waits until rtk is fixed and covariance is normal to check heading
if (abs(self.cur_traj.get_heading_by_distance(start_dist)) > np.pi/2):
# 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)

if (abs(current_heading - closest_heading) >= np.pi/2):
print("WARNING: INCORRECT HEADING! restart stack")
return False