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/auton/pose.py b/rb_ws/src/buggy/buggy/auton/pose.py new file mode 100644 index 0000000..b631ca6 --- /dev/null +++ b/rb_ws/src/buggy/buggy/auton/pose.py @@ -0,0 +1,260 @@ +import math + +import numpy as np + +from geometry_msgs.msg import Pose as ROSPose + + +class Pose: + """ + A data structure for storing 2D poses, as well as a set of + convenience methods for transforming/manipulating poses + + TODO: All Pose objects are with respect to World coordinates. + """ + + __x = None + __y = None + __theta = None + + # https://gist.github.com/salmagro/2e698ad4fbf9dae40244769c5ab74434 + @staticmethod + def euler_from_quaternion(x, y, z, w): + """ + Converts quaternion (w in last place) to euler roll, pitch, yaw + quaternion = [x, y, z, w] + """ + + sinr_cosp = 2 * (w * x + y * z) + cosr_cosp = 1 - 2 * (x * x + y * y) + roll = np.arctan2(sinr_cosp, cosr_cosp) + + sinp = 2 * (w * y - z * x) + pitch = np.arcsin(sinp) + + siny_cosp = 2 * (w * z + x * y) + cosy_cosp = 1 - 2 * (y * y + z * z) + yaw = np.arctan2(siny_cosp, cosy_cosp) + + return roll, pitch, yaw + + @staticmethod + def quaternion_from_euler(roll, pitch, yaw): + """ + Converts euler roll, pitch, yaw to quaternion (w in last place) + quat = [x, y, z, w] + Bellow should be replaced when porting for ROS 2 Python tf_conversions is done. + """ + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + q = [0] * 4 + q[0] = cy * cp * cr + sy * sp * sr + q[1] = cy * cp * sr - sy * sp * cr + q[2] = sy * cp * sr + cy * sp * cr + q[3] = sy * cp * cr - cy * sp * sr + + return q + + @staticmethod + def rospose_to_pose(rospose: ROSPose): + """ + Converts a geometry_msgs/Pose to a pose3d/Pose + + Args: + posestamped (geometry_msgs/Pose): pose to convert + + Returns: + Pose: converted pose + """ + (_, _, yaw) = Pose.euler_from_quaternion( + rospose.orientation.x, + rospose.orientation.y, + rospose.orientation.z, + rospose.orientation.w, + ) + + p = Pose(rospose.position.x, rospose.position.y, yaw) + return p + + def heading_to_quaternion(heading): + q = Pose.quaternion_from_euler(0, 0, heading) + return q + + def __init__(self, x, y, theta): + self.x = x + self.y = y + self.theta = theta + + def __repr__(self) -> str: + return f"Pose(x={self.x}, y={self.y}, theta={self.theta})" + + def copy(self): + return Pose(self.x, self.y, self.theta) + + @property + def x(self): + return self.__x + + @x.setter + def x(self, x): + self.__x = x + + @property + def y(self): + return self.__y + + @y.setter + def y(self, y): + self.__y = y + + @property + def theta(self): + if self.__theta > np.pi or self.__theta < -np.pi: + raise ValueError("Theta is not in [-pi, pi]") + return self.__theta + + @theta.setter + def theta(self, theta): + # normalize theta to [-pi, pi] + self.__theta = np.arctan2(np.sin(theta), np.cos(theta)) + + def to_mat(self): + """ + Returns the pose as a 3x3 homogeneous transformation matrix + """ + return np.array( + [ + [np.cos(self.theta), -np.sin(self.theta), self.x], + [np.sin(self.theta), np.cos(self.theta), self.y], + [0, 0, 1], + ] + ) + + @staticmethod + def from_mat(mat): + """ + Creates a pose from a 3x3 homogeneous transformation matrix + """ + return Pose(mat[0, 2], mat[1, 2], np.arctan2(mat[1, 0], mat[0, 0])) + + def invert(self): + """ + Inverts the pose + """ + return Pose.from_mat(np.linalg.inv(self.to_mat())) + + def convert_pose_from_global_to_local_frame(self, pose): + """ + Converts the inputted pose from the global frame to the local frame + (relative to the current pose) + """ + return pose / self + + def convert_pose_from_local_to_global_frame(self, pose): + """ + Converts the inputted pose from the local frame to the global frame + (relative to the current pose) + """ + return pose * self + + def convert_point_from_global_to_local_frame(self, point): + """ + Converts the inputted point from the global frame to the local frame + (relative to the current pose) + """ + point_hg = np.array([point[0], point[1], 1]) + point_hg_local = np.linalg.inv(self.to_mat()) @ point_hg + return ( + point_hg_local[0] / point_hg_local[2], + point_hg_local[1] / point_hg_local[2], + ) + + def convert_point_from_local_to_global_frame(self, point): + """ + Converts the inputted point from the local frame to the global frame + (relative to the current pose) + """ + point_hg = np.array([point[0], point[1], 1]) + point_hg_global = self.to_mat() @ point_hg + return ( + point_hg_global[0] / point_hg_global[2], + point_hg_global[1] / point_hg_global[2], + ) + + def convert_point_array_from_global_to_local_frame(self, points): + """ + Converts the inputted point array from the global frame to the local frame + (relative to the current pose) + + Args: + points (np.ndarray) [Size: (N,2)]: array of points to convert + """ + points_hg = np.array([points[:, 0], points[:, 1], np.ones(len(points))]) + points_hg_local = np.linalg.inv(self.to_mat()) @ points_hg.T + return (points_hg_local[:2, :] / points_hg_local[2, :]).T + + def convert_point_array_from_local_to_global_frame(self, points): + """ + Converts the inputted point array from the local frame to the global frame + (relative to the current pose) + + Args: + points (np.ndarray) [Size: (N,2)]: array of points to convert + """ + points_hg = np.array([points[:, 0], points[:, 1], np.ones(len(points))]) + points_hg_global = self.to_mat() @ points_hg.T + return (points_hg_global[:2, :] / points_hg_global[2, :]).T + + def rotate(self, angle): + """ + Rotates the pose by the given angle + """ + self.theta += angle + + def translate(self, x, y): + """ + Translates the pose by the given x and y distances + """ + self.x += x + self.y += y + + def __add__(self, other): + return Pose(self.x + other.x, self.y + other.y, self.theta + other.theta) + + def __sub__(self, other): + return Pose(self.x - other.x, self.y - other.y, self.theta - other.theta) + + def __neg__(self): + return Pose(-self.x, -self.y, -self.theta) + + def __mul__(self, other): + p1_mat = self.to_mat() + p2_mat = other.to_mat() + + return Pose.from_mat(p1_mat @ p2_mat) + + def __truediv__(self, other): + p1_mat = self.to_mat() + p2_mat = other.to_mat() + + return Pose.from_mat(np.linalg.inv(p2_mat) @ p1_mat) + + +if __name__ == "__main__": + # TODO: again do we want example code in these classes + rospose = ROSPose() + rospose.position.x = 1 + rospose.position.y = 2 + rospose.position.z = 3 + rospose.orientation.x = 0 + rospose.orientation.y = 0 + rospose.orientation.z = -0.061461 + rospose.orientation.w = 0.9981095 + + pose = Pose.rospose_to_pose(rospose) + print(pose) # Pose(x=1, y=2, theta=-0.123) \ No newline at end of file diff --git a/rb_ws/src/buggy/buggy/auton/world.py b/rb_ws/src/buggy/buggy/auton/world.py new file mode 100644 index 0000000..0558688 --- /dev/null +++ b/rb_ws/src/buggy/buggy/auton/world.py @@ -0,0 +1,226 @@ +import utm +import numpy as np + +from pose import Pose + + +class World: + """Abstraction for the world coordinate system + + The real world uses GPS coordinates, aka latitude and longitude. However, + using lat/lon is bad for path planning for several reasons. First, the difference + in numbers would be very tiny for small distances, alluding to roundoff errors. + Additionally, lat/lon follows a North-East-Down coordinate system, with headings + in the clockwise direction. We want to use an East-North-Up coordinate system, so + that the heading is in the counter-clockwise direction. + + We do this by converting GPS coordinates to UTM coordinates, which are in meters. + UTM works by dividing the world into a grid, where each grid cell has a unique + identifier. A UTM coordinate consists of the grid cell identifier and the "easting" + and "northing" within that grid cell. The easting (x) and northing (y) are in meters, + and are relative to the southwest corner of the grid cell. + + Last, we offset the UTM coordinates to be relative to some arbitrary zero point. That + way, the final world coordinates are relatively close to zero, which makes debugging + easier. + + This class provides methods to convert between GPS and world coordinates. There is + a version for single coordinates and a version for numpy arrays. + """ + + # Geolocates to around the southwest corner of Phipps + WORLD_EAST_ZERO = 589106 + WORLD_NORTH_ZERO = 4476929 + + @staticmethod + def gps_to_world(lat, lon): + """Converts GPS coordinates to world coordinates + + Args: + lat (float): latitude + lon (float): longitude + + Returns: + tuple: (x, y) in meters from some arbitrary zero point + """ + utm_coords = utm.from_latlon(lat, lon) + x = utm_coords[0] - World.WORLD_EAST_ZERO + y = utm_coords[1] - World.WORLD_NORTH_ZERO + + return x, y + + @staticmethod + def utm_to_world_pose(pose: Pose) -> Pose: + """Converts UTM coordinates to world coordinates + + Args: + pose (Pose): pose with utm coordinates + + Returns: + Pose: pose with world coordinates + """ + + utm_e = pose.x + utm_n = pose.y + x = utm_e - World.WORLD_EAST_ZERO + y = utm_n - World.WORLD_NORTH_ZERO + return Pose(x, y, pose.theta) + + @staticmethod + def world_to_utm_pose(pose: Pose) -> Pose: + """Converts world coordinates to utm coordinates + + Args: + pose (Pose): pose with world coordinates + + Returns: + Pose: pose with world coordinates + """ + + utm_e = pose.x + World.WORLD_EAST_ZERO + utm_n = pose.y + World.WORLD_NORTH_ZERO + return Pose(utm_e, utm_n, pose.theta) + + @staticmethod + def world_to_utm_numpy(coords): + """Converts world coordinates to utm coordinates + + Args: + coords (numpy.ndarray [size: (N,2)]): array of x, y pairs + + Returns: + numpy.ndarray [size: (N,2)]: array of utm_e, utm_n pairs + """ + + N = np.shape(coords)[0] + utm_offset_e = np.ones((N, )) * World.WORLD_EAST_ZERO + utm_offset_n = np.ones((N, )) * World.WORLD_NORTH_ZERO + utm_offset = np.stack((utm_offset_e, utm_offset_n), axis=1) + + return coords + utm_offset + + @staticmethod + def utm_to_world_numpy(coords): + """Converts utm coordinates to world coordinates + + Args: + coords (numpy.ndarray [size: (N,2)]): array of utm_e, utm_n pairs + + Returns: + numpy.ndarray [size: (N,2)]: array of x, y pairs + """ + + N = np.shape(coords)[0] + utm_offset_e = np.ones((N, )) * World.WORLD_EAST_ZERO + utm_offset_n = np.ones((N, )) * World.WORLD_NORTH_ZERO + utm_offset = np.stack((utm_offset_e, utm_offset_n), axis=1) + + return coords - utm_offset + + + @staticmethod + def world_to_gps(x, y): + """Converts world coordinates to GPS coordinates + + Args: + x (float): x in meters from some arbitrary zero point + y (float): y in meters from some arbitrary zero point + + Returns: + tuple: (lat, lon) + """ + utm_coords = utm.to_latlon( + x + World.WORLD_EAST_ZERO, y + World.WORLD_NORTH_ZERO, 17, "T" + ) + lat = utm_coords[0] + lon = utm_coords[1] + + return lat, lon + + @staticmethod + def utm_to_gps(x, y): + """Converts utm coordinates to GPS coordinates + + Args: + x (float): x in meters, in utm frame + y (float): y in meters, in utm frame + + Returns: + tuple: (lat, lon) + """ + utm_coords = utm.to_latlon( + x, y, 17, "T" + ) + lat = utm_coords[0] + lon = utm_coords[1] + + return lat, lon + + @staticmethod + def gps_to_world_numpy(coords): + """Converts GPS coordinates to world coordinates + + Args: + coords (numpy.ndarray [size: (N,2)]): array of lat, lon pairs + + Returns: + numpy.ndarray [size: (N,2)]: array of x, y pairs + """ + utm_coords = utm.from_latlon(coords[:, 0], coords[:, 1]) + x = utm_coords[0] - World.WORLD_EAST_ZERO + y = utm_coords[1] - World.WORLD_NORTH_ZERO + + return np.stack((x, y), axis=1) + + @staticmethod + def world_to_gps_numpy(coords): + """Converts world coordinates to GPS coordinates + + Args: + coords (numpy.ndarray [size: (N,2)]): array of x, y pairs + + Returns: + numpy.ndarray [size: (N,2)]: array of lat, lon pairs + """ + + # Pittsburgh is in UTM zone 17T. + utm_coords = utm.to_latlon( + coords[:, 0] + World.WORLD_EAST_ZERO, + coords[:, 1] + World.WORLD_NORTH_ZERO, + 17, + "T", + ) + lat = utm_coords[0] + lon = utm_coords[1] + + return np.stack((lat, lon), axis=1) + + @staticmethod + def gps_to_world_pose(pose: Pose): + """Converts GPS coordinates to world coordinates + + Args: + pose (Pose): pose with lat, lon coordinates (x=lon, y=lat) + + Returns: + Pose: pose with x, y coordinates + """ + world_coords = World.gps_to_world(pose.y, pose.x) + new_heading = pose.theta # INS uses ENU frame so no heading conversion needed + + return Pose(world_coords[0], world_coords[1], new_heading) + + @staticmethod + def world_to_gps_pose(pose: Pose): + """Converts world coordinates to GPS coordinates + + Args: + pose (Pose): pose with x, y coordinates + + Returns: + Pose: pose with lat, lon coordinates + """ + gps_coords = World.world_to_gps(pose.x, pose.y) + new_heading = pose.theta + + return Pose(gps_coords[1], gps_coords[0], new_heading) \ No newline at end of file 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..e7b7093 --- /dev/null +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -0,0 +1,254 @@ +import rclpy +from rclpy import Node + +from std_msgs.msg import Float64, Int8, UInt8, Bool + +from host_comm import * + +import argparse +from threading import Lock +import threading +from buggy.auton.world import World +from buggy.auton.pose import Pose + +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 not self.is_shutdown(): + if self.fresh_steer: + with self.lock: + self.comms.send_steering(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() + + if isinstance(packet, Odometry): + self.get_logger().debug("packet" + str(packet)) + # Publish to odom topic x and y coord + odom = ROSOdom() + # convert to long lat + try: + lat, long = World.utm_to_gps(packet.y, packet.x) + 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): + self.get_logger().debug("packet" + str(packet)) + odom = ROSOdom() + + # TODO: Not mock rolled accurately (Needs to be Fact Checked) + try: + lat, long = World.utm_to_gps(packet.x, packet.y) + odom.pose.pose.position.x = long + odom.pose.pose.position.y = lat + odom.twist.twist.angular.z = packet.heading_rate + heading = Pose.heading_to_quaternion(packet.heading) + + 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(packet[0])) + self.steering_angle_publisher.publish(Float64(packet[1])) + self.battery_voltage_publisher.publish(Float64(packet[2])) + self.operator_ready_publisher.publish(Bool(packet[3])) + self.steering_alarm_publisher.publish(Bool(packet[4])) + self.brake_status_publisher.publish(Bool(packet[5])) + self.use_auton_steer_publisher.publish(Bool(packet[6])) + self.rc_uplink_qual_publisher.publish(UInt8(packet[7])) + self.nand_fix_publisher.publish(UInt8(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()