diff --git a/python-requirements.txt b/python-requirements.txt index 68b0c6f..66b300e 100644 --- a/python-requirements.txt +++ b/python-requirements.txt @@ -5,6 +5,7 @@ numpy osqp pandas pymap3d +pyserial scipy setuptools==58.2.0 trimesh diff --git a/rb_ws/src/buggy/buggy/serial/host_comm.py b/rb_ws/src/buggy/buggy/serial/host_comm.py new file mode 100644 index 0000000..e98c23e --- /dev/null +++ b/rb_ws/src/buggy/buggy/serial/host_comm.py @@ -0,0 +1,218 @@ +import struct +import time +from dataclasses import dataclass + +from serial import Serial + +class Crc16: + def __init__(self): + self.accum = 0 + + def update(self, b): + if isinstance(b, int): + assert(0 <= b < 256) + + crc_table = [ + 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, + 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, + 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072, + 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041, + 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2, + 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, + 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, + 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, + 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192, + 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1, + 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1, + 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2, + 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, + 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, + 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, + 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101, + 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312, + 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321, + 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371, + 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, + 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, + 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, + 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2, + 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381, + 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291, + 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2, + 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, + 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, + 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, + 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261, + 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231, + 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202 + ] + + i = ((self.accum >> 8) ^ b) & 0xFF; + i %= 256 + self.accum = ((self.accum << 8) & 0xFF00) ^ crc_table[i]; + else: + for one_byte in b: + self.update(one_byte) + +SYNC_WORD = b'\xAA\xFF\x00\x55' + +MAX_PAYLOAD_LEN = 100 + +MSG_TYPE_DEBUG = b'DB' +MSG_TYPE_ODOMETRY = b'OD' +MSG_TYPE_STEERING = b'ST' +MSG_TYPE_BRAKE = b'BR' +MSG_TYPE_ALARM = b'AL' +MSG_TYPE_BNYATEL = b'BT' + +@dataclass +class Odometry: + x: float + y: float + radio_seqnum: int + gps_seqnum: 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 IncompletePacket(Exception): + pass + +class ChecksumMismatch(Exception): + pass + +class Comms: + def __init__(self, path_to_port): + self.port = Serial(path_to_port, 1_000_000) + self.rx_buffer = b'' + + def send_packet_raw(self, msg_type: bytes, payload: bytes): + assert(len(msg_type) == 2) + assert(len(payload) < MAX_PAYLOAD_LEN) + + checksum = Crc16() + def write_and_checksum(data: bytes): + nonlocal checksum + + self.port.write(data) + checksum.update(data) + + self.port.write(SYNC_WORD) + write_and_checksum(msg_type) + write_and_checksum(len(payload).to_bytes(2, 'little')) + write_and_checksum(payload) + self.port.write(checksum.accum.to_bytes(2, 'little')) + + def send_steering(self, angle: float): + self.send_packet_raw(MSG_TYPE_STEERING, struct.pack(' len(self.rx_buffer): + raise IncompletePacket() + + data = self.rx_buffer[index:][:count] + index += count + return data + + def read_and_checksum(count: int): + nonlocal checksum + + data = read(count) + checksum.update(data) + return data + + if read(1) != SYNC_WORD[0:1]: + self.rx_buffer = self.rx_buffer[1:] + continue + if read(1) != SYNC_WORD[1:2]: + self.rx_buffer = self.rx_buffer[1:] + continue + if read(1) != SYNC_WORD[2:3]: + self.rx_buffer = self.rx_buffer[1:] + continue + if read(1) != SYNC_WORD[3:4]: + self.rx_buffer = self.rx_buffer[1:] + continue + + msg_type = read_and_checksum(2) + msg_len = int.from_bytes(read_and_checksum(2), 'little') + + if msg_len > MAX_PAYLOAD_LEN: + self.rx_buffer = self.rx_buffer[1:] + continue + + payload = read_and_checksum(msg_len) + + rx_crc = int.from_bytes(read(2), 'little') + + self.rx_buffer = self.rx_buffer[4 + 3 + 2 + msg_len + 2:] + + if rx_crc != checksum.accum: + raise ChecksumMismatch() + + return (msg_type, payload) + + def read_packet(self): + packet = self.read_packet_raw() + if packet is None: + return None + + msg_type, payload = packet + if msg_type == MSG_TYPE_ODOMETRY: + # Odometry message + x, y, radio_seqnum, gps_seqnum = struct.unpack(' 0.01: + print(packet) + last_time = time.time() + comms.send_steering(1234.5) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py new file mode 100644 index 0000000..ca4bbea --- /dev/null +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -0,0 +1,255 @@ +import argparse +from threading import Lock +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 nav_msgs.msg import Odometry as ROSOdom + +class Translator(Node): + """ + Translates the output from bnyahaj serial (interpreted from host_comm) to ros topics and vice versa. + Performs reading (from Bnya Serial) and writing (from Ros Topics) on different python threads, so + be careful of multithreading synchronizaiton issues. + SC: + (ROS) Self Steering topic --> (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): + """ + 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) + """ + super().__init__('ROS_serial_translator') + + self.comms = Comms("/dev/" + teensy_name) + self.steer_angle = 0 + self.alarm = 0 + self.fresh_steer = False + self.lock = Lock() + + 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) + + # upper bound of reading data from Bnyahaj Serial, at 1ms + self.read_rate = self.create_rate(1000) + + # ISSUE: https://github.com/CMU-Robotics-Club/RoboBuggy2/issues/90 + 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.battery_voltage_publisher = self.create_publisher( + Float64, self_name + "/buggy/debug/battery_voltage", 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.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 + ) + + def set_alarm(self, msg): + """ + alarm ros topic reader, locked so that only one of the setters runs at once + """ + with self.lock: + self.get_logger().debug(f"Reading alarm of {msg.data}") + self.alarm = msg.data + + 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}") + # 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!") + + while rclpy.ok(): + if self.fresh_steer: + with self.lock: + self.comms.send_steering(self.steer_angle) + self.get_logger().debug(f"Sent steering angle of: {self.steer_angle}") + self.fresh_steer = False + + with self.lock: + self.comms.send_alarm(self.alarm) + + 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() + self.get_logger().debug("packet" + str(packet)) + + if isinstance(packet, Odometry): + # Publish to odom topic x and y coord + odom = ROSOdom() + # convert to long lat + try: + # TODO check if the order of y and x here is accurate + lat, long = utm.to_latlon(packet.y, packet.x, 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) + ) + + elif isinstance(packet, BnyaTelemetry): + odom = ROSOdom() + + # 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 = Rotation.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, 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])) + + self.read_rate.sleep() + + def loop(self): + """ + Initialies the reader and writer thread, should theoretically never finish as there are while loops + """ + p1 = threading.Thread(target=self.writer_thread) + p2 = threading.Thread(target=self.reader_thread) + + p1.start() + p2.start() + + p1.join() + p2.join() + + +# Initializes ros nodes, using self and other name +# other name is not requires, and if not submitted, use NONE +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "--self_name", type=str, help="name of ego-buggy", required=True + ) + parser.add_argument( + "--other_name", + type=str, + help="name of other buggy", + required=False, + default=None, + ) + parser.add_argument( + "--teensy_name", type=str, help="name of teensy port", required=True + ) + args, _ = parser.parse_known_args() + self_name = args.self_name + other_name = args.other_name + teensy_name = args.teensy_name + + rclpy.init() + + translate = Translator(self_name, other_name, teensy_name) + + if self_name == "SC" and other_name is None: + translate.get_logger().warn( + "Not reading NAND Odometry messages, double check roslaunch files for ros_to_bnyahaj" + ) + elif other_name is None: + translate.get_logger().info("No other name passed in, presuming that this is NAND ") + + rclpy.spin(translate) + + rclpy.shutdown()