From 6423af01b400a3400d2c0b0fe9871fae680b9b4e Mon Sep 17 00:00:00 2001 From: Rishi Kumar Date: Sun, 5 Jan 2025 13:14:50 -0500 Subject: [PATCH 1/3] Converted RoboBuggy2 telematics.py to use ROS2 --- .../buggy/buggy/visualization/telematics.py | 119 ++++++++++++++++++ 1 file changed, 119 insertions(+) create mode 100644 rb_ws/src/buggy/buggy/visualization/telematics.py diff --git a/rb_ws/src/buggy/buggy/visualization/telematics.py b/rb_ws/src/buggy/buggy/visualization/telematics.py new file mode 100644 index 0000000..48168ce --- /dev/null +++ b/rb_ws/src/buggy/buggy/visualization/telematics.py @@ -0,0 +1,119 @@ +#! /usr/bin/env python3 +# Runs the conversion script for all telematics data + +import rclpy +from rclpy.node import Node + +from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Odometry +from sensor_msgs.msg import NavSatFix +from std_msgs.msg import Float64MultiArray +from std_msgs.msg import String +from std_msgs.msg import Int8 + +from microstrain_inertial_msgs.msg import MipGnssFixInfo + +class Telematics(Node): + """ + 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. + """ + super().__init__('telematics') + + # Implements behavior of callback_args from rospy.Subscriber + def wrap_args(callback, callback_args): + return lambda msg: callback(msg, callback_args) + + self.odom_subscriber = self.create_subscription(Odometry, "/NAND/nav/odom", self.convert_odometry_to_navsatfix, 1) + self.odom_publisher = self.create_publisher(NavSatFix, "/NAND/nav/odom_NavSatFix", 10) + + self.gnss1_pose_publisher = self.create_publisher(PoseStamped, "/gnss1/fix_Pose", 10) + self.gnss1_covariance_publisher = self.create_publisher(Float64MultiArray, "/gnss1/fix_FloatArray_Covariance", 10) + self.gnss1_subscriber = self.create_subscription(NavSatFix, "/gnss1/llh_position", wrap_args(self.convert_navsatfix_to_pose_covariance, (self.gnss1_pose_publisher, self.gnss1_covariance_publisher)), 1) + + self.gnss2_pose_publisher = self.create_publisher(PoseStamped, "/gnss2/fix_Pose", 10) + self.gnss2_covariance_publisher = self.create_publisher(Float64MultiArray, "/gnss2/fix_FloatArray_Covariance", 10) + self.gnss2_subscriber = self.create_subscription(NavSatFix, "/gnss2/llh_position", wrap_args(self.convert_navsatfix_to_pose_covariance, (self.gnss2_pose_publisher, self.gnss2_covariance_publisher)), 1) + + self.gnss1_fixinfo_publisher = self.create_publisher(String, "/gnss1/fix_info_republished", 10) + self.gnss1_fixinfo_int_publisher = self.create_publisher(Int8, "/gnss1/fix_info_republished_int", 10) + self.gnss1_fixinfo_subscriber = self.create_subscription(MipGnssFixInfo, "/mip/gnss1/fix_info", wrap_args(self.republish_fixinfo, (self.gnss1_fixinfo_publisher, self.gnss1_fixinfo_int_publisher)), 1) + + self.gnss2_fixinfo_publisher = self.create_publisher(String, "/gnss2/fix_info_republished", 10) + self.gnss2_fixinfo_int_publisher = self.create_publisher(Int8, "/gnss2/fix_info_republished_int", 10) + self.gnss2_fixinfo_subscriber = self.create_subscription(MipGnssFixInfo, "/mip/gnss2/fix_info", wrap_args(self.republish_fixinfo, (self.gnss2_fixinfo_publisher, self.gnss2_fixinfo_int_publisher)), 1) + + 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__": + rclpy.init() + telem = Telematics() + rclpy.spin(telem) + + telem.destroy_node() + rclpy.shutdown() \ No newline at end of file From 5571f72333b806a419fc209a64aee55db4fa803f Mon Sep 17 00:00:00 2001 From: Rishi Kumar Date: Tue, 7 Jan 2025 00:08:30 +0000 Subject: [PATCH 2/3] self/state and other/state BuggyState conversion to NavSatFix --- .../buggy/buggy/visualization/telematics.py | 88 +++---------------- 1 file changed, 12 insertions(+), 76 deletions(-) diff --git a/rb_ws/src/buggy/buggy/visualization/telematics.py b/rb_ws/src/buggy/buggy/visualization/telematics.py index 48168ce..0a7e4d3 100644 --- a/rb_ws/src/buggy/buggy/visualization/telematics.py +++ b/rb_ws/src/buggy/buggy/visualization/telematics.py @@ -2,16 +2,10 @@ # Runs the conversion script for all telematics data import rclpy -from rclpy.node import Node - -from geometry_msgs.msg import PoseStamped from nav_msgs.msg import Odometry +from rclpy.node import Node from sensor_msgs.msg import NavSatFix -from std_msgs.msg import Float64MultiArray -from std_msgs.msg import String -from std_msgs.msg import Int8 -from microstrain_inertial_msgs.msg import MipGnssFixInfo class Telematics(Node): """ @@ -27,29 +21,19 @@ def __init__(self): def wrap_args(callback, callback_args): return lambda msg: callback(msg, callback_args) - self.odom_subscriber = self.create_subscription(Odometry, "/NAND/nav/odom", self.convert_odometry_to_navsatfix, 1) - self.odom_publisher = self.create_publisher(NavSatFix, "/NAND/nav/odom_NavSatFix", 10) - - self.gnss1_pose_publisher = self.create_publisher(PoseStamped, "/gnss1/fix_Pose", 10) - self.gnss1_covariance_publisher = self.create_publisher(Float64MultiArray, "/gnss1/fix_FloatArray_Covariance", 10) - self.gnss1_subscriber = self.create_subscription(NavSatFix, "/gnss1/llh_position", wrap_args(self.convert_navsatfix_to_pose_covariance, (self.gnss1_pose_publisher, self.gnss1_covariance_publisher)), 1) + self.self_publisher = self.create_publisher(NavSatFix, "/self/state_navsatfix", 10) + self.self_subscriber = self.create_subscription(Odometry, "/self/state", wrap_args(self.convert_buggystate, self.self_publisher), 1) - self.gnss2_pose_publisher = self.create_publisher(PoseStamped, "/gnss2/fix_Pose", 10) - self.gnss2_covariance_publisher = self.create_publisher(Float64MultiArray, "/gnss2/fix_FloatArray_Covariance", 10) - self.gnss2_subscriber = self.create_subscription(NavSatFix, "/gnss2/llh_position", wrap_args(self.convert_navsatfix_to_pose_covariance, (self.gnss2_pose_publisher, self.gnss2_covariance_publisher)), 1) + self.other_publisher = self.create_publisher(NavSatFix, "/other/state_navsatfix", 10) + self.other_subscriber = self.create_subscription(Odometry, "/other/state", wrap_args(self.convert_buggystate, self.other_publisher), 1) - self.gnss1_fixinfo_publisher = self.create_publisher(String, "/gnss1/fix_info_republished", 10) - self.gnss1_fixinfo_int_publisher = self.create_publisher(Int8, "/gnss1/fix_info_republished_int", 10) - self.gnss1_fixinfo_subscriber = self.create_subscription(MipGnssFixInfo, "/mip/gnss1/fix_info", wrap_args(self.republish_fixinfo, (self.gnss1_fixinfo_publisher, self.gnss1_fixinfo_int_publisher)), 1) - - self.gnss2_fixinfo_publisher = self.create_publisher(String, "/gnss2/fix_info_republished", 10) - self.gnss2_fixinfo_int_publisher = self.create_publisher(Int8, "/gnss2/fix_info_republished_int", 10) - self.gnss2_fixinfo_subscriber = self.create_subscription(MipGnssFixInfo, "/mip/gnss2/fix_info", wrap_args(self.republish_fixinfo, (self.gnss2_fixinfo_publisher, self.gnss2_fixinfo_int_publisher)), 1) - - def convert_odometry_to_navsatfix(self, msg): - """Convert Odometry-type to NavSatFix-type for plotting on Foxglove + # TODO Make this a static method? + def convert_buggystate(self, msg, publisher): + """Converts BuggyState/Odometry message to NavSatFix and publishes to specified publisher + Args: - msg (Odometry): odometry as per INS + msg (Odometry): Buggy state to convert + publisher (Publisher): Publisher to send NavSatFix message to """ lat = msg.pose.pose.position.y long = msg.pose.pose.position.x @@ -59,56 +43,8 @@ def convert_odometry_to_navsatfix(self, msg): 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" + publisher.publish(new_msg) - # 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__": rclpy.init() From 5479b6336fff75408417d97d552cbed61d947fd3 Mon Sep 17 00:00:00 2001 From: Rishi Kumar Date: Tue, 7 Jan 2025 19:06:23 +0000 Subject: [PATCH 3/3] Added converstion from utm -> latlon --- .../buggy/buggy/visualization/telematics.py | 26 ++++++++++++------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/rb_ws/src/buggy/buggy/visualization/telematics.py b/rb_ws/src/buggy/buggy/visualization/telematics.py index 0a7e4d3..9d1fd80 100644 --- a/rb_ws/src/buggy/buggy/visualization/telematics.py +++ b/rb_ws/src/buggy/buggy/visualization/telematics.py @@ -2,6 +2,7 @@ # Runs the conversion script for all telematics data import rclpy +import utm from nav_msgs.msg import Odometry from rclpy.node import Node from sensor_msgs.msg import NavSatFix @@ -35,15 +36,22 @@ def convert_buggystate(self, msg, publisher): msg (Odometry): Buggy state to convert publisher (Publisher): Publisher to send NavSatFix message to """ - 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 - publisher.publish(new_msg) + try: + y = msg.pose.pose.position.y + x = msg.pose.pose.position.x + lat, long = utm.to_latlon(x, y, 17, "T") + 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 + publisher.publish(new_msg) + + except Exception as e: + self.get_logger().warn( + "Unable to convert other buggy position to lon lat" + str(e) + ) if __name__ == "__main__":