Add imu assembly error publisher#217
Closed
CQin-Cat wants to merge 2 commits into
Closed
Conversation
There was a problem hiding this comment.
Pull request overview
Adds an IMU assembly error publisher to the orientation controller so roll/pitch (and derived yaw component) assembly errors can be computed from TF and published on a dedicated topic.
Changes:
- Initialize a realtime publisher for
rm_msgs::AssemblyErrorDataonimu_assembly_error. - Call a new publishing routine from the controller update loop to compute/publish assembly error.
- Extend the controller header with new includes, method declaration, and state members for publishing.
Reviewed changes
Copilot reviewed 2 out of 2 changed files in this pull request and generated 4 comments.
| File | Description |
|---|---|
| rm_orientation_controller/src/orientation_controller.cpp | Adds publisher initialization, update-loop call, and the AssemblyErrorPub computation/publish implementation. |
| rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h | Declares the new publisher, loop counter, and AssemblyErrorPub method; adds required message/publisher includes. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
Comment on lines
+108
to
+111
| catch (tf2::TransformException& ex) | ||
| { | ||
| ROS_WARN("%s", ex.what()); | ||
| } |
Comment on lines
+115
to
+118
| tf2::Matrix3x3(target2imu.getRotation()).getRPY(roll_imu, pitch_imu, yaw_imu); | ||
| roll_imu = std::round(roll_imu / (M_PI / 2)); | ||
| pitch_imu = std::round(pitch_imu / (M_PI / 2)); | ||
| yaw_imu = std::round(yaw_imu / (M_PI / 2)); |
Comment on lines
+126
to
+130
| error_eigen << roll_error, pitch_error, 0.0; | ||
| assembly_error_eigen = R_eigen.transpose() * error_eigen; | ||
| if (loop_count_ % 100 == 0) | ||
| { | ||
| if (assembly_error_pub_->trylock()) |
| bool getTransform(const ros::Time& time, geometry_msgs::TransformStamped& source2target, const double x, | ||
| const double y, const double z, const double w); | ||
| void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg); | ||
| void AssemblyErrorPub(const ros::Time& time); |
Author
|
I've added the required |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Add AssemblyErrorPub to calculate and publish IMU roll and pitch assembly errors. This function accurately publishes the assembly error if and only if the yaw offset is an integer multiple of 90 degrees.