diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 617fe59d..0338ed09 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -72,7 +72,7 @@ def __init__(self, trajectory, controller, brake_controller) -> None: "auton/debug/distance", Float64, queue_size=1 ) - self.auton_rate = 1000 + self.auton_rate = 100 self.rosrate = rospy.Rate(self.auton_rate) self.tick_caller() @@ -94,7 +94,8 @@ def tick_caller(self): (self.msg.pose.covariance[0] ** 2 + self.msg.pose.covariance[7] ** 2 > 1**2)): # Covariance larger than one meter. We definitely can't trust the pose rospy.sleep(0.001) - + print("Waiting for Covariance to be better: ", rospy.get_rostime()) + print("done checking covariance") while (not rospy.is_shutdown()): # start the actual control loop self.tick() self.ticks += 1 @@ -156,7 +157,7 @@ def tick(self): print("\n\nStarting Controller: " + str(arg_ctrl) + "\n\n") print("\n\nStarting at distance: " + str(arg_start_dist) + "\n\n") - trajectory = Trajectory("/rb_ws/src/buggy/paths/buggycourse_safe_1.json") + trajectory = Trajectory("/rb_ws/src/buggy/paths/frew_parkinglot_1.json") # calculate starting index start_index = trajectory.get_index_from_distance(start_dist)