use gps antenna position if filter diverges (#75)

* use gps antenna position if filter diverges

* use rtk position for planning if filter diverges

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

@@ -69,9 +69,15 @@ def __init__(self,
         self.lock = Lock()
         self.ticks = 0
         self.self_odom_msg = None
+        self.gps_odom_msg = None
         self.other_odom_msg = None
+        self.use_gps_pos = False
 
         rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_self_odom)
+        rospy.Subscriber(self_name + "/gnss1/odom", Odometry, self.update_self_odom_backup)
+        # only if the filtered position has separated do we use the antenna position
+        rospy.Subscriber(self_name + "/debug/filter_gps_seperation_status", Bool, self.update_use_gps)
+
         if self.has_other_buggy:
             rospy.Subscriber(other_name + "/nav/odom", Odometry, self.update_other_odom)
             self.other_steer_subscriber = rospy.Subscriber(
@@ -104,6 +110,14 @@ def __init__(self,
         self.profile = profile
         self.tick_caller()
 
+    def update_use_gps(self, msg):
+        with self.lock:
+            self.use_gps_pos = msg.data
+
+    def update_self_odom_backup(self, msg):
+        with self.lock:
+            self.gps_odom_msg = msg
+
     def update_self_odom(self, msg):
         with self.lock:
             self.self_odom_msg = msg
@@ -200,8 +214,13 @@ def local_controller_thread(self):
             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)
+        if not self.use_gps_pos:
+            with self.lock:
+                self_pose, self_speed = self.get_world_pose_and_speed(self.self_odom_msg)
+        else:
+            with self.lock:
+                self_pose, self_speed = self.get_world_pose_and_speed(self.gps_odom_msg)
+
 
         self.heading_publisher.publish(Float32(np.rad2deg(self_pose.theta)))
 
@@ -225,10 +244,15 @@ def planner_thread(self):
 
                 self.planner_tick()
 
-
     def planner_tick(self):
+        if not self.use_gps_pos:
+            with self.lock:
+                self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg)
+        else:
+            with self.lock:
+                self_pose, _ = self.get_world_pose_and_speed(self.gps_odom_msg)
+
         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)
 
         # update local trajectory via path planner