Skip to content

Commit

Permalink
changed message structure handling
Browse files Browse the repository at this point in the history
  • Loading branch information
@saransh323
saransh323 committed Nov 20, 2024
1 parent 201744a commit 7f92845
Showing 1 changed file with 89 additions and 55 deletions.
144 changes: 89 additions & 55 deletions rb_ws/src/buggy/buggy/buggy_state_converter.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,8 @@ def __init__(self):
)
self.self_state_publisher = self.create_publisher(Odometry, "self/state", 10)

# Check if namespace is "/SC" to add an additional "other" subscriber/publisher
if namespace == "/SC":
# 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
)
Expand All @@ -30,15 +30,15 @@ def __init__(self):
# 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
) # Check UTM EPSG code, using EPSG:32617 for UTM Zone 17N
) # 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":
if namespace == "SC":
converted_msg = self.convert_SC_state(msg)
elif namespace == "/NAND":
elif namespace == "NAND":
converted_msg = self.convert_NAND_state(msg)
else:
self.get_logger().warn("Namespace not recognized for buggy state conversion.")
Expand All @@ -49,12 +49,18 @@ def self_raw_state_callback(self, 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
converted_msg = self.convert_SC_state(msg)
# Convert the SC message and publish 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 /SC namespace raw state to clean state units and structure """
"""
Converts self/raw_state in SC namespace to clean state units and structure
Takes in ROS message in nav_msgs/Odometry format
Assumes that the SC namespace is using ECEF coordinates and quaternion orientation
"""

converted_msg = Odometry()
converted_msg.header = msg.header

Expand All @@ -64,89 +70,117 @@ def convert_SC_state(self, msg):
ecef_z = msg.pose.pose.position.z

# Convert ECEF to UTM
utm_x, utm_y, _ = self.ecef_to_utm_transformer.transform(ecef_x, ecef_y, ecef_z)
utm_x, utm_y, utm_z = self.ecef_to_utm_transformer.transform(ecef_x, ecef_y, ecef_z)
converted_msg.pose.pose.position.x = utm_x # UTM Easting
converted_msg.pose.pose.position.y = utm_y # UTM Northing
converted_msg.pose.pose.position.z = 0.0 # ignored
converted_msg.pose.pose.position.z = utm_z # UTM Altitude

# ---- 2. Convert Quaternion to Heading (Radians) ----

qx, qy, qz, qw = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w

# replaced with tf_transformations library function
# yaw = self.quaternion_to_yaw(qx, qy, qz, qw)
# converted_msg.pose.pose.orientation.x = yaw

# Use euler_from_quaternion to get roll, pitch, yaw
_, _, yaw = euler_from_quaternion([qx, qy, qz, qw])
yaw_aligned_east = yaw - np.pi / 2 # TODO: check if 0 is East
# Store the heading in the x component of the orientation
converted_msg.pose.pose.orientation.x = yaw_aligned_east
roll, pitch, yaw = euler_from_quaternion([qx, qy, qz, qw])

# ignored:
converted_msg.pose.pose.orientation.y = 0.0
converted_msg.pose.pose.orientation.z = 0.0
converted_msg.pose.pose.orientation.w = 0.0
# Store the heading in the x component of the orientation
converted_msg.pose.pose.orientation.x = roll
converted_msg.pose.pose.orientation.y = pitch
converted_msg.pose.pose.orientation.z = yaw
converted_msg.pose.pose.orientation.w = 0.0 # fourth (quaternion) term irrelevant for euler angles

# ---- 3. Copy Covariances (Unchanged) ----
# TODO: check whether covariances should be copied over or not sent at all
# converted_msg.pose.covariance = msg.pose.covariance
# converted_msg.twist.covariance = msg.twist.covariance
converted_msg.pose.covariance = msg.pose.covariance
converted_msg.twist.covariance = msg.twist.covariance

# ---- 4. Copy Linear Velocities ----
converted_msg.twist.twist.linear.x = msg.twist.twist.linear.x # m/s in x-direction
converted_msg.twist.twist.linear.y = msg.twist.twist.linear.y # m/s in x-direction
converted_msg.twist.twist.linear.z = msg.twist.twist.linear.z # Keep original Z velocity (??)
converted_msg.twist.twist.linear.z = msg.twist.twist.linear.z # keep original Z velocity

# ---- 5. Copy Angular Velocities ----
converted_msg.twist.twist.angular.x = msg.twist.twist.angular.z # rad/s for heading change rate (using yaw from twist.angular)
converted_msg.twist.twist.angular.y = 0.0 # ignored
converted_msg.twist.twist.angular.z = 0.0 # ignored
converted_msg.twist.twist.angular.x = msg.twist.twist.angular.x # copying over
converted_msg.twist.twist.angular.y = msg.twist.twist.angular.y # copying over
converted_msg.twist.twist.angular.z = msg.twist.twist.angular.z # rad/s, heading change rate

return converted_msg

def convert_NAND_state(self, msg):
""" Converts /NAND namespace raw state to clean state units and structure """
"""
Converts self/raw_state in NAND namespace to clean state units and structure
Takes in ROS message in nav_msgs/Odometry format
"""

converted_msg = Odometry()
converted_msg.header = msg.header

# ---- 1. Directly use UTM Coordinates ----
converted_msg.pose.pose.position.x = msg.pose.pose.position.x # UTM Easting
converted_msg.pose.pose.position.y = msg.pose.pose.position.y # UTM Northing
converted_msg.pose.pose.position.z = 0.0 # ignored
converted_msg.pose.pose.position.z = msg.pose.pose.position.z # UTM Altitude

# ---- 2. Orientation in Radians with 0 at East ----
# ---- 2. Orientation in Radians ----
converted_msg.pose.pose.orientation.x = msg.pose.pose.orientation.x
# ignored:
converted_msg.pose.pose.orientation.y = 0.0
converted_msg.pose.pose.orientation.z = 0.0
converted_msg.pose.pose.orientation.w = 0.0
converted_msg.pose.pose.orientation.y = msg.pose.pose.orientation.y
converted_msg.pose.pose.orientation.z = msg.pose.pose.orientation.z
converted_msg.pose.pose.orientation.w = 0.0 # fourth (quaternion) term irrelevant for euler angles

# ---- 3. Copy Covariances (Unchanged) ----
# TODO: check whether covariances should be copied over or not sent at all
# converted_msg.pose.covariance = msg.pose.covariance
# converted_msg.twist.covariance = msg.twist.covariance
converted_msg.pose.covariance = msg.pose.covariance
converted_msg.twist.covariance = msg.twist.covariance

# ---- 4. Copy Linear Velocities ----
# TODO: check that ROS serial translator node changes scalar speed to velocity x/y components before pushing to raw_state
converted_msg.twist.twist.linear.x = msg.twist.twist.linear.x # m/s in x-direction
converted_msg.twist.twist.linear.y = msg.twist.twist.linear.y # m/s in y-direction
converted_msg.twist.twist.linear.z = msg.twist.twist.linear.z # Keep original Z velocity (??)
# ---- 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

# Calculate velocity components
converted_msg.twist.twist.linear.x = speed * np.cos(heading) # m/s in x-direction
converted_msg.twist.twist.linear.y = speed * np.sin(heading) # m/s in y-direction
converted_msg.twist.twist.linear.z = 0.0

# ---- 5. Copy Angular Velocities ----
converted_msg.twist.twist.angular.x = msg.twist.twist.angular.x
converted_msg.twist.twist.angular.y = 0.0
converted_msg.twist.twist.angular.z = 0.0
converted_msg.twist.twist.angular.x = msg.twist.twist.angular.x # copying over
converted_msg.twist.twist.angular.y = msg.twist.twist.angular.y # copying over
converted_msg.twist.twist.angular.z = msg.twist.twist.angular.z # rad/s, heading change rate

return converted_msg

def convert_NAND_other_state(self, msg):
""" Converts other/raw_state in SC namespace (NAND data) to clean state units and structure """
converted_msg = Odometry()
converted_msg.header = msg.header

# ---- 1. Directly use UTM Coordinates ----
converted_msg.pose.pose.position.x = msg.x # UTM Easting
converted_msg.pose.pose.position.y = msg.y # UTM Northing
converted_msg.pose.pose.position.z = msg.z # UTM Altitude -- not provided in other/raw_state

# ---- 2. Orientation in Radians ----
converted_msg.pose.pose.orientation.x = msg.roll # roll not provided in other/raw_state
converted_msg.pose.pose.orientation.y = msg.pitch # pitch not provided in other/raw_state
converted_msg.pose.pose.orientation.z = msg.heading # heading in radians
converted_msg.pose.pose.orientation.w = 0.0 # fourth (quaternion) term irrelevant for euler angles

# ---- 3. Copy Covariances (Unchanged) ----
converted_msg.pose.covariance = msg.pose_covariance # not provided in other/raw_state
converted_msg.twist.covariance = msg.twist_covariance # not provided in other/raw_state

# ---- 4. Linear Velocities in m/s ----
# Convert scalar speed to velocity x/y components using heading (msg.heading)
speed = msg.speed # m/s scalar velocity
heading = msg.heading # heading in radians

# Calculate velocity components
converted_msg.twist.twist.linear.x = speed * np.cos(heading) # m/s in x-direction
converted_msg.twist.twist.linear.y = speed * np.sin(heading) # m/s in y-direction
converted_msg.twist.twist.linear.z = 0.0

# replaced custom function with tf_transformations library function
def quaternion_to_yaw(self, qx, qy, qz, qw):
"""Convert a quaternion to yaw (heading) in radians."""
siny_cosp = 2.0 * (qw * qz + qx * qy)
cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) # assumes quaternion is normalized, should be true in ROS,
# else use (qw * qw + qx * qx - qy * qy - qz * qz)
return np.arctan2(siny_cosp, cosy_cosp)
# ---- 5. Angular Velocities ----
converted_msg.twist.twist.angular.x = msg.roll_rate # roll rate not provided in other/raw_state
converted_msg.twist.twist.angular.y = msg.pitch_rate # pitch rate not provided in other/raw_state
converted_msg.twist.twist.angular.z = msg.heading_rate # rad/s, heading change rate

return converted_msg


def main(args=None):
Expand Down

0 comments on commit 7f92845

Please sign in to comment.