diff --git a/rb_ws/src/buggy/CMakeLists.txt b/rb_ws/src/buggy/CMakeLists.txt index f81c12e2..7e6610a8 100755 --- a/rb_ws/src/buggy/CMakeLists.txt +++ b/rb_ws/src/buggy/CMakeLists.txt @@ -54,7 +54,6 @@ add_message_files( FILES EncoderStatus.msg LoRaEvent.msg - BuggyState.msg ) ## Generate services in the 'srv' folder diff --git a/rb_ws/src/buggy/msg/BuggyState.msg b/rb_ws/src/buggy/msg/BuggyState.msg deleted file mode 100644 index 040e9f95..00000000 --- a/rb_ws/src/buggy/msg/BuggyState.msg +++ /dev/null @@ -1,7 +0,0 @@ -Header header -float64 easting -float64 northing -float64 yaw -float64 easting_v -float64 northing_v -float64 yaw_v diff --git a/rb_ws/src/buggy/scripts/auton/buggystate.py b/rb_ws/src/buggy/scripts/auton/buggystate.py index 1c6810cd..2a3ca5a7 100644 --- a/rb_ws/src/buggy/scripts/auton/buggystate.py +++ b/rb_ws/src/buggy/scripts/auton/buggystate.py @@ -1,214 +1,34 @@ import numpy as np - -from geometry_msgs.msg import Pose as ROSPose +from nav_msgs.msg import Odometry as ROSOdom from tf.transformations import euler_from_quaternion from tf.transformations import quaternion_from_euler +import rospy -class BuggyState: - """ - A data structure for storing the state of the buggy at a specific timestep (specifically, the UTM position of the buggy, its heading, and its velocity along the easting, northing, and radial directions. - - Also includes helper functions to convert between pose notations. +class BuggyState: """ + Basically a translator from ROSOdom to ensure everything is in the correct units. - __easting = None - __northing = None - __yaw = None - - __easting_v = None - __northing_v = None - __yaw_v = None - - - def __init__(self, east, north, yaw, east_v = 0, north_v = 0, yaw_v = 0): - self.easting = east - self.northing = north - self.yaw = yaw - - self.easting_v = east_v - self.northing_v = north_v - self.yaw_v = yaw_v - - - def __repr__(self) -> str: - return f"Pose(easting={self.easting}, northing={self.northing}, yaw={self.yaw}), easting velocity={self.easting_v}, northing velocity={self.northing_v}, yaw velocity={self.yaw_v})" - - def copy(self): - return BuggyState(self.easting, self.northing, self.yaw, self.easting_v, self.northing_v, self.yaw_v) - - @property - def easting(self): - return self.easting - - @__easting.setter - def easting(self, east): - self.__easting = east - - @property - def north(self): - return self.northing - - @__northing.setter - def northing(self, north): - self.__northing = north - - @property - def yaw(self): - if self.yaw > np.pi or self.yaw < -np.pi: - raise ValueError("Yaw is not in [-pi, pi]") - return self.yaw - - @__yaw.setter - def yaw(self, yaw): - # normalize theta to [-pi, pi] - self.__yaw = np.arctan2(np.sin(yaw), np.cos(yaw)) - - @property - def easting_v(self): - return self.easting_v - - @__easting_v.setter - def easting_v(self, eastv): - self.__easting_v = eastv - - @property - def northing_v(self): - return self.northing_v - - @__northing_v.setter - def easting(self, northv): - self.__northing_v = northv + Other files should ONLY interface with the nav/odom messages through this file! - @property - def yaw_v(self): - return self.yaw_v + (functionally: this should replace pose and world) - @__yaw_v.setter - def easting(self, yawv): - self.__yaw_v = yawv - - - def to_mat(self): - """ - Returns the pose as a 3x3 homogeneous transformation matrix - """ - return np.array( - [ - [np.cos(self.yaw), -np.sin(self.yaw), self.easting], - [np.sin(self.yaw), np.cos(self.yaw), self.northing], - [0, 0, 1], - ] - ) - - # TODO: currently creates state with zero velocity - @staticmethod - def from_mat(mat): - """ - Creates a pose from a 3x3 homogeneous transformation matrix - """ - return BuggyState(mat[0, 2], mat[1, 2], np.arctan2(mat[1, 0], mat[0, 0])) - - def invert(self): - """ - Inverts the pose - """ - return BuggyState.from_mat(np.linalg.inv(self.to_mat())) - - # TODO: do we want all these conversion methods? where are they even used? - 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.yaw += 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 BuggyState(self.easting + other.easting, self.northing + other.northing, self.yaw + other.yaw) - - def __sub__(self, other): - return BuggyState(self.easting - other.easting, self.northing - other.northing, self.yaw - other.yaw) - - def __neg__(self): - return BuggyState(-self.easting, -self.northing, -self.yaw) - - def __mul__(self, other): - p1_mat = self.to_mat() - p2_mat = other.to_mat() - - return BuggyState.from_mat(p1_mat @ p2_mat) + """ + def __init__(self): + self.rosodom : ROSOdom = None + rospy.Subscriber("/nav/odom", ROSOdom, self.get_odom) - def __truediv__(self, other): - p1_mat = self.to_mat() - p2_mat = other.to_mat() + def get_odom(self, msg): + self.rosodom = msg - return BuggyState.from_mat(np.linalg.inv(p2_mat) @ p1_mat) + """ + FUNCTIONALITY TO ADD: + get pose (in utm) + get velocity (in utm) + get covariance (in whatever units) + create a new rosodom message using some (maybe not all) information (by default set missing info to garbage vals) + + how does this interact with gps coords and all + can we just make this combine with that + """ \ No newline at end of file diff --git a/rb_ws/src/buggy/scripts/auton/pose.py b/rb_ws/src/buggy/scripts/auton/pose.py new file mode 100644 index 00000000..32f098cf --- /dev/null +++ b/rb_ws/src/buggy/scripts/auton/pose.py @@ -0,0 +1,219 @@ +import numpy as np + +from geometry_msgs.msg import Pose as ROSPose +from tf.transformations import euler_from_quaternion +from tf.transformations import quaternion_from_euler + + +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 + + @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) = 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 = 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