From 5ae86cb475489dce8cd7d5e2d12e2510b080d30a Mon Sep 17 00:00:00 2001 From: Rishi Kumar Date: Sun, 1 Dec 2024 23:42:44 +0000 Subject: [PATCH 1/7] Ported ros_to_bnyahaj.py - Added pyserial to project requirements - Added host_comm, pose, and world.py - Copied tf1 euler/quaternion conversion code to pose.py --- python-requirements.txt | 1 + rb_ws/src/buggy/buggy/auton/pose.py | 260 ++++++++++++++++++ rb_ws/src/buggy/buggy/auton/world.py | 226 +++++++++++++++ rb_ws/src/buggy/buggy/serial/host_comm.py | 218 +++++++++++++++ .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 255 +++++++++++++++++ 5 files changed, 960 insertions(+) create mode 100644 rb_ws/src/buggy/buggy/auton/pose.py create mode 100644 rb_ws/src/buggy/buggy/auton/world.py create mode 100644 rb_ws/src/buggy/buggy/serial/host_comm.py create mode 100644 rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py diff --git a/python-requirements.txt b/python-requirements.txt index 68b0c6fc..66b300e7 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 00000000..b631ca69 --- /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 00000000..0558688d --- /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 00000000..e98c23e0 --- /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 00000000..0e55a16d --- /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 std_msgs.msg import Float64, Int8, UInt8, Bool + +from host_comm import * + +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() From ed35e5a0d34b05db763e61b5ec5ee9848a6fb568 Mon Sep 17 00:00:00 2001 From: Rishi Kumar Date: Wed, 11 Dec 2024 19:50:19 +0000 Subject: [PATCH 2/7] Removed pose/world.py, updated ros_to_bnyahaj.py - Using scipy/utm package directly, removed dependency on pose/world.py - Added steering angle debug on publish - Standardizes use of rclpy.ok() instead of self.is_shutdown() - All packets are sent over debug telemetry --- rb_ws/src/buggy/buggy/auton/pose.py | 260 ------------------ rb_ws/src/buggy/buggy/auton/world.py | 226 --------------- .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 35 ++- 3 files changed, 17 insertions(+), 504 deletions(-) delete mode 100644 rb_ws/src/buggy/buggy/auton/pose.py delete mode 100644 rb_ws/src/buggy/buggy/auton/world.py diff --git a/rb_ws/src/buggy/buggy/auton/pose.py b/rb_ws/src/buggy/buggy/auton/pose.py deleted file mode 100644 index b631ca69..00000000 --- a/rb_ws/src/buggy/buggy/auton/pose.py +++ /dev/null @@ -1,260 +0,0 @@ -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 deleted file mode 100644 index 0558688d..00000000 --- a/rb_ws/src/buggy/buggy/auton/world.py +++ /dev/null @@ -1,226 +0,0 @@ -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/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index 0e55a16d..17f5ae7d 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -4,14 +4,13 @@ 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 buggy.auton.world import World -from buggy.auton.pose import Pose - from nav_msgs.msg import Odometry as ROSOdom class Translator(Node): @@ -122,10 +121,11 @@ def writer_thread(self): """ self.get_logger().info("Starting sending alarm and steering to teensy!") - while not self.is_shutdown(): + 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: @@ -143,14 +143,14 @@ def reader_thread(self): 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): - 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) + 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) @@ -160,16 +160,15 @@ def reader_thread(self): ) 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) + 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 = Pose.heading_to_quaternion(packet.heading) + 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] @@ -191,15 +190,15 @@ def reader_thread(self): 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.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() From a57c3168a01960035ad6be8c056e57eaa92f4143 Mon Sep 17 00:00:00 2001 From: Rishi Kumar Date: Wed, 18 Dec 2024 01:05:52 +0000 Subject: [PATCH 3/7] Removed import alias for SciPy Rotation in ros_to_bnyahaj.py, fixed possible swapping of packet x and y for ROS odom topic publishing. --- rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 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 17f5ae7d..ca4bbea6 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 scipy.spatial.transform import Rotation as R +from scipy.spatial.transform import Rotation import utm from std_msgs.msg import Float64, Int8, UInt8, Bool @@ -150,7 +150,8 @@ def reader_thread(self): odom = ROSOdom() # convert to long lat try: - lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") + # 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) @@ -168,7 +169,7 @@ def reader_thread(self): 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() + 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] From 55c4cae1b39073aff8c993c195f25ca0420a635e Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 18 Dec 2024 22:43:50 -0800 Subject: [PATCH 4/7] cleanup --- rb_ws/src/buggy/buggy/simulator/engine.py | 80 +++++++---------------- rb_ws/src/buggy/buggy/util.py | 8 +++ 2 files changed, 31 insertions(+), 57 deletions(-) create mode 100644 rb_ws/src/buggy/buggy/util.py diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 361ae4bf..912ec561 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -8,25 +8,17 @@ from nav_msgs.msg import Odometry import numpy as np import utm +from util import Utils 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') self.get_logger().info('INITIALIZED.') - 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": (Utils.UTM_EAST_ZERO + 0, Utils.UTM_NORTH_ZERO + 0, -110), + "Hill1_SC": (Utils.UTM_EAST_ZERO + 20, Utils.UTM_NORTH_ZERO + 30, -110), "WESTINGHOUSE": (589647, 4477143, -150), "UC_TO_PURNELL": (589635, 4477468, 160), "UC": (589681, 4477457, 160), @@ -39,9 +31,6 @@ 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) @@ -51,43 +40,43 @@ def __init__(self): 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 ) - - + # To read from velocity + self.velocity_subscriber = self.create_subscription( + Float64, "velocity", self.update_velocity, 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 = Utils.WHEELBASE_SC if (self.get_namespace() == "/NAND"): self.buggy_name = "NAND" self.declare_parameter('pose', "Hill1_NAND") - self.wheelbase = Simulator.WHEELBASE_NAND + self.wheelbase = Utils.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) def update_steering_angle(self, data: Float64): @@ -98,14 +87,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 +135,13 @@ def publish(self): (lat, long) = utm.to_latlon( p.position.x, p.position.y, - Simulator.UTM_ZONE_NUM, - Simulator.UTM_ZONE_LETTER, + Utils.UTM_ZONE_NUM, + Utils.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,8 +152,8 @@ def publish(self): odom.header.stamp = time_stamp odom_pose = Pose() - odom_pose.position.x = float(long) - odom_pose.position.y = float(lat) + odom_pose.position.x = float(long_noisy) + odom_pose.position.y = float(lat_noisy) odom_pose.position.z = float(260) odom_pose.orientation.z = np.sin(np.deg2rad(self.heading) / 2) @@ -192,20 +168,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 00000000..b5e78e1a --- /dev/null +++ b/rb_ws/src/buggy/buggy/util.py @@ -0,0 +1,8 @@ +class Utils: + 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 From 295913880f50b6449dfdbc8b1c983914e356e1fb Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 18 Dec 2024 23:02:03 -0800 Subject: [PATCH 5/7] cleaned up constants class --- rb_ws/src/buggy/buggy/simulator/engine.py | 18 +++++++++++------- rb_ws/src/buggy/buggy/util.py | 2 +- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 912ec561..0d684625 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,17 +9,20 @@ from nav_msgs.msg import Odometry import numpy as np import utm -from util import Utils +sys.path.append("/rb_ws/src/buggy/buggy") +from util import Constants class Simulator(Node): + def __init__(self): super().__init__('sim_single') self.get_logger().info('INITIALIZED.') + self.starting_poses = { - "Hill1_NAND": (Utils.UTM_EAST_ZERO + 0, Utils.UTM_NORTH_ZERO + 0, -110), - "Hill1_SC": (Utils.UTM_EAST_ZERO + 20, Utils.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), @@ -56,12 +60,12 @@ def __init__(self): if (self.get_namespace() == "/SC"): self.buggy_name = "SC" self.declare_parameter('pose', "Hill1_SC") - self.wheelbase = Utils.WHEELBASE_SC + self.wheelbase = Constants.WHEELBASE_SC if (self.get_namespace() == "/NAND"): self.buggy_name = "NAND" self.declare_parameter('pose', "Hill1_NAND") - self.wheelbase = Utils.WHEELBASE_NAND + self.wheelbase = Constants.WHEELBASE_NAND self.velocity = self.get_parameter("velocity").value init_pose_name = self.get_parameter("pose").value @@ -135,8 +139,8 @@ def publish(self): (lat, long) = utm.to_latlon( p.position.x, p.position.y, - Utils.UTM_ZONE_NUM, - Utils.UTM_ZONE_LETTER, + Constants.UTM_ZONE_NUM, + Constants.UTM_ZONE_LETTER, ) lat_noisy = lat + np.random.normal(0, 1e-6) diff --git a/rb_ws/src/buggy/buggy/util.py b/rb_ws/src/buggy/buggy/util.py index b5e78e1a..0bdfc8e3 100644 --- a/rb_ws/src/buggy/buggy/util.py +++ b/rb_ws/src/buggy/buggy/util.py @@ -1,4 +1,4 @@ -class Utils: +class Constants: UTM_EAST_ZERO = 589702.87 UTM_NORTH_ZERO = 4477172.947 UTM_ZONE_NUM = 17 From a537b8839b41d717152c49a700542ee7c117297b Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Mon, 23 Dec 2024 22:16:24 -0800 Subject: [PATCH 6/7] rearranged subscribers/publishers --- rb_ws/src/buggy/buggy/simulator/engine.py | 40 +++++++++++------------ 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 0d684625..8d119982 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -35,28 +35,7 @@ def __init__(self): "RACELINE_PASS": (589468.02, 4476993.07, -160), } - # 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 - ) - - self.navsatfix_noisy_publisher = self.create_publisher( - NavSatFix, "state/pose_navsat_noisy", 1 - ) - - # To read from velocity - self.velocity_subscriber = self.create_subscription( - Float64, "velocity", self.update_velocity, 1 - ) - - self.declare_parameter('velocity', 12) - if (self.get_namespace() == "/SC"): self.buggy_name = "SC" self.declare_parameter('pose', "Hill1_SC") @@ -83,6 +62,25 @@ def __init__(self): 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) + 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 From a5a6ab5600356ff992a914b7033981c4d7fe243c Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Mon, 23 Dec 2024 22:30:42 -0800 Subject: [PATCH 7/7] changed units to match clean_slate type (utm) --- rb_ws/src/buggy/buggy/simulator/engine.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 8d119982..efaf6dbe 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -75,6 +75,7 @@ def __init__(self): 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( @@ -154,13 +155,15 @@ def publish(self): odom.header.stamp = time_stamp odom_pose = Pose() - odom_pose.position.x = float(long_noisy) - odom_pose.position.y = float(lat_noisy) + 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)