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 b631ca6..0000000 --- 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 0558688..0000000 --- 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 0e55a16..17f5ae7 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()