Buggy State Converter Rewrite #4
Conversation
| 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) |
There was a problem hiding this comment.
other_raw_state wouldn't be of type SC, it would be a separate type, (take a look at ros2bnyahaj and host2comm from original repo, specifically for the Odometry class, that is what you are converting to.
| 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) |
There was a problem hiding this comment.
might as well take utm_z and use that (doesn't hurt, and no point removing data)
|
|
||
| # 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 |
There was a problem hiding this comment.
quaternion and our internal heading should have the same heading, I wouldn't worry about this.
There was a problem hiding this comment.
Also, take the other variables and it put into the function
| _, _, 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 |
There was a problem hiding this comment.
Yaw is typically orientation.z, orientation.x, and orientation.y will be roll, pitch
There was a problem hiding this comment.
I think the wiki said x so I just followed that, do we want to use z everywhere instead?
| converted_msg.pose.pose.orientation.w = 0.0 | ||
|
|
||
| # ---- 3. Copy Covariances (Unchanged) ---- | ||
| # TODO: check whether covariances should be copied over or not sent at all |
There was a problem hiding this comment.
This is correct, just uncomment it
|
|
||
| # ---- 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 |
There was a problem hiding this comment.
You don't need to ignore it, just copy it on over. more data, isn't a problem necessarily
| converted_msg.pose.pose.position.z = 0.0 # ignored | ||
|
|
||
| # ---- 2. Orientation in Radians with 0 at East ---- | ||
| converted_msg.pose.pose.orientation.x = msg.pose.pose.orientation.x |
There was a problem hiding this comment.
Again oreitnation.z but correct idea
| converted_msg.pose.pose.orientation.w = 0.0 | ||
|
|
||
| # ---- 3. Copy Covariances (Unchanged) ---- | ||
| # TODO: check whether covariances should be copied over or not sent at all |
There was a problem hiding this comment.
We dont' get covariancs, so they will be initialized to whatever, don't comment it
| return converted_msg | ||
|
|
||
| # replaced custom function with tf_transformations library function | ||
| def quaternion_to_yaw(self, qx, qy, qz, qw): |
There was a problem hiding this comment.
appreciate you using your own function, but I'm unsure if it's correct. Can you instead use the function in this library from tf.transformations import euler_from_quaternion
There was a problem hiding this comment.
yeah, it’s using the library, that’s just some old code from earlier, I’ll remove it
| 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.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 |
What type of PR is this? (check all applicable)
Description
Rewrote/wrote a state converter node to convert
raw_stateto cleanstatefor both SC and NAND. Benefits: unit and format conversion not done in arbitrary files anymore.Related Tickets & Documents
QA Instructions, Screenshots, Recordings
Test this node by checking if the node converts raw values exactly as they were converted in pre-refactor code.
Added/updated tests?
We encourage you to keep the code coverage percentage at 80% and above.
NAND/self,SC/self, andSC/otherhave not been included
[optional] Are there any post deployment tasks we need to perform?
raw_statetopics have to be created and published to[optional] What gif best describes this PR or how it makes you feel?