diff --git a/Dockerfile b/Dockerfile index b855e3c..4ffeb8d 100644 --- a/Dockerfile +++ b/Dockerfile @@ -24,7 +24,7 @@ RUN echo 'source "/opt/ros/humble/setup.bash" --' >> ~/.bashrc && \ echo 'cd rb_ws' >> ~/.bashrc && \ echo 'colcon build --symlink-install' >> ~/.bashrc && \ echo 'source install/local_setup.bash' >> ~/.bashrc && \ - echo 'cchmod -R +x src/buggy/scripts/' + echo 'chmod -R +x src/buggy/scripts/' >> ~/.bashrc # add mouse to tmux RUN echo 'set -g mouse on' >> ~/.tmux.conf diff --git a/rb_ws/src/buggy/scripts/buggy_state_converter.py b/rb_ws/src/buggy/scripts/buggy_state_converter.py index eab0cba..a864c28 100755 --- a/rb_ws/src/buggy/scripts/buggy_state_converter.py +++ b/rb_ws/src/buggy/scripts/buggy_state_converter.py @@ -13,50 +13,51 @@ def __init__(self): self.get_logger().info('INITIALIZED.') namespace = self.get_namespace() + if namespace == "/SC": + self.SC_raw_state_subscriber = self.create_subscription( + Odometry, "/raw_state", self.convert_SC_state_callback, 10 + ) + + self.NAND_other_raw_state_subscriber = self.create_subscription( + Odometry, "/NAND_raw_state", self.convert_NAND_other_state_callback, 10 + ) - # Create publisher and subscriber for "self" namespace - self.self_raw_state_subscriber = self.create_subscription( - Odometry, "self/raw_state", self.self_raw_state_callback, 10 - ) - self.self_state_publisher = self.create_publisher(Odometry, "self/state", 10) + self.other_state_publisher = self.create_publisher(Odometry, "/other/state", 10) - # Check if namespace is "SC" to add an "other" subscriber/publisher - if namespace == "/SC": - self.other_raw_state_subscriber = self.create_subscription( - Odometry, "other/raw_state", self.other_raw_state_callback, 10 + elif namespace == "/NAND": + self.NAND_raw_state_subscriber = self.create_subscription( + Odometry, "/raw_state", self.convert_NAND_state_callback, 10 ) - self.other_state_publisher = self.create_publisher(Odometry, "other/state", 10) + + else: + self.get_logger().warn(f"Namespace not recognized for buggy state conversion: {namespace}") + + self.self_state_publisher = self.create_publisher(Odometry, "/state", 10) # Initialize pyproj Transformer for ECEF -> UTM conversion for /SC self.ecef_to_utm_transformer = pyproj.Transformer.from_crs( "epsg:4978", "epsg:32617", always_xy=True ) # TODO: Confirm UTM EPSG code, using EPSG:32617 for UTM Zone 17N - def self_raw_state_callback(self, msg): - """ Callback for processing self/raw_state messages and publishing to self/state """ - namespace = self.get_namespace() - - self.get_logger().error("CALLED_SELF_CALLBACK") - - if namespace == "/SC": - converted_msg = self.convert_SC_state(msg) - elif namespace == "/NAND": - converted_msg = self.convert_NAND_state(msg) - else: - self.get_logger().warn(f"Namespace not recognized for buggy state conversion: {namespace}") - return + def convert_SC_state_callback(self, msg): + """ Callback for processing SC/raw_state messages and publishing to self/state """ + converted_msg = self.convert_SC_state(msg) + self.self_state_publisher.publish(converted_msg) + def convert_NAND_state_callback(self, msg): + """ Callback for processing NAND/raw_state messages and publishing to self/state """ + converted_msg = self.convert_NAND_state(msg) self.self_state_publisher.publish(converted_msg) - def other_raw_state_callback(self, msg): - """ Callback for processing other/raw_state messages and publishing to other/state """ - # Convert the SC message and publish to other/state - self.get_logger().error("CALLED_OTHER_CALLBACK") + + def convert_NAND_other_state_callback(self, msg): + """ Callback for processing SC/NAND_raw_state messages and publishing to other/state """ converted_msg = self.convert_NAND_other_state(msg) self.other_state_publisher.publish(converted_msg) + def convert_SC_state(self, msg): - """ + """ Converts self/raw_state in SC namespace to clean state units and structure Takes in ROS message in nav_msgs/Odometry format @@ -131,7 +132,6 @@ def convert_NAND_state(self, msg): converted_msg.twist.covariance = msg.twist.covariance # ---- 4. Linear Velocities in m/s ---- - # TODO: Check if scalar velocity is coming in from msg.twist.twist.linear.x # Convert scalar speed to velocity x/y components using heading (orientation.z) speed = msg.twist.twist.linear.x # m/s scalar velocity heading = msg.pose.pose.orientation.z # heading in radians diff --git a/rb_ws/src/buggy/scripts/controller/controller_node.py b/rb_ws/src/buggy/scripts/controller/controller_node.py index 7feb9fb..1461a33 100755 --- a/rb_ws/src/buggy/scripts/controller/controller_node.py +++ b/rb_ws/src/buggy/scripts/controller/controller_node.py @@ -58,7 +58,7 @@ def __init__(self): Float64, "input/steering", 1 ) self.heading_publisher = self.create_publisher( - Float32, "auton/debug/heading", 1 + Float32, "debug/heading", 1 ) # Subscribers @@ -142,7 +142,7 @@ def loop(self): self.heading_publisher.publish(Float32(data=np.rad2deg(self.odom.pose.pose.orientation.z))) steering_angle = self.controller.compute_control(self.odom, self.cur_traj) steering_angle_deg = np.rad2deg(steering_angle) - self.steer_publisher.publish(Float64(data=float(steering_angle_deg))) + self.steer_publisher.publish(Float64(data=float(steering_angle_deg.item()))) diff --git a/rb_ws/src/buggy/scripts/controller/stanley_controller.py b/rb_ws/src/buggy/scripts/controller/stanley_controller.py index b5378ec..8c3ac1c 100755 --- a/rb_ws/src/buggy/scripts/controller/stanley_controller.py +++ b/rb_ws/src/buggy/scripts/controller/stanley_controller.py @@ -30,10 +30,10 @@ class StanleyController(Controller): def __init__(self, start_index, namespace, node): super(StanleyController, self).__init__(start_index, namespace, node) self.debug_reference_pos_publisher = self.node.create_publisher( - NavSatFix, "auton/debug/reference_navsat", 1 + NavSatFix, "controller/debug/reference_navsat", 1 ) self.debug_error_publisher = self.node.create_publisher( - ROSPose, "auton/debug/error", 1 + ROSPose, "controller/debug/stanley_error", 1 ) def compute_control(self, state_msg : Odometry, trajectory : Trajectory): diff --git a/rb_ws/src/buggy/scripts/path_planner/path_planner.py b/rb_ws/src/buggy/scripts/path_planner/path_planner.py index 2f91b1c..a42e6a1 100755 --- a/rb_ws/src/buggy/scripts/path_planner/path_planner.py +++ b/rb_ws/src/buggy/scripts/path_planner/path_planner.py @@ -9,10 +9,10 @@ from nav_msgs.msg import Odometry from std_msgs.msg import Float64 +from geometry_msgs.msg import Pose from buggy.msg import TrajectoryMsg sys.path.append("/rb_ws/src/buggy/scripts") -from util.pose import Pose from util.trajectory import Trajectory class PathPlanner(Node): @@ -64,7 +64,7 @@ def __init__(self) -> None: self.left_curb = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + curb_name) #TODO: Fixed filepath, not good #Publishers - self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "self/debug/other_buggy_xtrack", 10) + self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "debug/other_buggy_xtrack", 10) self.traj_publisher = self.create_publisher(TrajectoryMsg, "self/cur_traj", 10) #Subscribers @@ -82,12 +82,12 @@ def __init__(self) -> None: def self_pose_callback(self, msg): with self.self_pose_lock: - self.self_pose = Pose.rospose_to_pose(msg.pose.pose) + self.self_pose = msg.pose.pose def other_pose_callback(self, msg): with self.other_pose_lock: - self.other_pose = Pose.rospose_to_pose(msg.pose.pose) - self.get_logger().debug("RECEIVED FROM NAND") + self.other_pose = msg.pose.pose + self.get_logger().debug("Received position of other buggy.") def timer_callback(self): with self.self_pose_lock and self.other_pose_lock: @@ -157,7 +157,7 @@ def compute_traj( Trajectory: list of x,y coords for ego-buggy to follow. """ # grab slice of nominal trajectory - nominal_idx = self.nominal_traj.get_closest_index_on_path(self_pose.x, self_pose.y) + nominal_idx = self.nominal_traj.get_closest_index_on_path(self_pose.position.x, self_pose.position.y) nominal_dist_along = self.nominal_traj.get_distance_from_index(nominal_idx) nominal_slice = np.empty((self.RESOLUTION, 2)) @@ -174,14 +174,14 @@ def compute_traj( )) # get index of the other buggy along the trajetory and convert to distance - other_idx = self.nominal_traj.get_closest_index_on_path(other_pose.x, other_pose.y) + other_idx = self.nominal_traj.get_closest_index_on_path(other_pose.position.x, other_pose.position.y) other_dist = self.nominal_traj.get_distance_from_index(other_idx) nominal_slice_to_other_dist = np.abs(nominal_slice_dists - other_dist) passing_offsets = self.offset_func(nominal_slice_to_other_dist) # compute signed cross-track distance between NAND and nominal - nominal_to_other = np.array((other_pose.x, other_pose.y)) - \ + nominal_to_other = np.array((other_pose.position.x, other_pose.position.y)) - \ np.array(self.nominal_traj.get_position_by_index(other_idx)) # dot product with the unit normal to produce left-positive signed distance @@ -189,7 +189,7 @@ def compute_traj( other_cross_track_dist = np.sum( nominal_to_other * other_normal, axis=1) - self.other_buggy_xtrack_publisher.publish(Float64(data=float(other_cross_track_dist))) + self.other_buggy_xtrack_publisher.publish(Float64(data=float(other_cross_track_dist.item()))) # here, use passing offsets to weight NAND's cross track signed distance: # if the sample point is far from SC, the cross track distance doesn't matter @@ -202,7 +202,7 @@ def compute_traj( # clamp passing offset distances to distance to the curb if self.left_curb is not None: # grab slice of curb correponding to slice of nominal trajectory. - curb_idx = self.left_curb.get_closest_index_on_path(self_pose.x, self_pose.y) + curb_idx = self.left_curb.get_closest_index_on_path(self_pose.position.x, self_pose.position.y) curb_dist_along = self.left_curb.get_distance_from_index(curb_idx) curb_idx_end = self.left_curb.get_closest_index_on_path(nominal_slice[-1, 0], nominal_slice[-1, 1]) curb_dist_along_end = self.left_curb.get_distance_from_index(curb_idx_end) @@ -229,7 +229,7 @@ def compute_traj( positions = nominal_slice + (passing_offsets[:, None] * nominal_normals) local_traj = Trajectory(json_filepath=None, positions=positions) - self.traj_publisher.publish(local_traj.pack(self_pose.x, self_pose.y )) + self.traj_publisher.publish(local_traj.pack(self_pose.position.x, self_pose.position.y )) def main(args=None): diff --git a/rb_ws/src/buggy/scripts/serial/host_comm.py b/rb_ws/src/buggy/scripts/serial/host_comm.py index e98c23e..766247e 100755 --- a/rb_ws/src/buggy/scripts/serial/host_comm.py +++ b/rb_ws/src/buggy/scripts/serial/host_comm.py @@ -58,28 +58,104 @@ def update(self, b): MAX_PAYLOAD_LEN = 100 -MSG_TYPE_DEBUG = b'DB' -MSG_TYPE_ODOMETRY = b'OD' + +# firmware --> software +MSG_TYPE_NAND_DEBUG = b'ND' +MSG_TYPE_NAND_UKF = b'NU' +MSG_TYPE_NAND_GPS = b'NG' +MSG_TYPE_RADIO = b'RA' +MSG_TYPE_SC_DEBUG = b'SD' +MSG_TYPE_SC_SENSORS = b'SS' +MSG_TYPE_ROUNDTRIP_TIMESTAMP = b'RT' + +# software --> firmware MSG_TYPE_STEERING = b'ST' -MSG_TYPE_BRAKE = b'BR' MSG_TYPE_ALARM = b'AL' -MSG_TYPE_BNYATEL = b'BT' +MSG_TYPE_SOFTWARE_TIMESTAMP = b'TM' + + @dataclass -class Odometry: - x: float - y: float - radio_seqnum: int +class NANDDebugInfo: + # 64 bits + heading_rate: float # double + encoder_angle: float # double + # 32 bits + timestamp: int + rc_steering_angle: float + software_steering_angle: float + true_steering_angle: float + rfm69_timeout_num: int + # 8 bits + operator_ready: bool + brake_status: bool + auton_steer: bool + tx12_state: bool + stepper_alarm: int # unsigned char + rc_uplink_quality: int # uint8 + +@dataclass +class NANDUKF: + # 64 bits + easting: float # double + northing: float # double + theta: float # double + heading_rate: float # double + velocity: float # double + # 32 bits + timestamp: int + +@dataclass +class NANDRawGPS: + # 64 bits + easting: float # double + northing: float # double + # this is a 2D accuracy value + accuracy: float # double + gps_time: int # uint64 + # 32 bits + gps_seqnum: int + timestamp: int + # 8 bits + gps_fix: int # uint8 + +@dataclass +class Radio: + nand_east_gps: float + nand_north_gps: float gps_seqnum: int + nand_gps_fix: int # uint8 + +@dataclass +class SCDebugInfo: + # 64 bits + encoder_angle: float # double + # 32 bits + rc_steering_angle: float + software_steering_angle: float + true_steering_angle: float + missed_packets: int + timestamp: int + # 8 bits + tx12_state: bool + operator_ready: bool + stepper_alarm: int # unsigned char + brake_status: bool + auton_steer: bool + rc_uplink_quality: int # uint8 @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 SCSensors: + # 64 bits + velocity: float # double + # 32 bits + steering_angle: float + timestamp: int + +@dataclass +class RoundtripTimestamp: + returned_time: float # double + class IncompletePacket(Exception): pass @@ -115,9 +191,11 @@ def send_steering(self, angle: float): def send_alarm(self, status: int): self.send_packet_raw(MSG_TYPE_ALARM, struct.pack(' (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): + def __init__(self, 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) + namespace = self.get_namespace() + if namespace == "/SC": + self.self_name = "SC" + else: + self.self_name = "NAND" + 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 + Float64, "/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 - ) + self.create_subscription(Int8, "/input/sanity_warning", self.set_alarm, 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) @@ -67,35 +49,88 @@ def __init__(self, self_name, other_name, teensy_name): # 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 + # DEBUG MESSAGE PUBLISHERS: + self.heading_rate_publisher = self.create_publisher( + Float64, "/debug/heading_rate", 1 + ) + self.encoder_angle_publisher = self.create_publisher( + Float64, "/debug/encoder_angle", 1 + ) self.rc_steering_angle_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/rc_steering_angle", 1 + Float64, "/debug/rc_steering_angle", 1 ) - self.steering_angle_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/steering_angle", 1 + self.software_steering_angle_publisher = self.create_publisher( + Float64, "/debug/software_steering_angle", 1 ) - self.battery_voltage_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/battery_voltage", 1 + self.true_steering_angle_publisher = self.create_publisher( + Float64, "/debug/true_steering_angle", 1 ) - self.operator_ready_publisher = self.create_publisher( - Bool, self_name + "/buggy/debug/operator_ready", 1 + self.rfm69_timeout_num_publisher = self.create_publisher( + Int32, "/debug/rfm_timeout_num", 1 ) - self.steering_alarm_publisher = self.create_publisher( - Bool, self_name + "/buggy/debug/steering_alarm", 1 + self.operator_ready_publisher = self.create_publisher( + Bool, "/debug/operator_ready", 1 ) self.brake_status_publisher = self.create_publisher( - Bool, self_name + "/buggy/debug/brake_status", 1 + Bool, "/debug/brake_status", 1 ) self.use_auton_steer_publisher = self.create_publisher( - Bool, self_name + "/buggy/debug/use_auton_steer", 1 + Bool, "/debug/use_auton_steer", 1 + ) + self.tx12_state_publisher = self.create_publisher( + Bool, "/debug/tx12_connected", 1 + ) + self.stepper_alarm_publisher = self.create_publisher( + UInt8, "/debug/steering_alarm", 1 ) self.rc_uplink_qual_publisher = self.create_publisher( - UInt8, self_name + "/buggy/debug/rc_uplink_quality", 1 + UInt8, "/debug/rc_uplink_quality", 1 + ) + self.nand_gps_seqnum_publisher = self.create_publisher( + Int32, "/debug/NAND_gps_seqnum", 1 ) - self.nand_fix_publisher = self.create_publisher( - UInt8, self_name + "/buggy/debug/nand_fix", 1 + + # SERIAL DEBUG PUBLISHERS + self.roundtrip_time_publisher = self.create_publisher( + Float64, "/debug/roundtrip_time", 1 ) + if self.self_name == "NAND": + # NAND POSITION PUBLISHERS + self.nand_ukf_odom_publisher = self.create_publisher( + Odometry, "/raw_state", 1 + ) + self.nand_gps_odom_publisher = self.create_publisher( + Odometry, "/debug/gps_odom", 1 + ) + + self.nand_gps_fix_publisher = self.create_publisher( + UInt8, "/debug/gps_fix", 1 + ) + self.nand_gps_acc_publisher = self.create_publisher( + Float64, "/debug/gps_accuracy", 1 + ) + + self.nand_gps_time_publisher = self.create_publisher( + UInt64, "/debug/gps_time", 1 + ) + + if self.self_name == "SC": + + # SC SENSOR PUBLISHERS + self.sc_velocity_publisher = self.create_publisher( + Float64, "/sensors/velocity", 1 + ) + self.sc_steering_angle_publisher = self.create_publisher( + Float64, "/sensors/steering_angle", 1 + ) + + # RADIO DATA PUBLISHER + self.observed_nand_odom_publisher = self.create_publisher( + Odometry, "/NAND_raw_state", 1 + ) + + def set_alarm(self, msg): """ alarm ros topic reader, locked so that only one of the setters runs at once @@ -108,9 +143,7 @@ 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)) + self.get_logger().debug(f"Read steering angle of: {msg.data}") with self.lock: self.steer_angle = msg.data self.fresh_steer = True @@ -119,7 +152,6 @@ 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!") @@ -132,76 +164,97 @@ def writer_thread(self): with self.lock: self.comms.send_alarm(self.alarm) + with self.lock: + self.comms.send_timestamp(time.time()) 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() self.get_logger().debug("packet" + str(packet)) - if isinstance(packet, Odometry): + if isinstance(packet, NANDDebugInfo): + self.heading_rate_publisher.publish(packet.heading_rate) + self.encoder_angle_publisher.publish(packet.encoder_angle) + self.rc_steering_angle_publisher.publish(packet.rc_steering_angle) + self.software_steering_angle_publisher.publish(packet.software_steering_angle) + self.true_steering_angle_publisher.publish(packet.true_steering_angle) + self.rfm69_timeout_num_publisher.publish(packet.rfm69_timeout_num) + self.operator_ready_publisher.publish(packet.operator_ready) + self.brake_status_publisher.publish(packet.brake_status) + self.use_auton_steer_publisher.publish(packet.auton_steer) + self.tx12_state_publisher.publish(packet.tx12_state) + self.stepper_alarm_publisher.publish(packet.stepper_alarm) + self.rc_uplink_qual_publisher.publish(packet.rc_uplink_quality) + self.get_logger().debug(f'NAND Debug Timestamp: {packet.timestamp}') + elif isinstance(packet, NANDUKF): + odom = Odometry() + odom.pose.pose.position.x = packet.easting + odom.pose.pose.position.y = packet.northing + odom.pose.pose.orientation.z = packet.theta + + odom.twist.twist.linear.x = packet.velocity + odom.twist.twist.angular.z = packet.heading_rate + + self.nand_ukf_odom_publisher.publish(odom) + self.get_logger().debug(f'NAND UKF Timestamp: {packet.timestamp}') + + + elif isinstance(packet, NANDRawGPS): + odom = Odometry() + odom.pose.pose.position.x = packet.easting + odom.pose.pose.position.y = packet.northing + odom.pose.pose.orientation.z = 0 + odom.twist.twist.linear.x = 0 + odom.twist.twist.linear.y = 0 + odom.twist.twist.angular.z = 0 + + self.nand_gps_odom_publisher.publish(odom) + self.nand_gps_fix_publisher.publish(packet.gps_fix) + self.nand_gps_acc_publisher.publish(packet.accuracy) + self.nand_gps_seqnum_publisher.publish(packet.gps_seqnum) + self.nand_gps_time_publisher.publish(packet.gps_time) + self.get_logger().debug(f'NAND Raw GPS Timestamp: {packet.timestamp}') + + + # this packet is received on Short Circuit + elif isinstance(packet, Radio): + # Publish to odom topic x and y coord - odom = ROSOdom() - # convert to long lat - try: - # 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) - except Exception as e: - self.get_logger().warn( - "Unable to convert other buggy position to lon lat" + str(e) - ) - - elif isinstance(packet, BnyaTelemetry): - odom = ROSOdom() - - # TODO: Not mock rolled accurately (Needs to be Fact Checked) - try: - 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 = Rotation.from_euler('x', packet.heading, degrees=False).as_quat() - - 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(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])) + odom = Odometry() + + odom.pose.pose.position.x = packet.nand_east_gps + odom.pose.pose.position.y = packet.nand_north_gps + self.observed_nand_odom_publisher.publish(odom) + self.nand_gps_seqnum_publisher.publish(packet.gps_seqnum) + + + + + elif isinstance(packet, SCDebugInfo): + self.encoder_angle_publisher.publish(packet.encoder_angle) + self.rc_steering_angle_publisher.publish(packet.rc_steering_angle) + self.software_steering_angle_publisher.publish(packet.software_steering_angle) + self.true_steering_angle_publisher.publish(packet.true_steering_angle) + self.operator_ready_publisher.publish(packet.operator_ready) + self.brake_status_publisher.publish(packet.brake_status) + self.use_auton_steer_publisher.publish(packet.auton_steer) + self.tx12_state_publisher.publish(packet.tx12_state) + self.stepper_alarm_publisher.publish(packet.stepper_alarm) + self.rc_uplink_qual_publisher.publish(packet.rc_uplink_quality) + self.get_logger().debug(f'SC Debug Timestamp: {packet.timestamp}') + + + elif isinstance(packet, SCSensors): + self.sc_velocity_publisher.publish(packet.velocity) + self.sc_steering_angle_publisher.publish(packet.steering_angle) + self.get_logger().debug(f'SC Sensors Timestamp: {packet.timestamp}') + + + elif isinstance(packet, RoundtripTimestamp): + self.roundtrip_time_publisher.publish(time.time() - packet.returned_time) self.read_rate.sleep()