From ad1cba77a9b74e28d9f5ed58933642463588b669 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 26 Jun 2024 23:52:00 -0700 Subject: [PATCH 1/3] take 2 (added new buggystate file) --- rb_ws/src/buggy/scripts/auton/buggystate.py | 116 ++++++++++++++++++++ 1 file changed, 116 insertions(+) create mode 100644 rb_ws/src/buggy/scripts/auton/buggystate.py diff --git a/rb_ws/src/buggy/scripts/auton/buggystate.py b/rb_ws/src/buggy/scripts/auton/buggystate.py new file mode 100644 index 0000000..3e06273 --- /dev/null +++ b/rb_ws/src/buggy/scripts/auton/buggystate.py @@ -0,0 +1,116 @@ +import numpy as np +import utm +from nav_msgs.msg import Odometry as ROSOdom +from sensor_msgs.msg import NavSatFix + +from tf.transformations import euler_from_quaternion +from tf.transformations import quaternion_from_euler + +from geometry_msgs.msg import PoseStamped +from std_msgs.msg import Float32, Float64, Bool + +from microstrain_inertial_msgs.msg import GNSSFixInfo + + + +import rospy + + +class BuggyState: + """ + Basically a translator from ROSOdom to ensure everything is in the correct units. + + Other files should ONLY interface with the nav/odom messages through this file! + + (functionally: this should replace telematics, pose and world) + + """ + + def __init__(self, name = "sc"): + self.filter_odom : ROSOdom = None + self.gnss_1 : ROSOdom = None + self.gnss_2 : ROSOdom = None + self.gps_fix : int = 0 + + + # to report if the filter position has separated (so we need to break) + rospy.Subscriber(name + "/debug/filter_gps_seperation_status", Bool, self.update_use_gps) + + rospy.Subscriber(name + "/nav/odom", ROSOdom, self.update_odom) + rospy.Subscriber("/gnss1/odom", ROSOdom, self.update_gnss1) + rospy.Subscriber("/gnss2/odom", ROSOdom, self.update_gnss2) + rospy.Subscriber("/gnss1/fix_info", GNSSFixInfo, self.update_gnss1_fix) + + + def update_use_gps(self, msg): + self.use_gps = msg + def update_odom(self, msg): + self.filter_odom = msg + def update_gnss1(self, msg): + self.gnss_1 = msg + def update_gnss2(self, msg): + self.gnss_2 = msg + def update_gnss1_fix(self, msg): + self.gps_fix = msg.fix_type + + def odom_to_navsatfix(self, odom): + """Convert Odometry-type to NavSatFix-type for plotting on Foxglove + Args: + odom (Odometry): odometry object to convert + """ + lat = odom.pose.pose.position.y + long = odom.pose.pose.position.x + down = odom.pose.pose.position.z + new_odom = NavSatFix() + new_odom.header = odom.header + new_odom.latitude = lat + new_odom.longitude = long + new_odom.altitude = down + return new_odom + + def get_gps_fix(self): + fix_string = "fix type: " + if (self.gps_fix == 0): + fix_string += "FIX_3D" + elif (self.gps_fix == 1): + fix_string += "FIX_2D" + elif (self.gps_fix == 2): + fix_string += "FIX_TIME_ONLY" + elif (self.gps_fix == 3): + fix_string += "FIX_NONE" + elif (self.gps_fix == 4): + fix_string += "FIX_INVALID" + elif (self.gps_fix == 5): + fix_string += "FIX_RTK_FLOAT" + else: + fix_string += "FIX_RTK_FIXED" + return fix_string + + def get_pose(self): + if self.filter_odom == None: + return None + + rospose = self.filter_odom.pose.pose + yaw = (_, _, yaw) = euler_from_quaternion( + [ + rospose.orientation.x, + rospose.orientation.y, + rospose.orientation.z, + rospose.orientation.w, + ] + ) + + easting, northing = utm.from_latlon(rospose.position.x, rospose.position.y) + + return (easting, northing, yaw) + + def get_pos_covariance(self): + if self.filter_odom == None: + return None + return self.filter_odom.pose.covariance + + def get_velocity(self): + if self.filter_odom == None: + return None + return self.filter_odom.twist.twist.linear + From 5065565a17f092c1eddd6a03a2569533b8ce5aeb Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 26 Jun 2024 23:52:20 -0700 Subject: [PATCH 2/3] pylint --- rb_ws/src/buggy/scripts/auton/buggystate.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/rb_ws/src/buggy/scripts/auton/buggystate.py b/rb_ws/src/buggy/scripts/auton/buggystate.py index 3e06273..91f4537 100644 --- a/rb_ws/src/buggy/scripts/auton/buggystate.py +++ b/rb_ws/src/buggy/scripts/auton/buggystate.py @@ -1,13 +1,10 @@ -import numpy as np import utm from nav_msgs.msg import Odometry as ROSOdom from sensor_msgs.msg import NavSatFix from tf.transformations import euler_from_quaternion -from tf.transformations import quaternion_from_euler -from geometry_msgs.msg import PoseStamped -from std_msgs.msg import Float32, Float64, Bool +from std_msgs.msg import Bool from microstrain_inertial_msgs.msg import GNSSFixInfo From 2bf9039f87d09e24c714e33157b9dfe2e917ab9d Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Thu, 27 Jun 2024 21:22:53 -0700 Subject: [PATCH 3/3] fixed latlong and pattern matching w variable names --- rb_ws/src/buggy/scripts/auton/buggystate.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rb_ws/src/buggy/scripts/auton/buggystate.py b/rb_ws/src/buggy/scripts/auton/buggystate.py index 91f4537..f806476 100644 --- a/rb_ws/src/buggy/scripts/auton/buggystate.py +++ b/rb_ws/src/buggy/scripts/auton/buggystate.py @@ -88,7 +88,7 @@ def get_pose(self): return None rospose = self.filter_odom.pose.pose - yaw = (_, _, yaw) = euler_from_quaternion( + (_, _, yaw) = euler_from_quaternion( [ rospose.orientation.x, rospose.orientation.y, @@ -97,7 +97,7 @@ def get_pose(self): ] ) - easting, northing = utm.from_latlon(rospose.position.x, rospose.position.y) + (easting, northing, _, _) = utm.from_latlon(rospose.position.y, rospose.position.x) return (easting, northing, yaw)