diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index 48b1086..bbe5a5e 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -12,47 +12,51 @@ def __init__(self): super().__init__("buggy_state_converter") namespace = self.get_namespace() + if namespace == "/SC": + self.SC_raw_state_subscriber = self.create_subscription( + Odometry, "/raw_state", self.convert_SC_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.NAND_other_raw_state_subscriber = self.create_subscription( + Odometry, "/NAND_raw_state", self.convert_NAND_other_state_callback, 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 + self.other_state_publisher = self.create_publisher(Odometry, "/other/state", 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() - - 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 + + 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 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 0525518..0a8c3ad 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -85,42 +85,46 @@ def __init__(self, teensy_name): UInt8, self_name + "/debug/rc_uplink_quality", 1 ) - # NAND POSITION PUBLISHERS - self.nand_ukf_odom_publisher = self.create_publisher( - Odometry, "NAND/nav/odom", 1 - ) - self.nand_gps_odom_publisher = self.create_publisher( - Odometry, "NAND/debug/gps_odom", 1 - ) - self.observed_nand_odom_publisher = self.create_publisher( - Odometry, "SC/nav/NAND_odom", 1 - ) - self.nand_gps_fix_publisher = self.create_publisher( - UInt8, "NAND/debug/gps_fix", 1 - ) - self.nand_gps_acc_publisher = self.create_publisher( - Float64, "NAND/debug/gps_accuracy", 1 - ) - self.nand_gps_seqnum_publisher = self.create_publisher( - Int32, "NAND/debug/gps_seqnum", 1 - ) - self.nand_gps_time_publisher = self.create_publisher( - UInt64, "NAND/debug/gps_time", 1 - ) - - # SC SENSOR PUBLISHERS - self.sc_velocity_publisher = self.create_publisher( - Float64, "SC/sensors/velocity", 1 - ) - self.sc_steering_angle_publisher = self.create_publisher( - Float64, "SC/sensors/steering_angle", 1 - ) - # SERIAL DEBUG PUBLISHERS self.roundtrip_time_publisher = self.create_publisher( Float64, self_name + "/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_seqnum_publisher = self.create_publisher( + Int32, "/debug/gps_seqnum", 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, "SC/sensors/velocity", 1 + ) + self.sc_steering_angle_publisher = self.create_publisher( + Float64, "SC/sensors/steering_angle", 1 + ) + + self.observed_nand_odom_publisher = self.create_publisher( + Odometry, "/NAND_raw_state", 1 + ) + def set_alarm(self, msg): """ @@ -135,8 +139,6 @@ def set_steering(self, msg): Steering Angle Updater, updates the steering angle locally if updated on ros stopic """ self.get_logger().debug(f"Read steering 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 @@ -185,8 +187,6 @@ def reader_thread(self): self.get_logger().debug(f'NAND Debug Timestamp: {packet.timestamp}') elif isinstance(packet, NANDUKF): odom = Odometry() - - # NOTE: this data is published as an Odometry object per the BuggyState specs: https://github.com/CMU-Robotics-Club/robobuggy-software/wiki/BuggyState#buggystate-definition-as-a-navodom-ros-msg odom.pose.pose.position.x = packet.easting odom.pose.pose.position.y = packet.northing odom.pose.pose.orientation.z = packet.theta @@ -201,7 +201,6 @@ def reader_thread(self): elif isinstance(packet, NANDRawGPS): odom = Odometry() - # NOTE: this data is published as an Odometry object per the BuggyState specs: https://github.com/CMU-Robotics-Club/robobuggy-software/wiki/BuggyState#buggystate-definition-as-a-navodom-ros-msg odom.pose.pose.position.x = packet.easting odom.pose.pose.position.y = packet.northing odom.pose.pose.orientation.z = 0