Skip to content

Commit

Permalink
refactor buggy_state_converter to subscribe from the 3 seperate data …
Browse files Browse the repository at this point in the history 
…sources
  • Loading branch information
@TiaSinghania
TiaSinghania committed Jan 9, 2025
1 parent 5d69c19 commit 36c7a97
Show file tree
Hide file tree
Showing 2 changed files with 64 additions and 61 deletions.
54 changes: 29 additions & 25 deletions rb_ws/src/buggy/buggy/buggy_state_converter.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
71 changes: 35 additions & 36 deletions rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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):
"""
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down

0 comments on commit 36c7a97

Please sign in to comment.