Turned on debug logging for INS

rb_ws/src/buggy/INS_params.yml

@@ -3,14 +3,14 @@
 # Note: Feature support is device-dependent and some of the following settings may have no affect on your device.
 # Please consult your device's documentation for supported features
 
-# ****************************************************************** 
+# ******************************************************************
 # NOTE: This file is formatted to work with ROS and will not work if specified as the params_file argument in ROS2.
 #       If you want to override parameters for ROS2, start with https://github.com/LORD-MicroStrain/microstrain_inertial/blob/ros2/microstrain_inertial_driver/config/empty.yml
-# ****************************************************************** 
+# ******************************************************************
 
-# ****************************************************************** 
-# General Settings 
-# ****************************************************************** 
+# ******************************************************************
+# General Settings
+# ******************************************************************
 
 # port is the main port that the device will communicate over. For all devices except the GQ7, this is the only available port.
 # aux_port is only available for the GQ7 and is only needed when streaming RTCM corrections to the device from ROS, or if you want to publish NMEA sentences from this node
@@ -83,18 +83,18 @@ raw_file_enable : False
 #      true  - Request the additional channels (please see notes below!)
 #
 #      Notes: **We recommend only enabling this feature when specifically requested by Microstrain.**
-#      
+#
 #      Including this feature increases communication bandwidth requirements significantly... for serial data connections
-#      please ensure the baudrate is sufficient for the added data channels.     
+#      please ensure the baudrate is sufficient for the added data channels.
 raw_file_include_support_data : False
 
 # The directory to store the raw data file (no trailing '/')
 raw_file_directory : "/home/your_name"
 
 
-# ****************************************************************** 
-# IMU Settings 
-# ****************************************************************** 
+# ******************************************************************
+# IMU Settings
+# ******************************************************************
 imu_data_rate : 100
 
 # The speed at which the individual IMU publishers will publish at.
@@ -106,15 +106,15 @@ imu_gps_corr_data_rate         : -1  # Rate of gps_corr topic
                                      # Note that for philo devices (GX5, CV5, CX5), this will be published at the highest IMU data rate if "use_device_timestamp" is set to true
 imu_overrange_status_data_rate : -1  # Rate of imu/overrange_status topic
 
-# Static IMU message covariance values (the device does not generate these) 
-# Since internally these are std::vector we need to use the rosparam tags 
+# Static IMU message covariance values (the device does not generate these)
+# Since internally these are std::vector we need to use the rosparam tags
 imu_orientation_cov : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
 imu_linear_cov      : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
 imu_angular_cov     : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
 
-# ****************************************************************** 
-# GNSS Settings (only applicable for devices with GNSS) 
-# ****************************************************************** 
+# ******************************************************************
+# GNSS Settings (only applicable for devices with GNSS)
+# ******************************************************************
 gnss1_data_rate : 100
 
 # The speed at which the individual GNSS1 publishers will publish at.
@@ -137,7 +137,7 @@ gnss1_rf_error_detection_data_rate : -1  # Rate of gnss1/rf_error_detection topi
 #     Note: Make this as accurate as possible for good performance
 gnss1_antenna_offset : [-0.6007, 0.0, 0.2225]
 
-# GNSS2 settings are only applicable for multi-GNSS systems (e.g. GQ7) 
+# GNSS2 settings are only applicable for multi-GNSS systems (e.g. GQ7)
 gnss2_data_rate : 100
 
 # The speed at which the individual GNSS2 publishers will publish at.
@@ -154,7 +154,7 @@ gnss2_sbas_info_data_rate          : -1  # Rate of gnss2/sbas_info topic
 gnss2_rf_error_detection_data_rate : -1  # Rate of gnss1/rf_error_detection topic
 
 # Antenna #2 lever arm offset vector (In the vehicle frame wrt IMU origin (meters) )
-# Note: Make this as accurate as possible for good performance 
+# Note: Make this as accurate as possible for good performance
 gnss2_antenna_offset : [0.4968, 0.0, 0.3268]
 
 # (GQ7 Only) Enable RTK dongle interface
@@ -181,9 +181,9 @@ rtcm_topic     : "/rtcm"
 #       this will publish NMEA sentences from both the main and aux port on the same topic.
 publish_nmea   : False
 
-# ****************************************************************** 
-# Kalman Filter Settings (only applicable for devices with a Kalman Filter) 
-# ****************************************************************** 
+# ******************************************************************
+# Kalman Filter Settings (only applicable for devices with a Kalman Filter)
+# ******************************************************************
 filter_data_rate : 50
 
 # The speed at which the individual Filter publishers will publish at.
@@ -221,35 +221,35 @@ filter_reset_after_config : True
 # Controls if the Kalman filter will auto-init or requires manual initialization
 filter_auto_init : True
 
-# (All, except -10, and -15 products) Declination Source 1 = None, 2 = magnetic model, 3 = manual 
+# (All, except -10, and -15 products) Declination Source 1 = None, 2 = magnetic model, 3 = manual
 # Note: When using a CV7, this MUST be changed to either 1, or 3 or the node will not start
 filter_declination_source : 2
 filter_declination        : 0.23
 
-# (All, except GQ7, CV7, -10, and -15 products) Heading Source 0 = None, 1 = magnetic, 2 = GNSS velocity (note: see manual for limitations)  
+# (All, except GQ7, CV7, -10, and -15 products) Heading Source 0 = None, 1 = magnetic, 2 = GNSS velocity (note: see manual for limitations)
 # Note: When using a -10/-AR product. This MUST be set to 0 or the node will not start
 filter_heading_source : 1
 
-# (GX5 and previous,-45 models only) Dynamics Mode 1 = Portable (default), 2 = Automotive, 3 = Airborne (<2Gs), 4 = Airborne High G (<4Gs) 
+# (GX5 and previous,-45 models only) Dynamics Mode 1 = Portable (default), 2 = Automotive, 3 = Airborne (<2Gs), 4 = Airborne High G (<4Gs)
 filter_dynamics_mode : 1
 
 # Controls what kind of linear acceleration data is used in the Filter IMU message.
 #      If this is set to true, the acceleration will not factor out gravity, if set to false gravity will be filtered out of the linear acceleration.
 filter_use_compensated_accel : True
 
-# ZUPT control 
+# ZUPT control
 filter_velocity_zupt_topic : "/moving_vel"
 filter_angular_zupt_topic  : "/moving_ang"
 filter_velocity_zupt       : True
 filter_angular_zupt        : True
 
 # (GQ7/CV7 full support, GX5-45 limited support) Adaptive filter settings
 #      Adaptive level: 0 - off, 1 - Conservative, 2 = Moderate (default), 3 = agressive
-#      Time limit: Max duration of measurement rejection prior to recovery, in milliseconds - default = 15000 
+#      Time limit: Max duration of measurement rejection prior to recovery, in milliseconds - default = 15000
 filter_adaptive_level         : 2
 filter_adaptive_time_limit_ms : 15000
 
-# (GQ7/CV7 only) Aiding measurement control 
+# (GQ7/CV7 only) Aiding measurement control
 filter_enable_gnss_pos_vel_aiding     : True
 filter_enable_gnss_heading_aiding     : True
 filter_enable_altimeter_aiding        : False

rb_ws/src/buggy/launch/debug_filter.launch

@@ -1,7 +1,7 @@
 <!-- roslaunch buggy main.launch -->
 
 <launch>
-    <arg name="controller"      default="mpc" />
+    <arg name="controller"      default="stanley" />
     <arg name="start_dist"      default="0.0" />
     <arg name="path"      default="buggycourse_safe_1.json" />
 
@@ -22,7 +22,7 @@
     </node>
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
     <node name="telematics" pkg="buggy" type="telematics.py" />
-    <node name="publish_rtk_err" pkg="buggy" type="publish_rtk_err.py" />
+    <node name="publish_rtk_err" pkg="buggy" type="publish_rtk_err.py" output="screen"/>
 
 
     <!-- ENABLE AUTON -->

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

@@ -120,11 +120,13 @@ def tick_caller(self):
             (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
+
+
+        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("Waiting for Covariance to be better: ",  rospy.get_rostime())
         print("done checking covariance")
 
         # initialize global trajectory index
@@ -148,6 +150,7 @@ def tick_caller(self):
                     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)))
 
                 # profiling
                 if self.profile:

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

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+
+
+from abc import ABC, abstractmethod
+
+from threading import Lock
+import sys
+
+import numpy as np
+
+import rospy
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import NavSatFix
+from geometry_msgs.msg import PoseStamped
+from std_msgs.msg import Float64
+from pose import Pose
+from world import World
+
+class RTKErrPublisher():
+
+    """
+    Publish distance between front antenna and filtered output
+    """
+
+    def __init__(self) -> None:
+        self.odom_subscriber = rospy.Subscriber("/nav/odom", Odometry, self.update_odom)
+        self.gnss_subscriber = rospy.Subscriber("/gnss2/fix_Pose", PoseStamped, self.update_gnss2)
+
+        self.distance_publisher = rospy.Publisher(
+            "/gnss_odom_distance", Float64, queue_size=1
+        )
+
+        self.rate = 100
+        self.odom_msg = None
+        self.gnss2_msg = None
+        self.lock = Lock()
+
+
+    def update_odom(self, msg):
+        with self.lock:
+            self.odom_msg = msg
+
+    def update_gnss2(self, msg):
+        with self.lock:
+            self.gnss2_msg = msg
+
+        if not (self.odom_msg is None) and not (self.gnss2_msg is None):
+            odom_pose = World.gps_to_world_pose(Pose.rospose_to_pose(self.odom_msg.pose.pose))
+            gnss1_pose = World.gps_to_world_pose(Pose.rospose_to_pose(self.gnss2_msg.pose))
+
+            distance = (odom_pose.x - gnss1_pose.x) ** 2 + (odom_pose.y - gnss1_pose.y) ** 2
+            distance = np.sqrt(distance)
+
+            self.distance_publisher.publish(Float64(distance))
+
+if __name__ == "__main__":
+    rospy.init_node("publish_rtk_err")
+    d = RTKErrPublisher()
+    rospy.spin()