From 5efb025df7f8a97ddebcc4a7b1e9d1950d85b0bb Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 18 Dec 2024 20:56:50 -0800 Subject: [PATCH 01/18] created dataclasses for new packets --- rb_ws/src/buggy/buggy/serial/host_comm.py | 132 ++++++++++++++---- .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 7 - 2 files changed, 105 insertions(+), 34 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/host_comm.py b/rb_ws/src/buggy/buggy/serial/host_comm.py index e98c23e..76f4859 100644 --- a/rb_ws/src/buggy/buggy/serial/host_comm.py +++ b/rb_ws/src/buggy/buggy/serial/host_comm.py @@ -58,28 +58,88 @@ def update(self, b): MAX_PAYLOAD_LEN = 100 -MSG_TYPE_DEBUG = b'DB' -MSG_TYPE_ODOMETRY = b'OD' + +# firmware --> software +MSG_TYPE_NAND_DEBUG = b'ND' +MSG_TYPE_NAND_UKF = b'NU' +MSG_TYPE_NAND_GPS = b'NG' +MSG_TYPE_RADIO = b'RA' +MSG_TYPE_SC_DEBUG = b'SD' +MSG_TYPE_SC_SENSORS = b'SS' +MSG_TYPE_ROUNDTRIP_TIMESTAMP = b'RT' + +# software --> firmware MSG_TYPE_STEERING = b'ST' -MSG_TYPE_BRAKE = b'BR' MSG_TYPE_ALARM = b'AL' -MSG_TYPE_BNYATEL = b'BT' +MSG_TYPE_SOFTWARE_TIMESTAMP = b'TM' + + + +@dataclass +class NANDDebug: + timestamp: int + rc_steering_angle: float + stepper_steering_angle: float + accelerometer: float + encoder_angle: float + operator_ready: bool + brake_status: bool + auton_steer: bool + rfm69_timeout: bool + tx12_state: bool + stepper_alarm: bool + rc_uplink_quality: int + +@dataclass +class NANDUKF: + timestamp: int + easting: float + northing: float + theta: float + omega: float + velocity: float @dataclass -class Odometry: - x: float - y: float - radio_seqnum: int +class NANDGPS: gps_seqnum: int + easting: float + northing: float + gps_fix: int + # this is a 2D accuracy value + accuracy: int + timestamp: int + +@dataclass +class Radio: + gps_seqnum: int + nand_x_gps: float + nand_y_gps: float + nand_gps_fix: float + +@dataclass +class SCDebug: + timestamp: int + rc_steering_angle: float + stepper_steering_angle: float + # encoder_angle: float + operator_ready: bool + brake_status: bool + auton_steer: bool + tx12_state: bool + stepper_alarm: bool + rc_uplink_quality: int + + +@dataclass +class SCSensors: + steering: float + # velocity: float + timestamp: int @dataclass -class BnyaTelemetry: - x: float - y: float - velocity: float # In METERS / SECOND - steering: float # In DEGREES - heading: float # In RADIANS - heading_rate: float # In RADIANS / SECOND +class RoundtripTimestamp: + returned_time: int + class IncompletePacket(Exception): pass @@ -115,9 +175,11 @@ def send_steering(self, angle: float): def send_alarm(self, status: int): self.send_packet_raw(MSG_TYPE_ALARM, struct.pack(' (Bnyahaj) Stepper Motor - (Bnyahaj) NAND Odom --> (ROS) NAND Odom topic - - NAND: - (ROS) Self Steering --> (Bnyahaj) Stepper Motor - (Bnyahaj) Self Odom from UKF --> (ROS) NAND Odom topic """ def __init__(self, self_name, other_name, teensy_name): From 4f5663ada8f1f96e327c035c22fefa7f7c72826b Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Thu, 19 Dec 2024 16:04:50 -0800 Subject: [PATCH 02/18] fixed packet types --- rb_ws/src/buggy/buggy/serial/host_comm.py | 78 ++++++++++++++--------- 1 file changed, 48 insertions(+), 30 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/host_comm.py b/rb_ws/src/buggy/buggy/serial/host_comm.py index 76f4859..97b93e8 100644 --- a/rb_ws/src/buggy/buggy/serial/host_comm.py +++ b/rb_ws/src/buggy/buggy/serial/host_comm.py @@ -76,64 +76,82 @@ def update(self, b): @dataclass -class NANDDebug: +class NANDDebugInfo: + # 64 bits + heading_rate: float # double + encoder_angle: float # double + # 32 bits timestamp: int rc_steering_angle: float - stepper_steering_angle: float - accelerometer: float - encoder_angle: float + software_steering_angle: float + true_steering_angle: float + rfm69_timeout_num: int + # 8 bits operator_ready: bool brake_status: bool auton_steer: bool - rfm69_timeout: bool tx12_state: bool - stepper_alarm: bool - rc_uplink_quality: int + stepper_alarm: int # unsigned char + rc_uplink_quality: int # uint8 @dataclass class NANDUKF: + # 64 bits + easting: float # double + northing: float # double + theta: float # double + heading_rate: float # double + velocity: float # double + # 32 bits timestamp: int - easting: float - northing: float - theta: float - omega: float - velocity: float @dataclass -class NANDGPS: - gps_seqnum: int - easting: float - northing: float - gps_fix: int +class NANDRawGPS: + # 64 bits + easting: float # double + northing: float # double # this is a 2D accuracy value - accuracy: int + accuracy: float # double + gps_time: int # uint64 + # 32 bits + gps_seqnum: int timestamp: int + # 8 bits + gps_fix: int # uint8 +# TODO: make sure this is same as on firmware @dataclass class Radio: - gps_seqnum: int nand_x_gps: float nand_y_gps: float - nand_gps_fix: float + gps_seqnum: int + nand_gps_fix: int # uint8 @dataclass -class SCDebug: - timestamp: int +class SCDebugInfo: + # 64 bits + encoder_angle: float # double + # 32 bits rc_steering_angle: float - stepper_steering_angle: float - # encoder_angle: float + software_steering_angle: float + true_steering_angle: float + missed_packets: int + timestamp: int + # 8 bits + tx12_state: bool operator_ready: bool + stepper_alarm: int # unsigned char brake_status: bool auton_steer: bool - tx12_state: bool - stepper_alarm: bool - rc_uplink_quality: int - + rc_uplink_quality: int # uint8 @dataclass class SCSensors: - steering: float - # velocity: float + # 64 bits + velocity: float # double + # 32 bits + # TODO: this isnt on firmware rn? + steering_angle: float timestamp: int @dataclass From 70ce62bf27adc711f9f5ff62a6f42f5be0ffa657 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Mon, 23 Dec 2024 15:35:28 -0800 Subject: [PATCH 03/18] updated unpacking new packets --- rb_ws/src/buggy/buggy/serial/host_comm.py | 32 +++++++++++------------ 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/host_comm.py b/rb_ws/src/buggy/buggy/serial/host_comm.py index 97b93e8..873dbb5 100644 --- a/rb_ws/src/buggy/buggy/serial/host_comm.py +++ b/rb_ws/src/buggy/buggy/serial/host_comm.py @@ -150,13 +150,12 @@ class SCSensors: # 64 bits velocity: float # double # 32 bits - # TODO: this isnt on firmware rn? steering_angle: float timestamp: int @dataclass class RoundtripTimestamp: - returned_time: int + returned_time: float # double class IncompletePacket(Exception): @@ -193,6 +192,7 @@ def send_steering(self, angle: float): def send_alarm(self, status: int): self.send_packet_raw(MSG_TYPE_ALARM, struct.pack(' Date: Mon, 23 Dec 2024 16:42:03 -0800 Subject: [PATCH 04/18] added all publishers required --- .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 82 ++++++++++++------- 1 file changed, 53 insertions(+), 29 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index 4335be4..54df8f1 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -11,7 +11,7 @@ from host_comm import * -from nav_msgs.msg import Odometry as ROSOdom +from nav_msgs.msg import Odometry class Translator(Node): """ @@ -28,6 +28,8 @@ def __init__(self, self_name, other_name, teensy_name): Initializes the subscribers, rates, and ros topics (including debug topics) """ + # TODO: if we switch to using namespaces everywhere, use get_namespace instead of passing around self_name + super().__init__('ROS_serial_translator') self.comms = Comms("/dev/" + teensy_name) @@ -39,19 +41,8 @@ def __init__(self, self_name, other_name, teensy_name): self.create_subscription( Float64, self_name + "/buggy/input/steering", self.set_steering, 1 ) - self.create_subscription(Int8, self_name + "/debug/sanity_warning", self.set_alarm, 1) - # ISSUE: https://github.com/CMU-Robotics-Club/RoboBuggy2/issues/83 - if other_name is None and self_name == "NAND": - self.odom_publisher = self.create_publisher( - ROSOdom, self_name + "/nav/odom", 1 - ) - else: - self.odom_publisher = self.create_publisher( - ROSOdom, other_name + "/nav/odom", 1 - ) - # upper bound of steering update rate, make sure auton sends slower than 500 Hz or update / 2ms self.steer_send_rate = self.create_rate(500) @@ -59,34 +50,73 @@ def __init__(self, self_name, other_name, teensy_name): self.read_rate = self.create_rate(1000) # ISSUE: https://github.com/CMU-Robotics-Club/RoboBuggy2/issues/90 + # TODO: why are all the topics published under buggy/ ? + # DEBUG MESSAGE PUBLISHERS: + self.heading_rate_publisher = self.create_publisher( + Float64, self_name + "/buggy/debug/heading_rate", 1 + ) + self.encoder_angle_publisher = self.create_publisher( + Float64, self_name + "/buggy/debug/encoder_angle", 1 + ) self.rc_steering_angle_publisher = self.create_publisher( Float64, self_name + "/buggy/debug/rc_steering_angle", 1 ) - self.steering_angle_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/steering_angle", 1 + self.software_steering_angle_publisher = self.create_publisher( + Float64, self_name + "/buggy/debug/software_steering_angle", 1 + ) + self.true_steering_angle_publisher = self.create_publisher( + Float64, self_name + "/buggy/debug/true_steering_angle", 1 ) - self.battery_voltage_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/battery_voltage", 1 + self.rfm69_timeout_num_publisher = self.create_publisher( + int, self_name + "/buggy/debug/rfm_timeout_num", 1 ) self.operator_ready_publisher = self.create_publisher( Bool, self_name + "/buggy/debug/operator_ready", 1 ) - self.steering_alarm_publisher = self.create_publisher( - Bool, self_name + "/buggy/debug/steering_alarm", 1 - ) self.brake_status_publisher = self.create_publisher( Bool, self_name + "/buggy/debug/brake_status", 1 ) self.use_auton_steer_publisher = self.create_publisher( Bool, self_name + "/buggy/debug/use_auton_steer", 1 ) + self.tx12_state_publisher = self.create_publisher( + Bool, self_name + "/buggy/debug/tx12_connected", 1 + ) + self.stepper_alarm_publisher = self.create_publisher( + UInt8, self_name + "/buggy/debug/steering_alarm", 1 + ) self.rc_uplink_qual_publisher = self.create_publisher( UInt8, self_name + "/buggy/debug/rc_uplink_quality", 1 ) - self.nand_fix_publisher = self.create_publisher( - UInt8, self_name + "/buggy/debug/nand_fix", 1 + + # NAND POSITION PUBLISHERS + self.nand_ukf_odom_publisher = self.create_publisher( + Odometry, "NAND/nav/odom", 1 + ) + self.nand_gps_odom_publisher = self.create_publisher( + Odometry, "NAND/buggy/debug/gps_odom", 1 + ) + self.observed_nand_odom_publisher = self.create_publisher( + Odometry, "SC/NAND_odom", 1 + ) + self.nand_gps_fix_publisher = self.create_publisher( + UInt8, "NAND/buggy/debug/gps_fix", 1 + ) + + # SC SENSOR PUBLISHERS + self.sc_velocity_publisher = self.create_publisher( + Float64, "SC/sensors/velocity", 1 + ) + self.sc_steering_angle_publisher = self.create_publisher( + Float64, "SC/sensors/steering_angle", 1 ) + # SERIAL DEBUG PUBLISHERS + self.roundtrip_time_publisher = self.create_publisher( + Float64, self_name + "/buggy/debug/roundtrip_time", 1 + ) + + def set_alarm(self, msg): """ alarm ros topic reader, locked so that only one of the setters runs at once @@ -99,18 +129,18 @@ def set_steering(self, msg): """ Steering Angle Updater, updates the steering angle locally if updated on ros stopic """ - self.get_logger().debug(f"Read steeering angle of: {msg.data}") + self.get_logger().debug(f"Read steering angle of: {msg.data}") # print("Steering angle: " + str(msg.data)) # print("SET STEERING: " + str(msg.data)) with self.lock: self.steer_angle = msg.data self.fresh_steer = True + def writer_thread(self): """ Sends ROS Topics to bnayhaj serial, only sends a steering angle when we receive a fresh one Will send steering and alarm node. - TODO: Does alarm node exist for NAND? """ self.get_logger().info("Starting sending alarm and steering to teensy!") @@ -127,12 +157,6 @@ def writer_thread(self): self.steer_send_rate.sleep() def reader_thread(self): - """ - Reads three different types of packets, that should have better ways of differntiating - Odometry -> (SC) NAND Odomotery - BnyaTelemetry -> (NAND) Self Odom - tuple -> (SC, maybe NAND?) Debug Info - """ self.get_logger().info("Starting reading odom from teensy!") while rclpy.ok(): packet = self.comms.read_packet() From 69ddd2c172c03a1af107b887cbf4dd1c5996a6ba Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Mon, 23 Dec 2024 16:54:09 -0800 Subject: [PATCH 05/18] started publishing based on updated packet types --- .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 203 ++++++++++++++++-- 1 file changed, 187 insertions(+), 16 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index 54df8f1..f1d0628 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -90,6 +90,7 @@ def __init__(self, self_name, other_name, teensy_name): ) # NAND POSITION PUBLISHERS + #TODO: these should be buggy_state publishers, NOT odometry! self.nand_ukf_odom_publisher = self.create_publisher( Odometry, "NAND/nav/odom", 1 ) @@ -162,9 +163,109 @@ def reader_thread(self): packet = self.comms.read_packet() self.get_logger().debug("packet" + str(packet)) - if isinstance(packet, Odometry): + # TODO: do we want to add double checks to make sure the correct buggy is recieveing the packet? + if isinstance(packet, NANDDebugInfo): + self.heading_rate_publisher.publish(packet.heading_rate) + self.encoder_angle_publisher.publish(packet.encoder_angle) + self.rc_steering_angle_publisher.publish(packet.rc_steering_angle) + self.software_steering_angle_publisher.publish(packet.software_steering_angle) + self.true_steering_angle_publisher.publish(packet.true_steering_angle) + self.rfm69_timeout_num_publisher.publish(packet.rfm69_timeout_num) + self.operator_ready_publisher.publish(packet.operator_ready) + self.brake_status_publisher.publish(packet.brake_status) + self.use_auton_steer_publisher.publish(packet.auton_steer) + self.tx12_state_publisher.publish(packet.tx12_state) + self.stepper_alarm_publisher.publish(packet.stepper_alarm) + self.rc_uplink_qual_publisher.publish(packet.rc_uplink_quality) + + elif isinstance(packet, NANDUKF): + odom = Odometry() + + # IMPORTANT TODO: all data here is in UTM even though *technically* Odometry uses lat/long - once buggystate is merged and exists, all these packets need to be changed to buggystate type + try: + lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") + odom.pose.pose.position.x = long + odom.pose.pose.position.y = lat + odom.twist.twist.angular.z = packet.heading_rate + heading = R.from_euler('x', packet.heading, degrees=False).as_quat() + + odom.pose.pose.orientation.x = heading[0] + odom.pose.pose.orientation.y = heading[1] + odom.pose.pose.orientation.z = heading[2] + odom.pose.pose.orientation.w = heading[3] + + speed = packet.velocity + # TODO: fix this + # why this works: autonsystem only cares about magnitude of velocity + # so setting an arbitrary axis to the speed and leave other at 0 + # works. However, this should be done properly ASAP + odom.twist.twist.linear.x = speed + self.odom_publisher.publish(odom) + except Exception as e: + self.get_logger().warn( + "Unable to convert other buggy position to lon lat" + str(e) + ) + + + + elif isinstance(packet, NANDUKF): + odom = Odometry() + + # TODO: Not mock rolled accurately (Needs to be Fact Checked) + try: + lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") + odom.pose.pose.position.x = long + odom.pose.pose.position.y = lat + odom.twist.twist.angular.z = packet.heading_rate + heading = R.from_euler('x', packet.heading, degrees=False).as_quat() + + odom.pose.pose.orientation.x = heading[0] + odom.pose.pose.orientation.y = heading[1] + odom.pose.pose.orientation.z = heading[2] + odom.pose.pose.orientation.w = heading[3] + + speed = packet.velocity + # TODO: fix this + # why this works: autonsystem only cares about magnitude of velocity + # so setting an arbitrary axis to the speed and leave other at 0 + # works. However, this should be done properly ASAP + odom.twist.twist.linear.x = speed + self.odom_publisher.publish(odom) + except Exception as e: + self.get_logger().warn( + "Unable to convert other buggy position to lon lat" + str(e) + ) + elif isinstance(packet, NANDRawGPS): + odom = Odometry() + + # TODO: Not mock rolled accurately (Needs to be Fact Checked) + try: + lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") + odom.pose.pose.position.x = long + odom.pose.pose.position.y = lat + odom.twist.twist.angular.z = packet.heading_rate + heading = R.from_euler('x', packet.heading, degrees=False).as_quat() + + odom.pose.pose.orientation.x = heading[0] + odom.pose.pose.orientation.y = heading[1] + odom.pose.pose.orientation.z = heading[2] + odom.pose.pose.orientation.w = heading[3] + + speed = packet.velocity + # TODO: fix this + # why this works: autonsystem only cares about magnitude of velocity + # so setting an arbitrary axis to the speed and leave other at 0 + # works. However, this should be done properly ASAP + odom.twist.twist.linear.x = speed + self.odom_publisher.publish(odom) + except Exception as e: + self.get_logger().warn( + "Unable to convert other buggy position to lon lat" + str(e) + ) + elif isinstance(packet, Radio): + # Publish to odom topic x and y coord - odom = ROSOdom() + odom = Odometry() # convert to long lat try: lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") @@ -176,8 +277,62 @@ def reader_thread(self): "Unable to convert other buggy position to lon lat" + str(e) ) - elif isinstance(packet, BnyaTelemetry): - odom = ROSOdom() + + odom = Odometry() + + # TODO: Not mock rolled accurately (Needs to be Fact Checked) + try: + lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") + odom.pose.pose.position.x = long + odom.pose.pose.position.y = lat + odom.twist.twist.angular.z = packet.heading_rate + heading = R.from_euler('x', packet.heading, degrees=False).as_quat() + + odom.pose.pose.orientation.x = heading[0] + odom.pose.pose.orientation.y = heading[1] + odom.pose.pose.orientation.z = heading[2] + odom.pose.pose.orientation.w = heading[3] + + speed = packet.velocity + # TODO: fix this + # why this works: autonsystem only cares about magnitude of velocity + # so setting an arbitrary axis to the speed and leave other at 0 + # works. However, this should be done properly ASAP + odom.twist.twist.linear.x = speed + self.odom_publisher.publish(odom) + except Exception as e: + self.get_logger().warn( + "Unable to convert other buggy position to lon lat" + str(e) + ) + elif isinstance(packet, SCDebugInfo): + odom = Odometry() + + # TODO: Not mock rolled accurately (Needs to be Fact Checked) + try: + lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") + odom.pose.pose.position.x = long + odom.pose.pose.position.y = lat + odom.twist.twist.angular.z = packet.heading_rate + heading = R.from_euler('x', packet.heading, degrees=False).as_quat() + + odom.pose.pose.orientation.x = heading[0] + odom.pose.pose.orientation.y = heading[1] + odom.pose.pose.orientation.z = heading[2] + odom.pose.pose.orientation.w = heading[3] + + speed = packet.velocity + # TODO: fix this + # why this works: autonsystem only cares about magnitude of velocity + # so setting an arbitrary axis to the speed and leave other at 0 + # works. However, this should be done properly ASAP + odom.twist.twist.linear.x = speed + self.odom_publisher.publish(odom) + except Exception as e: + self.get_logger().warn( + "Unable to convert other buggy position to lon lat" + str(e) + ) + elif isinstance(packet, SCSensors): + odom = Odometry() # TODO: Not mock rolled accurately (Needs to be Fact Checked) try: @@ -204,18 +359,34 @@ def reader_thread(self): "Unable to convert other buggy position to lon lat" + str(e) ) - elif isinstance( - packet, tuple - ): # Are there any other packet that is a tuple - self.rc_steering_angle_publisher.publish(Float64(data=packet[0])) - self.steering_angle_publisher.publish(Float64(data=packet[1])) - self.battery_voltage_publisher.publish(Float64(data=packet[2])) - self.operator_ready_publisher.publish(Bool(data=packet[3])) - self.steering_alarm_publisher.publish(Bool(data=packet[4])) - self.brake_status_publisher.publish(Bool(data=packet[5])) - self.use_auton_steer_publisher.publish(Bool(data=packet[6])) - self.rc_uplink_qual_publisher.publish(UInt8(data=packet[7])) - self.nand_fix_publisher.publish(UInt8(data=packet[8])) + elif isinstance(packet, RoundtripTimestamp): + odom = Odometry() + + # TODO: Not mock rolled accurately (Needs to be Fact Checked) + try: + lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") + odom.pose.pose.position.x = long + odom.pose.pose.position.y = lat + odom.twist.twist.angular.z = packet.heading_rate + heading = R.from_euler('x', packet.heading, degrees=False).as_quat() + + odom.pose.pose.orientation.x = heading[0] + odom.pose.pose.orientation.y = heading[1] + odom.pose.pose.orientation.z = heading[2] + odom.pose.pose.orientation.w = heading[3] + + speed = packet.velocity + # TODO: fix this + # why this works: autonsystem only cares about magnitude of velocity + # so setting an arbitrary axis to the speed and leave other at 0 + # works. However, this should be done properly ASAP + odom.twist.twist.linear.x = speed + self.odom_publisher.publish(odom) + except Exception as e: + self.get_logger().warn( + "Unable to convert other buggy position to lon lat" + str(e) + ) + self.read_rate.sleep() From 0c924982099363b343321b74acbc90ed5b8dace8 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Tue, 24 Dec 2024 14:50:54 -0800 Subject: [PATCH 06/18] updated names for rostopics --- .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index f1d0628..c188132 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -39,9 +39,9 @@ def __init__(self, self_name, other_name, teensy_name): self.lock = Lock() self.create_subscription( - Float64, self_name + "/buggy/input/steering", self.set_steering, 1 + Float64, self_name + "/input/steering", self.set_steering, 1 ) - self.create_subscription(Int8, self_name + "/debug/sanity_warning", self.set_alarm, 1) + self.create_subscription(Int8, self_name + "/input/sanity_warning", self.set_alarm, 1) # upper bound of steering update rate, make sure auton sends slower than 500 Hz or update / 2ms self.steer_send_rate = self.create_rate(500) @@ -53,40 +53,40 @@ def __init__(self, self_name, other_name, teensy_name): # TODO: why are all the topics published under buggy/ ? # DEBUG MESSAGE PUBLISHERS: self.heading_rate_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/heading_rate", 1 + Float64, self_name + "/debug/heading_rate", 1 ) self.encoder_angle_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/encoder_angle", 1 + Float64, self_name + "/debug/encoder_angle", 1 ) self.rc_steering_angle_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/rc_steering_angle", 1 + Float64, self_name + "/debug/rc_steering_angle", 1 ) self.software_steering_angle_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/software_steering_angle", 1 + Float64, self_name + "/debug/software_steering_angle", 1 ) self.true_steering_angle_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/true_steering_angle", 1 + Float64, self_name + "/debug/true_steering_angle", 1 ) self.rfm69_timeout_num_publisher = self.create_publisher( - int, self_name + "/buggy/debug/rfm_timeout_num", 1 + int, self_name + "/debug/rfm_timeout_num", 1 ) self.operator_ready_publisher = self.create_publisher( - Bool, self_name + "/buggy/debug/operator_ready", 1 + Bool, self_name + "/debug/operator_ready", 1 ) self.brake_status_publisher = self.create_publisher( - Bool, self_name + "/buggy/debug/brake_status", 1 + Bool, self_name + "/debug/brake_status", 1 ) self.use_auton_steer_publisher = self.create_publisher( - Bool, self_name + "/buggy/debug/use_auton_steer", 1 + Bool, self_name + "/debug/use_auton_steer", 1 ) self.tx12_state_publisher = self.create_publisher( - Bool, self_name + "/buggy/debug/tx12_connected", 1 + Bool, self_name + "/debug/tx12_connected", 1 ) self.stepper_alarm_publisher = self.create_publisher( - UInt8, self_name + "/buggy/debug/steering_alarm", 1 + UInt8, self_name + "/debug/steering_alarm", 1 ) self.rc_uplink_qual_publisher = self.create_publisher( - UInt8, self_name + "/buggy/debug/rc_uplink_quality", 1 + UInt8, self_name + "/debug/rc_uplink_quality", 1 ) # NAND POSITION PUBLISHERS @@ -95,13 +95,13 @@ def __init__(self, self_name, other_name, teensy_name): Odometry, "NAND/nav/odom", 1 ) self.nand_gps_odom_publisher = self.create_publisher( - Odometry, "NAND/buggy/debug/gps_odom", 1 + Odometry, "NAND/debug/gps_odom", 1 ) self.observed_nand_odom_publisher = self.create_publisher( - Odometry, "SC/NAND_odom", 1 + Odometry, "SC/nav/NAND_odom", 1 ) self.nand_gps_fix_publisher = self.create_publisher( - UInt8, "NAND/buggy/debug/gps_fix", 1 + UInt8, "NAND/debug/gps_fix", 1 ) # SC SENSOR PUBLISHERS @@ -114,7 +114,7 @@ def __init__(self, self_name, other_name, teensy_name): # SERIAL DEBUG PUBLISHERS self.roundtrip_time_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/roundtrip_time", 1 + Float64, self_name + "/debug/roundtrip_time", 1 ) From 71ad951faba800b957d58b33d29b997f73e10253 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 25 Dec 2024 13:20:25 -0800 Subject: [PATCH 07/18] finshed adding publishers --- rb_ws/src/buggy/buggy/serial/host_comm.py | 4 +- .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 227 +++--------------- 2 files changed, 40 insertions(+), 191 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/host_comm.py b/rb_ws/src/buggy/buggy/serial/host_comm.py index 873dbb5..e503bd0 100644 --- a/rb_ws/src/buggy/buggy/serial/host_comm.py +++ b/rb_ws/src/buggy/buggy/serial/host_comm.py @@ -122,8 +122,8 @@ class NANDRawGPS: # TODO: make sure this is same as on firmware @dataclass class Radio: - nand_x_gps: float - nand_y_gps: float + nand_east_gps: float + nand_north_gps: float gps_seqnum: int nand_gps_fix: int # uint8 diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index c188132..6d35710 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -103,6 +103,9 @@ def __init__(self, self_name, other_name, teensy_name): self.nand_gps_fix_publisher = self.create_publisher( UInt8, "NAND/debug/gps_fix", 1 ) + self.nand_gps_acc_publisher = self.create_publisher( + Float64, "NAND/debug/gps_accuracy", 1 + ) # SC SENSOR PUBLISHERS self.sc_velocity_publisher = self.create_publisher( @@ -181,212 +184,58 @@ def reader_thread(self): elif isinstance(packet, NANDUKF): odom = Odometry() - # IMPORTANT TODO: all data here is in UTM even though *technically* Odometry uses lat/long - once buggystate is merged and exists, all these packets need to be changed to buggystate type - try: - lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") - odom.pose.pose.position.x = long - odom.pose.pose.position.y = lat - odom.twist.twist.angular.z = packet.heading_rate - heading = R.from_euler('x', packet.heading, degrees=False).as_quat() - - odom.pose.pose.orientation.x = heading[0] - odom.pose.pose.orientation.y = heading[1] - odom.pose.pose.orientation.z = heading[2] - odom.pose.pose.orientation.w = heading[3] - - speed = packet.velocity - # TODO: fix this - # why this works: autonsystem only cares about magnitude of velocity - # so setting an arbitrary axis to the speed and leave other at 0 - # works. However, this should be done properly ASAP - odom.twist.twist.linear.x = speed - self.odom_publisher.publish(odom) - except Exception as e: - self.get_logger().warn( - "Unable to convert other buggy position to lon lat" + str(e) - ) - + # NOTE: this data is published as an Odometry object per the BuggyState specs: https://github.com/CMU-Robotics-Club/robobuggy-software/wiki/BuggyState#buggystate-definition-as-a-navodom-ros-msg + odom.pose.pose.position.x = packet.easting + odom.pose.pose.position.y = packet.northing + odom.pose.pose.orientation.z = packet.theta + # why this works: autonsystem only cares about magnitude of velocity so setting an arbitrary axis to the speed and leave other at 0 works. + odom.twist.twist.linear.x = packet.velocity + odom.twist.twist.angular.z = packet.heading_rate - elif isinstance(packet, NANDUKF): - odom = Odometry() + self.nand_ukf_odom_publisher.publish(odom) - # TODO: Not mock rolled accurately (Needs to be Fact Checked) - try: - lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") - odom.pose.pose.position.x = long - odom.pose.pose.position.y = lat - odom.twist.twist.angular.z = packet.heading_rate - heading = R.from_euler('x', packet.heading, degrees=False).as_quat() - - odom.pose.pose.orientation.x = heading[0] - odom.pose.pose.orientation.y = heading[1] - odom.pose.pose.orientation.z = heading[2] - odom.pose.pose.orientation.w = heading[3] - - speed = packet.velocity - # TODO: fix this - # why this works: autonsystem only cares about magnitude of velocity - # so setting an arbitrary axis to the speed and leave other at 0 - # works. However, this should be done properly ASAP - odom.twist.twist.linear.x = speed - self.odom_publisher.publish(odom) - except Exception as e: - self.get_logger().warn( - "Unable to convert other buggy position to lon lat" + str(e) - ) elif isinstance(packet, NANDRawGPS): odom = Odometry() + # NOTE: this data is published as an Odometry object per the BuggyState specs: https://github.com/CMU-Robotics-Club/robobuggy-software/wiki/BuggyState#buggystate-definition-as-a-navodom-ros-msg + odom.pose.pose.position.x = packet.easting + odom.pose.pose.position.y = packet.northing + odom.pose.pose.orientation.z = 0 + odom.twist.twist.linear.x = 0 + odom.twist.twist.linear.y = 0 + odom.twist.twist.angular.z = 0 + + self.nand_gps_odom_publisher.publish(odom) + self.nand_gps_fix_publisher.publish(packet.gps_fix) + self.nand_gps_acc_publisher.publish(packet.accuracy) - # TODO: Not mock rolled accurately (Needs to be Fact Checked) - try: - lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") - odom.pose.pose.position.x = long - odom.pose.pose.position.y = lat - odom.twist.twist.angular.z = packet.heading_rate - heading = R.from_euler('x', packet.heading, degrees=False).as_quat() - - odom.pose.pose.orientation.x = heading[0] - odom.pose.pose.orientation.y = heading[1] - odom.pose.pose.orientation.z = heading[2] - odom.pose.pose.orientation.w = heading[3] - - speed = packet.velocity - # TODO: fix this - # why this works: autonsystem only cares about magnitude of velocity - # so setting an arbitrary axis to the speed and leave other at 0 - # works. However, this should be done properly ASAP - odom.twist.twist.linear.x = speed - self.odom_publisher.publish(odom) - except Exception as e: - self.get_logger().warn( - "Unable to convert other buggy position to lon lat" + str(e) - ) elif isinstance(packet, Radio): # Publish to odom topic x and y coord odom = Odometry() - # convert to long lat - try: - lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") - odom.pose.pose.position.x = long - odom.pose.pose.position.y = lat - self.odom_publisher.publish(odom) - except Exception as e: - self.get_logger().warn( - "Unable to convert other buggy position to lon lat" + str(e) - ) - - odom = Odometry() + odom.pose.pose.position.x = packet.nand_east_gps + odom.pose.pose.position.y = packet.nand_north_gps + self.observed_nand_odom_publisher.publish(odom) - # TODO: Not mock rolled accurately (Needs to be Fact Checked) - try: - lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") - odom.pose.pose.position.x = long - odom.pose.pose.position.y = lat - odom.twist.twist.angular.z = packet.heading_rate - heading = R.from_euler('x', packet.heading, degrees=False).as_quat() - - odom.pose.pose.orientation.x = heading[0] - odom.pose.pose.orientation.y = heading[1] - odom.pose.pose.orientation.z = heading[2] - odom.pose.pose.orientation.w = heading[3] - - speed = packet.velocity - # TODO: fix this - # why this works: autonsystem only cares about magnitude of velocity - # so setting an arbitrary axis to the speed and leave other at 0 - # works. However, this should be done properly ASAP - odom.twist.twist.linear.x = speed - self.odom_publisher.publish(odom) - except Exception as e: - self.get_logger().warn( - "Unable to convert other buggy position to lon lat" + str(e) - ) elif isinstance(packet, SCDebugInfo): - odom = Odometry() + self.encoder_angle_publisher.publish(packet.encoder_angle) + self.rc_steering_angle_publisher.publish(packet.rc_steering_angle) + self.software_steering_angle_publisher.publish(packet.software_steering_angle) + self.true_steering_angle_publisher.publish(packet.true_steering_angle) + self.operator_ready_publisher.publish(packet.operator_ready) + self.brake_status_publisher.publish(packet.brake_status) + self.use_auton_steer_publisher.publish(packet.auton_steer) + self.tx12_state_publisher.publish(packet.tx12_state) + self.stepper_alarm_publisher.publish(packet.stepper_alarm) + self.rc_uplink_qual_publisher.publish(packet.rc_uplink_quality) - # TODO: Not mock rolled accurately (Needs to be Fact Checked) - try: - lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") - odom.pose.pose.position.x = long - odom.pose.pose.position.y = lat - odom.twist.twist.angular.z = packet.heading_rate - heading = R.from_euler('x', packet.heading, degrees=False).as_quat() - - odom.pose.pose.orientation.x = heading[0] - odom.pose.pose.orientation.y = heading[1] - odom.pose.pose.orientation.z = heading[2] - odom.pose.pose.orientation.w = heading[3] - - speed = packet.velocity - # TODO: fix this - # why this works: autonsystem only cares about magnitude of velocity - # so setting an arbitrary axis to the speed and leave other at 0 - # works. However, this should be done properly ASAP - odom.twist.twist.linear.x = speed - self.odom_publisher.publish(odom) - except Exception as e: - self.get_logger().warn( - "Unable to convert other buggy position to lon lat" + str(e) - ) elif isinstance(packet, SCSensors): - odom = Odometry() - - # TODO: Not mock rolled accurately (Needs to be Fact Checked) - try: - lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") - odom.pose.pose.position.x = long - odom.pose.pose.position.y = lat - odom.twist.twist.angular.z = packet.heading_rate - heading = R.from_euler('x', packet.heading, degrees=False).as_quat() - - odom.pose.pose.orientation.x = heading[0] - odom.pose.pose.orientation.y = heading[1] - odom.pose.pose.orientation.z = heading[2] - odom.pose.pose.orientation.w = heading[3] - - speed = packet.velocity - # TODO: fix this - # why this works: autonsystem only cares about magnitude of velocity - # so setting an arbitrary axis to the speed and leave other at 0 - # works. However, this should be done properly ASAP - odom.twist.twist.linear.x = speed - self.odom_publisher.publish(odom) - except Exception as e: - self.get_logger().warn( - "Unable to convert other buggy position to lon lat" + str(e) - ) + self.sc_velocity_publisher.publish(packet.velocity) + self.sc_steering_angle_publisher.publish(packet.steering_angle) elif isinstance(packet, RoundtripTimestamp): - odom = Odometry() - - # TODO: Not mock rolled accurately (Needs to be Fact Checked) - try: - lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") - odom.pose.pose.position.x = long - odom.pose.pose.position.y = lat - odom.twist.twist.angular.z = packet.heading_rate - heading = R.from_euler('x', packet.heading, degrees=False).as_quat() - - odom.pose.pose.orientation.x = heading[0] - odom.pose.pose.orientation.y = heading[1] - odom.pose.pose.orientation.z = heading[2] - odom.pose.pose.orientation.w = heading[3] - - speed = packet.velocity - # TODO: fix this - # why this works: autonsystem only cares about magnitude of velocity - # so setting an arbitrary axis to the speed and leave other at 0 - # works. However, this should be done properly ASAP - odom.twist.twist.linear.x = speed - self.odom_publisher.publish(odom) - except Exception as e: - self.get_logger().warn( - "Unable to convert other buggy position to lon lat" + str(e) - ) - + self.roundtrip_time_publisher.publish(packet.returned_time) self.read_rate.sleep() From d9402ada148e9d582170bc5600f2ce7182a6563d Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 25 Dec 2024 13:23:00 -0800 Subject: [PATCH 08/18] comment cleanup --- rb_ws/src/buggy/buggy/serial/host_comm.py | 2 -- rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py | 4 ---- 2 files changed, 6 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/host_comm.py b/rb_ws/src/buggy/buggy/serial/host_comm.py index e503bd0..766247e 100644 --- a/rb_ws/src/buggy/buggy/serial/host_comm.py +++ b/rb_ws/src/buggy/buggy/serial/host_comm.py @@ -119,7 +119,6 @@ class NANDRawGPS: # 8 bits gps_fix: int # uint8 -# TODO: make sure this is same as on firmware @dataclass class Radio: nand_east_gps: float @@ -192,7 +191,6 @@ def send_steering(self, angle: float): def send_alarm(self, status: int): self.send_packet_raw(MSG_TYPE_ALARM, struct.pack(' Date: Wed, 25 Dec 2024 13:26:39 -0800 Subject: [PATCH 09/18] pylint --- rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index e44bb9c..740c7dd 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -1,16 +1,10 @@ import argparse from threading import Lock import threading - import rclpy from rclpy import Node -from scipy.spatial.transform import Rotation as R -import utm - from std_msgs.msg import Float64, Int8, UInt8, Bool - from host_comm import * - from nav_msgs.msg import Odometry class Translator(Node): From eacd5aabf946e68c2d3c9359ab06f00f96f239c2 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Mon, 6 Jan 2025 22:43:53 -0800 Subject: [PATCH 10/18] pylint --- rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index d57b92a..253956b 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -3,8 +3,6 @@ import threading import rclpy from rclpy import Node -from scipy.spatial.transform import Rotation -import utm from std_msgs.msg import Float64, Int8, UInt8, Bool from host_comm import * From 5d2eadbf8f378db56817ba547caa484ceef59f0a Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Tue, 7 Jan 2025 22:12:04 -0800 Subject: [PATCH 11/18] minor cleanup --- .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index 253956b..0952529 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -4,7 +4,7 @@ import rclpy from rclpy import Node -from std_msgs.msg import Float64, Int8, UInt8, Bool +from std_msgs.msg import Float64, Int8, Int32, UInt8, Bool from host_comm import * from nav_msgs.msg import Odometry @@ -15,19 +15,22 @@ class Translator(Node): be careful of multithreading synchronizaiton issues. """ - def __init__(self, self_name, other_name, teensy_name): + def __init__(self, teensy_name): """ - self_name: Buggy namespace - other_name: Only requried by SC for passing buggy namespace teensy_name: required for communication, different for SC and NAND Initializes the subscribers, rates, and ros topics (including debug topics) """ - # TODO: if we switch to using namespaces everywhere, use get_namespace instead of passing around self_name super().__init__('ROS_serial_translator') self.comms = Comms("/dev/" + teensy_name) + namespace = self.get_namespace() + if namespace == "\SC": + self.self_name = "SC" + else: + self.self_name = "NAND" + self.steer_angle = 0 self.alarm = 0 self.fresh_steer = False @@ -61,7 +64,7 @@ def __init__(self, self_name, other_name, teensy_name): Float64, self_name + "/debug/true_steering_angle", 1 ) self.rfm69_timeout_num_publisher = self.create_publisher( - int, self_name + "/debug/rfm_timeout_num", 1 + Int32, self_name + "/debug/rfm_timeout_num", 1 ) self.operator_ready_publisher = self.create_publisher( Bool, self_name + "/debug/operator_ready", 1 @@ -149,6 +152,8 @@ def writer_thread(self): with self.lock: self.comms.send_alarm(self.alarm) + with self.lock: + self.comms.send_timestamp(time.time()) self.steer_send_rate.sleep() @@ -226,7 +231,7 @@ def reader_thread(self): self.sc_steering_angle_publisher.publish(packet.steering_angle) elif isinstance(packet, RoundtripTimestamp): - self.roundtrip_time_publisher.publish(packet.returned_time) + self.roundtrip_time_publisher.publish(time.time() - packet.returned_time) self.read_rate.sleep() From 9ab7370baa18c3d2bdf431e937c23465fbda1c61 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 8 Jan 2025 23:09:03 -0800 Subject: [PATCH 12/18] published nand gps seqnum/time when recieved --- rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index 0952529..c634000 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -4,7 +4,7 @@ import rclpy from rclpy import Node -from std_msgs.msg import Float64, Int8, Int32, UInt8, Bool +from std_msgs.msg import Float64, Int8, Int32, UInt8, Bool, UInt64 from host_comm import * from nav_msgs.msg import Odometry @@ -101,6 +101,12 @@ def __init__(self, teensy_name): self.nand_gps_acc_publisher = self.create_publisher( Float64, "NAND/debug/gps_accuracy", 1 ) + self.nand_gps_seqnum_publisher = self.create_publisher( + Int32, "NAND/debug/gps_seqnum", 1 + ) + self.nand_gps_time_publisher = self.create_publisher( + UInt64, "NAND/debug/gps_time", 1 + ) # SC SENSOR PUBLISHERS self.sc_velocity_publisher = self.create_publisher( @@ -204,6 +210,8 @@ def reader_thread(self): self.nand_gps_odom_publisher.publish(odom) self.nand_gps_fix_publisher.publish(packet.gps_fix) self.nand_gps_acc_publisher.publish(packet.accuracy) + self.nand_gps_seqnum_publisher.publish(packet.gps_seqnum) + self.nand_gps_time_publisher.publish(packet.gps_time) elif isinstance(packet, Radio): From f4af527729d6b821f96b707242a357c514b8497d Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 8 Jan 2025 23:09:49 -0800 Subject: [PATCH 13/18] wrong slash --- rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index c634000..34daa60 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -26,7 +26,7 @@ def __init__(self, teensy_name): self.comms = Comms("/dev/" + teensy_name) namespace = self.get_namespace() - if namespace == "\SC": + if namespace == "/SC": self.self_name = "SC" else: self.self_name = "NAND" From 5d69c19309a37e22b592f25c179913f79e16d3c5 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 8 Jan 2025 23:25:40 -0800 Subject: [PATCH 14/18] added logging timestamps + other missing data --- rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index 34daa60..0525518 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -182,7 +182,7 @@ def reader_thread(self): self.tx12_state_publisher.publish(packet.tx12_state) self.stepper_alarm_publisher.publish(packet.stepper_alarm) self.rc_uplink_qual_publisher.publish(packet.rc_uplink_quality) - + self.get_logger().debug(f'NAND Debug Timestamp: {packet.timestamp}') elif isinstance(packet, NANDUKF): odom = Odometry() @@ -196,6 +196,8 @@ def reader_thread(self): odom.twist.twist.angular.z = packet.heading_rate self.nand_ukf_odom_publisher.publish(odom) + self.get_logger().debug(f'NAND UKF Timestamp: {packet.timestamp}') + elif isinstance(packet, NANDRawGPS): odom = Odometry() @@ -212,7 +214,10 @@ def reader_thread(self): self.nand_gps_acc_publisher.publish(packet.accuracy) self.nand_gps_seqnum_publisher.publish(packet.gps_seqnum) self.nand_gps_time_publisher.publish(packet.gps_time) + self.get_logger().debug(f'NAND Raw GPS Timestamp: {packet.timestamp}') + + # this packet is received on Short Circuit elif isinstance(packet, Radio): # Publish to odom topic x and y coord @@ -221,6 +226,9 @@ def reader_thread(self): odom.pose.pose.position.x = packet.nand_east_gps odom.pose.pose.position.y = packet.nand_north_gps self.observed_nand_odom_publisher.publish(odom) + self.get_logger().debug(f'Radio Received NAND GPS Seqnum: {packet.gps_seqnum}') + + elif isinstance(packet, SCDebugInfo): self.encoder_angle_publisher.publish(packet.encoder_angle) @@ -233,10 +241,14 @@ def reader_thread(self): self.tx12_state_publisher.publish(packet.tx12_state) self.stepper_alarm_publisher.publish(packet.stepper_alarm) self.rc_uplink_qual_publisher.publish(packet.rc_uplink_quality) + self.get_logger().debug(f'SC Debug Timestamp: {packet.timestamp}') + elif isinstance(packet, SCSensors): self.sc_velocity_publisher.publish(packet.velocity) self.sc_steering_angle_publisher.publish(packet.steering_angle) + self.get_logger().debug(f'SC Sensors Timestamp: {packet.timestamp}') + elif isinstance(packet, RoundtripTimestamp): self.roundtrip_time_publisher.publish(time.time() - packet.returned_time) From 36c7a97f7ed9ab35cfb79c01cabfc8cae8ccf8a3 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Thu, 9 Jan 2025 00:22:20 -0800 Subject: [PATCH 15/18] refactor buggy_state_converter to subscribe from the 3 seperate data sources --- .../src/buggy/buggy/buggy_state_converter.py | 54 +++++++------- .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 71 +++++++++---------- 2 files changed, 64 insertions(+), 61 deletions(-) diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index 48b1086..bbe5a5e 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -12,47 +12,51 @@ def __init__(self): super().__init__("buggy_state_converter") namespace = self.get_namespace() + if namespace == "/SC": + self.SC_raw_state_subscriber = self.create_subscription( + Odometry, "/raw_state", self.convert_SC_state_callback, 10 + ) - # Create publisher and subscriber for "self" namespace - self.self_raw_state_subscriber = self.create_subscription( - Odometry, "self/raw_state", self.self_raw_state_callback, 10 - ) - self.self_state_publisher = self.create_publisher(Odometry, "self/state", 10) + self.NAND_other_raw_state_subscriber = self.create_subscription( + Odometry, "/NAND_raw_state", self.convert_NAND_other_state_callback, 10 + ) - # Check if namespace is "SC" to add an "other" subscriber/publisher - if namespace == "/SC": - self.other_raw_state_subscriber = self.create_subscription( - Odometry, "other/raw_state", self.other_raw_state_callback, 10 + self.other_state_publisher = self.create_publisher(Odometry, "/other/state", 10) + + elif namespace == "/NAND": + self.NAND_raw_state_subscriber = self.create_subscription( + Odometry, "/raw_state", self.convert_NAND_state_callback, 10 ) - self.other_state_publisher = self.create_publisher(Odometry, "other/state", 10) + + else: + self.get_logger().warn(f"Namespace not recognized for buggy state conversion: {namespace}") + + self.self_state_publisher = self.create_publisher(Odometry, "/state", 10) # Initialize pyproj Transformer for ECEF -> UTM conversion for /SC self.ecef_to_utm_transformer = pyproj.Transformer.from_crs( "epsg:4978", "epsg:32617", always_xy=True ) # TODO: Confirm UTM EPSG code, using EPSG:32617 for UTM Zone 17N - def self_raw_state_callback(self, msg): - """ Callback for processing self/raw_state messages and publishing to self/state """ - namespace = self.get_namespace() - - if namespace == "/SC": - converted_msg = self.convert_SC_state(msg) - elif namespace == "/NAND": - converted_msg = self.convert_NAND_state(msg) - else: - self.get_logger().warn(f"Namespace not recognized for buggy state conversion: {namespace}") - return + def convert_SC_state_callback(self, msg): + """ Callback for processing SC/raw_state messages and publishing to self/state """ + converted_msg = self.convert_SC_state(msg) + self.self_state_publisher.publish(converted_msg) + def convert_NAND_state_callback(self, msg): + """ Callback for processing NAND/raw_state messages and publishing to self/state """ + converted_msg = self.convert_NAND_state(msg) self.self_state_publisher.publish(converted_msg) - def other_raw_state_callback(self, msg): - """ Callback for processing other/raw_state messages and publishing to other/state """ - # Convert the SC message and publish to other/state + + def convert_NAND_other_state_callback(self, msg): + """ Callback for processing SC/NAND_raw_state messages and publishing to other/state """ converted_msg = self.convert_NAND_other_state(msg) self.other_state_publisher.publish(converted_msg) + def convert_SC_state(self, msg): - """ + """ Converts self/raw_state in SC namespace to clean state units and structure Takes in ROS message in nav_msgs/Odometry format diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index 0525518..0a8c3ad 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -85,42 +85,46 @@ def __init__(self, teensy_name): UInt8, self_name + "/debug/rc_uplink_quality", 1 ) - # NAND POSITION PUBLISHERS - self.nand_ukf_odom_publisher = self.create_publisher( - Odometry, "NAND/nav/odom", 1 - ) - self.nand_gps_odom_publisher = self.create_publisher( - Odometry, "NAND/debug/gps_odom", 1 - ) - self.observed_nand_odom_publisher = self.create_publisher( - Odometry, "SC/nav/NAND_odom", 1 - ) - self.nand_gps_fix_publisher = self.create_publisher( - UInt8, "NAND/debug/gps_fix", 1 - ) - self.nand_gps_acc_publisher = self.create_publisher( - Float64, "NAND/debug/gps_accuracy", 1 - ) - self.nand_gps_seqnum_publisher = self.create_publisher( - Int32, "NAND/debug/gps_seqnum", 1 - ) - self.nand_gps_time_publisher = self.create_publisher( - UInt64, "NAND/debug/gps_time", 1 - ) - - # SC SENSOR PUBLISHERS - self.sc_velocity_publisher = self.create_publisher( - Float64, "SC/sensors/velocity", 1 - ) - self.sc_steering_angle_publisher = self.create_publisher( - Float64, "SC/sensors/steering_angle", 1 - ) - # SERIAL DEBUG PUBLISHERS self.roundtrip_time_publisher = self.create_publisher( Float64, self_name + "/debug/roundtrip_time", 1 ) + if self.self_name == "NAND": + # NAND POSITION PUBLISHERS + self.nand_ukf_odom_publisher = self.create_publisher( + Odometry, "/raw_state", 1 + ) + self.nand_gps_odom_publisher = self.create_publisher( + Odometry, "/debug/gps_odom", 1 + ) + + self.nand_gps_fix_publisher = self.create_publisher( + UInt8, "/debug/gps_fix", 1 + ) + self.nand_gps_acc_publisher = self.create_publisher( + Float64, "/debug/gps_accuracy", 1 + ) + self.nand_gps_seqnum_publisher = self.create_publisher( + Int32, "/debug/gps_seqnum", 1 + ) + self.nand_gps_time_publisher = self.create_publisher( + UInt64, "/debug/gps_time", 1 + ) + + if self.self_name == "SC": + # SC SENSOR PUBLISHERS + self.sc_velocity_publisher = self.create_publisher( + Float64, "SC/sensors/velocity", 1 + ) + self.sc_steering_angle_publisher = self.create_publisher( + Float64, "SC/sensors/steering_angle", 1 + ) + + self.observed_nand_odom_publisher = self.create_publisher( + Odometry, "/NAND_raw_state", 1 + ) + def set_alarm(self, msg): """ @@ -135,8 +139,6 @@ def set_steering(self, msg): Steering Angle Updater, updates the steering angle locally if updated on ros stopic """ self.get_logger().debug(f"Read steering angle of: {msg.data}") - # print("Steering angle: " + str(msg.data)) - # print("SET STEERING: " + str(msg.data)) with self.lock: self.steer_angle = msg.data self.fresh_steer = True @@ -185,8 +187,6 @@ def reader_thread(self): self.get_logger().debug(f'NAND Debug Timestamp: {packet.timestamp}') elif isinstance(packet, NANDUKF): odom = Odometry() - - # NOTE: this data is published as an Odometry object per the BuggyState specs: https://github.com/CMU-Robotics-Club/robobuggy-software/wiki/BuggyState#buggystate-definition-as-a-navodom-ros-msg odom.pose.pose.position.x = packet.easting odom.pose.pose.position.y = packet.northing odom.pose.pose.orientation.z = packet.theta @@ -201,7 +201,6 @@ def reader_thread(self): elif isinstance(packet, NANDRawGPS): odom = Odometry() - # NOTE: this data is published as an Odometry object per the BuggyState specs: https://github.com/CMU-Robotics-Club/robobuggy-software/wiki/BuggyState#buggystate-definition-as-a-navodom-ros-msg odom.pose.pose.position.x = packet.easting odom.pose.pose.position.y = packet.northing odom.pose.pose.orientation.z = 0 From 38bf0522f631a6b6cb149de75f4b690880ee2055 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Thu, 9 Jan 2025 00:25:26 -0800 Subject: [PATCH 16/18] confirmed buggy_state_converter is consistent with ros2bnya --- rb_ws/src/buggy/buggy/buggy_state_converter.py | 1 - rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py | 1 - 2 files changed, 2 deletions(-) diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index bbe5a5e..acc3d80 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -131,7 +131,6 @@ def convert_NAND_state(self, msg): converted_msg.twist.covariance = msg.twist.covariance # ---- 4. Linear Velocities in m/s ---- - # TODO: Check if scalar velocity is coming in from msg.twist.twist.linear.x # Convert scalar speed to velocity x/y components using heading (orientation.z) speed = msg.twist.twist.linear.x # m/s scalar velocity heading = msg.pose.pose.orientation.z # heading in radians diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index 0a8c3ad..1a130f8 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -191,7 +191,6 @@ def reader_thread(self): odom.pose.pose.position.y = packet.northing odom.pose.pose.orientation.z = packet.theta - # why this works: autonsystem only cares about magnitude of velocity so setting an arbitrary axis to the speed and leave other at 0 works. odom.twist.twist.linear.x = packet.velocity odom.twist.twist.angular.z = packet.heading_rate From d736ce9901618f9151a7f6fe4b2e108cf7ab6d01 Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Thu, 9 Jan 2025 09:56:30 -0800 Subject: [PATCH 17/18] fixing topic names --- .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index 1a130f8..292f5a2 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -37,9 +37,9 @@ def __init__(self, teensy_name): self.lock = Lock() self.create_subscription( - Float64, self_name + "/input/steering", self.set_steering, 1 + Float64, "/input/steering", self.set_steering, 1 ) - self.create_subscription(Int8, self_name + "/input/sanity_warning", self.set_alarm, 1) + self.create_subscription(Int8, "/input/sanity_warning", self.set_alarm, 1) # upper bound of steering update rate, make sure auton sends slower than 500 Hz or update / 2ms self.steer_send_rate = self.create_rate(500) @@ -49,45 +49,45 @@ def __init__(self, teensy_name): # DEBUG MESSAGE PUBLISHERS: self.heading_rate_publisher = self.create_publisher( - Float64, self_name + "/debug/heading_rate", 1 + Float64, "/debug/heading_rate", 1 ) self.encoder_angle_publisher = self.create_publisher( - Float64, self_name + "/debug/encoder_angle", 1 + Float64, "/debug/encoder_angle", 1 ) self.rc_steering_angle_publisher = self.create_publisher( - Float64, self_name + "/debug/rc_steering_angle", 1 + Float64, "/debug/rc_steering_angle", 1 ) self.software_steering_angle_publisher = self.create_publisher( - Float64, self_name + "/debug/software_steering_angle", 1 + Float64, "/debug/software_steering_angle", 1 ) self.true_steering_angle_publisher = self.create_publisher( - Float64, self_name + "/debug/true_steering_angle", 1 + Float64, "/debug/true_steering_angle", 1 ) self.rfm69_timeout_num_publisher = self.create_publisher( - Int32, self_name + "/debug/rfm_timeout_num", 1 + Int32, "/debug/rfm_timeout_num", 1 ) self.operator_ready_publisher = self.create_publisher( - Bool, self_name + "/debug/operator_ready", 1 + Bool, "/debug/operator_ready", 1 ) self.brake_status_publisher = self.create_publisher( - Bool, self_name + "/debug/brake_status", 1 + Bool, "/debug/brake_status", 1 ) self.use_auton_steer_publisher = self.create_publisher( - Bool, self_name + "/debug/use_auton_steer", 1 + Bool, "/debug/use_auton_steer", 1 ) self.tx12_state_publisher = self.create_publisher( - Bool, self_name + "/debug/tx12_connected", 1 + Bool, "/debug/tx12_connected", 1 ) self.stepper_alarm_publisher = self.create_publisher( - UInt8, self_name + "/debug/steering_alarm", 1 + UInt8, "/debug/steering_alarm", 1 ) self.rc_uplink_qual_publisher = self.create_publisher( - UInt8, self_name + "/debug/rc_uplink_quality", 1 + UInt8, "/debug/rc_uplink_quality", 1 ) # SERIAL DEBUG PUBLISHERS self.roundtrip_time_publisher = self.create_publisher( - Float64, self_name + "/debug/roundtrip_time", 1 + Float64, "/debug/roundtrip_time", 1 ) if self.self_name == "NAND": @@ -115,10 +115,10 @@ def __init__(self, teensy_name): if self.self_name == "SC": # SC SENSOR PUBLISHERS self.sc_velocity_publisher = self.create_publisher( - Float64, "SC/sensors/velocity", 1 + Float64, "/sensors/velocity", 1 ) self.sc_steering_angle_publisher = self.create_publisher( - Float64, "SC/sensors/steering_angle", 1 + Float64, "/sensors/steering_angle", 1 ) self.observed_nand_odom_publisher = self.create_publisher( From 94a1139c1ee51938951ef081f6942d6603e9928c Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Thu, 9 Jan 2025 09:59:18 -0800 Subject: [PATCH 18/18] published nand gps seqnum --- rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index 292f5a2..974ef87 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -84,6 +84,9 @@ def __init__(self, teensy_name): self.rc_uplink_qual_publisher = self.create_publisher( UInt8, "/debug/rc_uplink_quality", 1 ) + self.nand_gps_seqnum_publisher = self.create_publisher( + Int32, "/debug/NAND_gps_seqnum", 1 + ) # SERIAL DEBUG PUBLISHERS self.roundtrip_time_publisher = self.create_publisher( @@ -105,14 +108,13 @@ def __init__(self, teensy_name): self.nand_gps_acc_publisher = self.create_publisher( Float64, "/debug/gps_accuracy", 1 ) - self.nand_gps_seqnum_publisher = self.create_publisher( - Int32, "/debug/gps_seqnum", 1 - ) + self.nand_gps_time_publisher = self.create_publisher( UInt64, "/debug/gps_time", 1 ) if self.self_name == "SC": + # SC SENSOR PUBLISHERS self.sc_velocity_publisher = self.create_publisher( Float64, "/sensors/velocity", 1 @@ -121,6 +123,7 @@ def __init__(self, teensy_name): Float64, "/sensors/steering_angle", 1 ) + # RADIO DATA PUBLISHER self.observed_nand_odom_publisher = self.create_publisher( Odometry, "/NAND_raw_state", 1 ) @@ -224,7 +227,8 @@ def reader_thread(self): odom.pose.pose.position.x = packet.nand_east_gps odom.pose.pose.position.y = packet.nand_north_gps self.observed_nand_odom_publisher.publish(odom) - self.get_logger().debug(f'Radio Received NAND GPS Seqnum: {packet.gps_seqnum}') + self.nand_gps_seqnum_publisher.publish(packet.gps_seqnum) +