diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index a5924c97..394e6030 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -70,7 +70,7 @@ def __init__(self, # TODO: will need to be updated when we add namespaces self.sc_state = BuggyState("sc") - self.nand_state = BuggyState("sc") + self.nand_state = BuggyState("nand") # rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_self_odom) diff --git a/rb_ws/src/buggy/scripts/auton/buggystate.py b/rb_ws/src/buggy/scripts/auton/buggystate.py index ca4a7625..3e062739 100644 --- a/rb_ws/src/buggy/scripts/auton/buggystate.py +++ b/rb_ws/src/buggy/scripts/auton/buggystate.py @@ -1,12 +1,17 @@ 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 @@ -17,41 +22,75 @@ class BuggyState: Other files should ONLY interface with the nav/odom messages through this file! - (functionally: this should replace pose and world) + (functionally: this should replace telematics, pose and world) """ - # TODO: point of discussion: do we want to create a custom message for "utm_pose" since we are constantly passing around easting, northing, yaw? or do we just want to create a really simple wrapper for utm_pose with some translators to more publishable ros poses? (kind of in favor of this...) - # + def __init__(self, name = "sc"): - self.use_gps = False - self.rosodom : ROSOdom = None - self.gpspose : PoseStamped = None + 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 use the antenna position) + + # 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) - # TODO: this node is reading in a posestamped, do we want to translate HERE or translate in telematics? - self.gnss_subscriber = rospy.Subscriber(name + "/gnss1/fix_Pose", PoseStamped, self.update_gnss) def update_use_gps(self, msg): self.use_gps = msg - def update_odom(self, msg): - self.rosodom = msg - - def update_gnss(self, msg): - self.gpspose = 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.rosodom == None: + if self.filter_odom == None: return None - if self.use_gps: - return self.gpspose.pose - - rospose = self.rosodom.pose.pose + rospose = self.filter_odom.pose.pose yaw = (_, _, yaw) = euler_from_quaternion( [ rospose.orientation.x, @@ -66,16 +105,12 @@ def get_pose(self): return (easting, northing, yaw) def get_pos_covariance(self): - if self.rosodom == None: + if self.filter_odom == None: return None - return self.rosodom.pose.covariance + return self.filter_odom.pose.covariance - # TODO: finish this! def get_velocity(self): - if self.rosodom == None: + if self.filter_odom == None: return None - return None + return self.filter_odom.twist.twist.linear - # TODO: finish this! - def create_odom(self): - return None diff --git a/rb_ws/src/buggy/scripts/auton/pose.py b/rb_ws/src/buggy/scripts/auton/pose.py index 32f098cf..7853abec 100644 --- a/rb_ws/src/buggy/scripts/auton/pose.py +++ b/rb_ws/src/buggy/scripts/auton/pose.py @@ -10,7 +10,7 @@ class Pose: A data structure for storing 2D poses, as well as a set of convenience methods for transforming/manipulating poses - TODO: All Pose objects are with respect to World coordinates. + NOTE: this class should be deprecated eventually! it is currently ONLY to be used in the MPC calculations where relevant, for all other purposes, use buggystate! """ __x = None @@ -49,12 +49,10 @@ def __init__(self, x, y, theta): self.y = y self.theta = theta + # NOTE: what is this O_o it is referenced in a file i do not understand. def __repr__(self) -> str: return f"Pose(x={self.x}, y={self.y}, theta={self.theta})" - def copy(self): - return Pose(self.x, self.y, self.theta) - @property def x(self): return self.__x @@ -107,20 +105,6 @@ def invert(self): """ return Pose.from_mat(np.linalg.inv(self.to_mat())) - def convert_pose_from_global_to_local_frame(self, pose): - """ - Converts the inputted pose from the global frame to the local frame - (relative to the current pose) - """ - return pose / self - - def convert_pose_from_local_to_global_frame(self, pose): - """ - Converts the inputted pose from the local frame to the global frame - (relative to the current pose) - """ - return pose * self - def convert_point_from_global_to_local_frame(self, point): """ Converts the inputted point from the global frame to the local frame @@ -133,77 +117,6 @@ def convert_point_from_global_to_local_frame(self, point): point_hg_local[1] / point_hg_local[2], ) - def convert_point_from_local_to_global_frame(self, point): - """ - Converts the inputted point from the local frame to the global frame - (relative to the current pose) - """ - point_hg = np.array([point[0], point[1], 1]) - point_hg_global = self.to_mat() @ point_hg - return ( - point_hg_global[0] / point_hg_global[2], - point_hg_global[1] / point_hg_global[2], - ) - - def convert_point_array_from_global_to_local_frame(self, points): - """ - Converts the inputted point array from the global frame to the local frame - (relative to the current pose) - - Args: - points (np.ndarray) [Size: (N,2)]: array of points to convert - """ - points_hg = np.array([points[:, 0], points[:, 1], np.ones(len(points))]) - points_hg_local = np.linalg.inv(self.to_mat()) @ points_hg.T - return (points_hg_local[:2, :] / points_hg_local[2, :]).T - - def convert_point_array_from_local_to_global_frame(self, points): - """ - Converts the inputted point array from the local frame to the global frame - (relative to the current pose) - - Args: - points (np.ndarray) [Size: (N,2)]: array of points to convert - """ - points_hg = np.array([points[:, 0], points[:, 1], np.ones(len(points))]) - points_hg_global = self.to_mat() @ points_hg.T - return (points_hg_global[:2, :] / points_hg_global[2, :]).T - - def rotate(self, angle): - """ - Rotates the pose by the given angle - """ - self.theta += angle - - def translate(self, x, y): - """ - Translates the pose by the given x and y distances - """ - self.x += x - self.y += y - - def __add__(self, other): - return Pose(self.x + other.x, self.y + other.y, self.theta + other.theta) - - def __sub__(self, other): - return Pose(self.x - other.x, self.y - other.y, self.theta - other.theta) - - def __neg__(self): - return Pose(-self.x, -self.y, -self.theta) - - def __mul__(self, other): - p1_mat = self.to_mat() - p2_mat = other.to_mat() - - return Pose.from_mat(p1_mat @ p2_mat) - - def __truediv__(self, other): - p1_mat = self.to_mat() - p2_mat = other.to_mat() - - return Pose.from_mat(np.linalg.inv(p2_mat) @ p1_mat) - - if __name__ == "__main__": # TODO: again do we want example code in these classes rospose = ROSPose() diff --git a/rb_ws/src/buggy/scripts/visualization/telematics.py b/rb_ws/src/buggy/scripts/visualization/telematics.py deleted file mode 100755 index 182b2743..00000000 --- a/rb_ws/src/buggy/scripts/visualization/telematics.py +++ /dev/null @@ -1,108 +0,0 @@ -#! /usr/bin/env python3 -# Runs the conversion script for all telematics data -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 Float64MultiArray -from std_msgs.msg import String -from std_msgs.msg import Int8 - -from microstrain_inertial_msgs.msg import GNSSFixInfo - -class Telematics: - """ - Converts subscribers and publishers that need to be reformated, so that they are readible. - """ - - def __init__(self): - """Generate all the subscribers and publishers that need to be reformatted. - """ - - self.odom_subscriber = rospy.Subscriber("/NAND/nav/odom", Odometry, self.convert_odometry_to_navsatfix) - self.odom_publisher = rospy.Publisher("/NAND/nav/odom_NavSatFix", NavSatFix, queue_size=10) - - self.gnss1_pose_publisher = rospy.Publisher("/gnss1/fix_Pose", PoseStamped, queue_size=10) - self.gnss1_covariance_publisher = rospy.Publisher("/gnss1/fix_FloatArray_Covariance", Float64MultiArray, queue_size=10) - self.gnss1_subscriber = rospy.Subscriber("/gnss1/fix", NavSatFix, self.convert_navsatfix_to_pose_covariance, (self.gnss1_pose_publisher, self.gnss1_covariance_publisher)) - - self.gnss2_pose_publisher = rospy.Publisher("/gnss2/fix_Pose", PoseStamped, queue_size=10) - self.gnss2_covariance_publisher = rospy.Publisher("/gnss2/fix_FloatArray_Covariance", Float64MultiArray, queue_size=10) - self.gnss2_subscriber = rospy.Subscriber("/gnss2/fix", NavSatFix, self.convert_navsatfix_to_pose_covariance, (self.gnss2_pose_publisher, self.gnss2_covariance_publisher)) - - self.gnss1_fixinfo_publisher = rospy.Publisher("/gnss1/fix_info_republished", String, queue_size=10) - self.gnss1_fixinfo_int_publisher = rospy.Publisher("/gnss1/fix_info_republished_int", Int8, queue_size=10) - self.gnss1_fixinfo_subscriber = rospy.Subscriber("/gnss1/fix_info", GNSSFixInfo, self.republish_fixinfo, (self.gnss1_fixinfo_publisher, self.gnss1_fixinfo_int_publisher)) - - self.gnss2_fixinfo_publisher = rospy.Publisher("/gnss2/fix_info_republished", String, queue_size=10) - self.gnss2_fixinfo_int_publisher = rospy.Publisher("/gnss2/fix_info_republished_int", Int8, queue_size=10) - self.gnss2_fixinfo_subscriber = rospy.Subscriber("/gnss2/fix_info", GNSSFixInfo, self.republish_fixinfo, (self.gnss2_fixinfo_publisher, self.gnss2_fixinfo_int_publisher)) - - def convert_odometry_to_navsatfix(self, msg): - """Convert Odometry-type to NavSatFix-type for plotting on Foxglove - Args: - msg (Odometry): odometry as per INS - """ - lat = msg.pose.pose.position.y - long = msg.pose.pose.position.x - down = msg.pose.pose.position.z - new_msg = NavSatFix() - new_msg.header = msg.header - new_msg.latitude = lat - new_msg.longitude = long - new_msg.altitude = down - self.odom_publisher.publish(new_msg) - - def convert_navsatfix_to_pose_covariance(self, msg, publishers): - """Convert NavSatFix-type and related covariance matrix to Pose-type and array - respectively for easy visualization in Foxglove. - - Args: - msg (NavSatFix): original msg - publishers (tuple): tuple of publishes - """ - pose = PoseStamped() - pose.pose.position.y = msg.latitude - pose.pose.position.x = msg.longitude - pose.pose.position.z = msg.altitude - pose.header = msg.header - publishers[0].publish(pose) - - covariance = Float64MultiArray() - covariance.data = [ - round(msg.position_covariance[0], 4), - round(msg.position_covariance[4], 4), - round(msg.position_covariance[8], 4)] - publishers[1].publish(covariance) - - def republish_fixinfo(self, msg, publishers): - """ - convert gnss/fixinfo to a string for visualization in foxglove - """ - fix_type = msg.fix_type - fix_string = "fix type: " - - if (fix_type == 0): - fix_string += "FIX_3D" - elif (fix_type == 1): - fix_string += "FIX_2D" - elif (fix_type == 2): - fix_string += "FIX_TIME_ONLY" - elif (fix_type == 3): - fix_string += "FIX_NONE" - elif (fix_type == 4): - fix_string += "FIX_INVALID" - elif (fix_type == 5): - fix_string += "FIX_RTK_FLOAT" - else: - fix_string += "FIX_RTK_FIXED" - - fix_string += "\nsbas_used: " + str(msg.sbas_used) - fix_string += "\ndngss_used: " + str(msg.dngss_used) - publishers[0].publish(fix_string) - publishers[1].publish(fix_type) - -if __name__ == "__main__": - rospy.init_node("telematics") - telem = Telematics() - rospy.spin()