Skip to content

Commit

Permalink
Update telematics.py
Browse files Browse the repository at this point in the history
  • Loading branch information
christianvluu authored Nov 19, 2023
1 parent 752c93e commit 4cdf6b1
Showing 1 changed file with 6 additions and 10 deletions.
16 changes: 6 additions & 10 deletions rb_ws/src/buggy/scripts/visualization/telematics.py
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,11 @@ def __init__(self):
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:
Expand All @@ -47,7 +47,7 @@ def convert_odometry_to_navsatfix(self, msg):
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.
Expand All @@ -69,14 +69,14 @@ def convert_navsatfix_to_pose_covariance(self, msg, publishers):
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):
Expand All @@ -91,17 +91,13 @@ def republish_fixinfo(self, msg, publishers):
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()

0 comments on commit 4cdf6b1

Please sign in to comment.