Merge remote-tracking branch 'origin' into MockRoll

rb_ws/src/buggy/scripts/auton/autonsystem.py

@@ -1,13 +1,13 @@
 #!/usr/bin/env python3
 
 import argparse
-import cProfile
 from threading import Lock
 
+import threading
 import rospy
 
 # ROS Message Imports
-from std_msgs.msg import Float32, Float64, Bool
+from std_msgs.msg import Float32, Float64, Bool, Int8
 from nav_msgs.msg import Odometry
 
 import numpy as np
@@ -63,6 +63,7 @@ def __init__(self,
 
         self.path_planner = PathPlanner(global_trajectory)
         self.other_steering = 0
+        self.rtk_status = 0
 
         self.lock = Lock()
         self.ticks = 0
@@ -75,9 +76,10 @@ def __init__(self,
             self.other_steer_subscriber = rospy.Subscriber(
                 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)
 
-        self.covariance_warning_publisher = rospy.Publisher(
-            self_name + "/debug/is_high_covariance", Bool, queue_size=1
+        self.init_check_publisher = rospy.Publisher(
+            self_name + "/debug/init_safety_check", Bool, queue_size=1
         )
         self.steer_publisher = rospy.Publisher(
             self_name + "/buggy/input/steering", Float64, queue_size=1
@@ -96,8 +98,11 @@ def __init__(self,
         )
 
 
-        self.auton_rate = 100
-        self.rosrate = rospy.Rate(self.auton_rate)
+        self.controller_rate = 100
+        self.rosrate_controller = rospy.Rate(self.controller_rate)
+
+        self.planner_rate = 10
+        self.rosrate_planner = rospy.Rate(self.planner_rate)
 
         self.profile = profile
         self.tick_caller()
@@ -114,68 +119,70 @@ def update_other_steering_angle(self, msg):
         with self.lock:
             self.other_steering = msg.data
 
-    def tick_caller(self):
-        while ((not rospy.is_shutdown()) and
-            (self.self_odom_msg == None or
-            (self.has_other_buggy and self.other_odom_msg == None))): # with no message, we wait
-            rospy.sleep(0.001)
-        # wait for covariance matrix to be better
-
+    def update_rtk_status(self, msg):
+        with self.lock:
+            self.rtk_status = msg.data
 
-        print("Waiting for Covariance to be better")
-        while ((not rospy.is_shutdown()) and
-               (self.self_odom_msg.pose.covariance[0] ** 2 + self.self_odom_msg.pose.covariance[7] ** 2 > 1**2)):
-            # Covariance larger than one meter. We definitely can't trust the pose
-            rospy.sleep(0.01)
-        print("done checking covariance")
+    def init_check(self):
+        # checks that messages are being receieved
+        # (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.self_odom_msg.pose.covariance[0] ** 2 + self.self_odom_msg.pose.covariance[7] ** 2 > 1**2):
+            return False
 
-        # initialize global trajectory index
+        # waits until covariance is acceptable to check heading
 
         with self.lock:
-            e, _ = self.get_world_pose_and_speed(self.self_odom_msg)
+            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(self_pose.x, self_pose.y))
 
-        while (not rospy.is_shutdown()):
-            # start the actual control loop
-            # run the planner every 10 ticks
-            # the main cycle runs at 100hz, the planner runs at 10hz.
-            # See LOOKAHEAD_TIME in path_planner.py for the horizon of the
-            # planner. Make sure it is significantly (at least 2x) longer
-            # than 1 period of the planner when you change the planner frequency.
-
-            if not self.other_odom_msg is None and self.ticks == 0:
-                # for debugging, publish distance to other buggy
-                with self.lock:
-                    self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg)
-                    other_pose, _ = self.get_world_pose_and_speed(self.other_odom_msg)
-                    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.heading_publisher.publish(Float32(np.rad2deg(self_pose.theta)))
+        # 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
 
-                # profiling
-                if self.profile:
-                    cProfile.runctx('self.planner_tick()', globals(), locals(), sort="cumtime")
-                else:
-                    self.planner_tick()
+        if closest_heading < 0:
+            closest_heading = 2*np.pi + closest_heading
 
-            self.local_controller_tick()
+        if (abs(current_heading - closest_heading) >= np.pi/2):
+            print("WARNING: INCORRECT HEADING! restart stack")
+            return False
 
-            self.ticks += 1
+        return True
 
-            if self.ticks >= 10:
-                self.ticks = 0
+    def tick_caller(self):
+
+        while ((not rospy.is_shutdown()) and not self.init_check()):
+            self.init_check_publisher.publish(False)
+            rospy.sleep(0.001)
+        print("done checking initialization status")
+        self.init_check_publisher.publish(True)
 
-            self.rosrate.sleep()
 
+        # initialize global trajectory index
+
+        with self.lock:
+            _, _ = self.get_world_pose_and_speed(self.self_odom_msg)
+
+        p2 = threading.Thread(target=self.planner_thread)
+        p1 = threading.Thread(target=self.local_controller_thread)
+
+        # starting processes
+        # See LOOKAHEAD_TIME in path_planner.py for the horizon of the
+        # planner. Make sure it is significantly (at least 2x) longer
+        # than 1 period of the planner when you change the planner frequency.
+        p2.start() #Planner runs every 10 hz
+        p1.start() #Main Cycles runs at 100hz
+
+        p2.join()
+        p1.join()
 
     def get_world_pose_and_speed(self, msg):
         current_rospose = msg.pose.pose
         # Check if the pose covariance is a sane value. Publish a warning if insane
-        if msg.pose.covariance[0] ** 2 + msg.pose.covariance[7] ** 2 > 1**2:
-            # Covariance larger than one meter. We definitely can't trust the pose
-            self.covariance_warning_publisher.publish(Bool(True))
-        else:
-            self.covariance_warning_publisher.publish(Bool(False))
         current_speed = np.sqrt(
             msg.twist.twist.linear.x**2 + msg.twist.twist.linear.y**2
         )
@@ -184,6 +191,11 @@ def get_world_pose_and_speed(self, msg):
         pose_gps = Pose.rospose_to_pose(current_rospose)
         return World.gps_to_world_pose(pose_gps), current_speed
 
+    def local_controller_thread(self):
+        while (not rospy.is_shutdown()):
+            self.local_controller_tick()
+            self.rosrate_controller.sleep()
+
     def local_controller_tick(self):
         with self.lock:
             self_pose, self_speed = self.get_world_pose_and_speed(self.self_odom_msg)
@@ -194,6 +206,21 @@ def local_controller_tick(self):
         steering_angle_deg = np.rad2deg(steering_angle)
         self.steer_publisher.publish(Float64(steering_angle_deg))
 
+
+    def planner_thread(self):
+        while (not rospy.is_shutdown()):
+            if not self.other_odom_msg is None:
+                with self.lock:
+                    self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg)
+                    other_pose, _ = self.get_world_pose_and_speed(self.other_odom_msg)
+                    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()
+                self.rosrate_planner.sleep()
+
+
     def planner_tick(self):
         with self.lock:
             other_pose, other_speed = self.get_world_pose_and_speed(self.other_odom_msg)