From 01cd57882085994fd729929efefdbc40cc8fc2c6 Mon Sep 17 00:00:00 2001 From: Buggy Date: Tue, 27 Feb 2024 16:55:52 -0500 Subject: [PATCH] Turned on debug logging for INS --- rb_ws/src/buggy/INS_params.yml | 52 ++++++++-------- rb_ws/src/buggy/launch/debug_filter.launch | 4 +- rb_ws/src/buggy/scripts/auton/autonsystem.py | 5 +- .../buggy/scripts/auton/publish_rtk_err.py | 59 +++++++++++++++++++ 4 files changed, 91 insertions(+), 29 deletions(-) create mode 100755 rb_ws/src/buggy/scripts/auton/publish_rtk_err.py diff --git a/rb_ws/src/buggy/INS_params.yml b/rb_ws/src/buggy/INS_params.yml index b5b2e0ac..1d50c468 100644 --- a/rb_ws/src/buggy/INS_params.yml +++ b/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,23 +221,23 @@ 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 @@ -245,11 +245,11 @@ 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 diff --git a/rb_ws/src/buggy/launch/debug_filter.launch b/rb_ws/src/buggy/launch/debug_filter.launch index 23b97ea8..c0a8c7be 100644 --- a/rb_ws/src/buggy/launch/debug_filter.launch +++ b/rb_ws/src/buggy/launch/debug_filter.launch @@ -1,7 +1,7 @@ - + @@ -22,7 +22,7 @@ - + diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 9b1420fa..154cdadf 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/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: diff --git a/rb_ws/src/buggy/scripts/auton/publish_rtk_err.py b/rb_ws/src/buggy/scripts/auton/publish_rtk_err.py new file mode 100755 index 00000000..04a8bb52 --- /dev/null +++ b/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() \ No newline at end of file