diff --git a/rb_ws/src/buggy/scripts/auton/pose.py b/rb_ws/src/buggy/scripts/auton/pose.py index e08aa7a8..7bb09e94 100755 --- a/rb_ws/src/buggy/scripts/auton/pose.py +++ b/rb_ws/src/buggy/scripts/auton/pose.py @@ -2,6 +2,7 @@ from geometry_msgs.msg import Pose as ROSPose from tf.transformations import euler_from_quaternion +from tf.transformations import quaternion_from_euler class Pose: @@ -38,6 +39,10 @@ def rospose_to_pose(rospose: ROSPose): 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 @@ -94,7 +99,7 @@ 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 diff --git a/rb_ws/src/buggy/scripts/serial/host_comm.py b/rb_ws/src/buggy/scripts/serial/host_comm.py index 14d4e932..0d882e33 100644 --- a/rb_ws/src/buggy/scripts/serial/host_comm.py +++ b/rb_ws/src/buggy/scripts/serial/host_comm.py @@ -63,6 +63,7 @@ def update(self, b): MSG_TYPE_STEERING = b'ST' MSG_TYPE_BRAKE = b'BR' MSG_TYPE_ALARM = b'AL' +MSG_TYPE_BNYATEL = b'BT' @dataclass class Odometry: @@ -71,6 +72,15 @@ class Odometry: 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 @@ -183,6 +193,9 @@ def read_packet(self): debug = struct.unpack('