diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index 4fc2f1d..b45f94b 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -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 ) @@ -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.") @@ -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 @@ -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):