diff --git a/python-requirements.txt b/python-requirements.txt index 46d0064..d75cc06 100644 --- a/python-requirements.txt +++ b/python-requirements.txt @@ -6,6 +6,7 @@ osqp pandas pymap3d pyproj +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() diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 361ae4b..efaf6db 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -1,5 +1,6 @@ #! /usr/bin/env python3 import threading +import sys import rclpy from rclpy.node import Node from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance @@ -8,16 +9,11 @@ from nav_msgs.msg import Odometry import numpy as np import utm +sys.path.append("/rb_ws/src/buggy/buggy") +from util import Constants class Simulator(Node): - # simulator constants: - UTM_EAST_ZERO = 589702.87 - UTM_NORTH_ZERO = 4477172.947 - UTM_ZONE_NUM = 17 - UTM_ZONE_LETTER = "T" - #TODO: make these values accurate - WHEELBASE_SC = 1.17 - WHEELBASE_NAND= 1.17 + def __init__(self): super().__init__('sim_single') @@ -25,8 +21,8 @@ def __init__(self): self.starting_poses = { - "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 0, Simulator.UTM_NORTH_ZERO + 0, -110), - "Hill1_SC": (Simulator.UTM_EAST_ZERO + 20, Simulator.UTM_NORTH_ZERO + 30, -110), + "Hill1_NAND": (Constants.UTM_EAST_ZERO + 0, Constants.UTM_NORTH_ZERO + 0, -110), + "Hill1_SC": (Constants.UTM_EAST_ZERO + 20, Constants.UTM_NORTH_ZERO + 30, -110), "WESTINGHOUSE": (589647, 4477143, -150), "UC_TO_PURNELL": (589635, 4477468, 160), "UC": (589681, 4477457, 160), @@ -39,57 +35,53 @@ def __init__(self): "RACELINE_PASS": (589468.02, 4476993.07, -160), } - self.number_publisher = self.create_publisher(Float64, 'numbers', 1) - self.i = 0 - - # for X11 matplotlib (direction included) - self.plot_publisher = self.create_publisher(Pose, "sim_2d/utm", 1) - - # simulate the INS's outputs (noise included) - self.pose_publisher = self.create_publisher(Odometry, "nav/odom", 1) - - self.steering_subscriber = self.create_subscription( - Float64, "buggy/input/steering", self.update_steering_angle, 1 - ) - # To read from velocity - self.velocity_subscriber = self.create_subscription( - Float64, "velocity", self.update_velocity, 1 - ) - self.navsatfix_noisy_publisher = self.create_publisher( - NavSatFix, "state/pose_navsat_noisy", 1 - ) - - - - - self.declare_parameter('velocity', 12) if (self.get_namespace() == "/SC"): self.buggy_name = "SC" self.declare_parameter('pose', "Hill1_SC") - self.wheelbase = Simulator.WHEELBASE_SC + self.wheelbase = Constants.WHEELBASE_SC if (self.get_namespace() == "/NAND"): self.buggy_name = "NAND" self.declare_parameter('pose', "Hill1_NAND") - self.wheelbase = Simulator.WHEELBASE_NAND + self.wheelbase = Constants.WHEELBASE_NAND self.velocity = self.get_parameter("velocity").value init_pose_name = self.get_parameter("pose").value + self.init_pose = self.starting_poses[init_pose_name] self.e_utm, self.n_utm, self.heading = self.init_pose self.steering_angle = 0 # degrees - self.rate = 200 # Hz - self.pub_skip = 2 # publish every pub_skip ticks - self.pub_tick_count = 0 + self.rate = 100 # Hz + self.tick_count = 0 + self.interval = 2 # how frequently to publish self.lock = threading.Lock() - freq = 10 - timer_period = 1/freq # seconds + timer_period = 1/self.rate # seconds self.timer = self.create_timer(timer_period, self.loop) + self.steering_subscriber = self.create_subscription( + Float64, "buggy/input/steering", self.update_steering_angle, 1 + ) + + # To read from velocity + self.velocity_subscriber = self.create_subscription( + Float64, "velocity", self.update_velocity, 1 + ) + + # for X11 matplotlib (direction included) + self.plot_publisher = self.create_publisher(Pose, "sim_2d/utm", 1) + + # simulate the INS's outputs (noise included) + # this is published as a BuggyState (UTM and radians) + self.pose_publisher = self.create_publisher(Odometry, "nav/odom", 1) + + self.navsatfix_noisy_publisher = self.create_publisher( + NavSatFix, "state/pose_navsat_noisy", 1 + ) + def update_steering_angle(self, data: Float64): with self.lock: self.steering_angle = data.data @@ -98,14 +90,6 @@ def update_velocity(self, data: Float64): with self.lock: self.velocity = data.data - def get_steering_arc(self): - with self.lock: - steering_angle = self.steering_angle - if steering_angle == 0.0: - return np.inf - - return Simulator.WHEELBASE / np.tan(np.deg2rad(steering_angle)) - def dynamics(self, state, v): l = self.wheelbase _, _, theta, delta = state @@ -154,18 +138,13 @@ def publish(self): (lat, long) = utm.to_latlon( p.position.x, p.position.y, - Simulator.UTM_ZONE_NUM, - Simulator.UTM_ZONE_LETTER, + Constants.UTM_ZONE_NUM, + Constants.UTM_ZONE_LETTER, ) - nsf = NavSatFix() - nsf.latitude = lat - nsf.longitude = long - nsf.altitude = float(260) - nsf.header.stamp = time_stamp - lat_noisy = lat + np.random.normal(0, 1e-6) long_noisy = long + np.random.normal(0, 1e-6) + nsf_noisy = NavSatFix() nsf_noisy.latitude = lat_noisy nsf_noisy.longitude = long_noisy @@ -176,13 +155,15 @@ def publish(self): odom.header.stamp = time_stamp odom_pose = Pose() - odom_pose.position.x = float(long) - odom_pose.position.y = float(lat) + east, north = utm.from_latlon(lat_noisy, long_noisy) + odom_pose.position.x = float(east) + odom_pose.position.y = float(north) odom_pose.position.z = float(260) odom_pose.orientation.z = np.sin(np.deg2rad(self.heading) / 2) odom_pose.orientation.w = np.cos(np.deg2rad(self.heading) / 2) + # NOTE: autonsystem only cares about magnitude of velocity, so we don't need to split into components odom_twist = Twist() odom_twist.linear.x = float(velocity) @@ -192,20 +173,10 @@ def publish(self): self.pose_publisher.publish(odom) def loop(self): - msg = Float64() - msg.data = float(self.i) - - self.number_publisher.publish(msg) - self.i += 1 - self.step() - if self.pub_tick_count == self.pub_skip: + if self.tick_count % self.interval == 0: self.publish() - self.pub_tick_count = 0 - else: - self.pub_tick_count += 1 - - + self.tick_count += 1 def main(args=None): diff --git a/rb_ws/src/buggy/buggy/util.py b/rb_ws/src/buggy/buggy/util.py new file mode 100644 index 0000000..0bdfc8e --- /dev/null +++ b/rb_ws/src/buggy/buggy/util.py @@ -0,0 +1,8 @@ +class Constants: + UTM_EAST_ZERO = 589702.87 + UTM_NORTH_ZERO = 4477172.947 + UTM_ZONE_NUM = 17 + UTM_ZONE_LETTER = "T" + #TODO: make these values accurate + WHEELBASE_SC = 1.17 + WHEELBASE_NAND= 1.17 \ No newline at end of file