Skip to content

Add imu assembly error publisher #217

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
CQin-Cat wants to merge 2 commits into from
+57 −0
Closed

Add imu assembly error publisher #217

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0d944f6
Add imu assembly error publisher
CQin-Cat Jul 17, 2026
938701c
Add code comments
CQin-Cat Jul 18, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,8 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <rm_common/hardware_interface/robot_state_interface.h>
#include <sensor_msgs/Imu.h>
#include <rm_msgs/AssemblyErrorData.h>
#include <realtime_tools/realtime_publisher.h>

namespace rm_orientation_controller
{
Expand All @@ -25,6 +27,7 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
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);

rm_control::RmImuSensorHandle imu_sensor_;
rm_control::RobotStateHandle robot_state_;
Expand All @@ -38,6 +41,8 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
std::string frame_target_;

ros::Subscriber imu_data_sub_;
std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::AssemblyErrorData>> assembly_error_pub_;
bool receive_imu_msg_ = false;
int loop_count_{};
};
} // namespace rm_orientation_controller
52 changes: 52 additions & 0 deletions rm_orientation_controller/src/orientation_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro

tf_broadcaster_.init(root_nh);
imu_data_sub_ = root_nh.subscribe<sensor_msgs::Imu>("data", 1, &Controller::imuDataCallback, this);
assembly_error_pub_.reset(
new realtime_tools::RealtimePublisher<rm_msgs::AssemblyErrorData>(root_nh, "imu_assembly_error", 10));
source2target_msg_.header.frame_id = frame_source_;
source2target_msg_.child_frame_id = frame_target_;
source2target_msg_.transform.rotation.w = 1.0;
Expand All @@ -48,6 +50,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period)
if (!receive_imu_msg_)
tf_broadcaster_.sendTransform(source2target_msg_);
}
AssemblyErrorPub(time); // Accurate only for yaw offsets in multiples of 90 degrees.
}

bool Controller::getTransform(const ros::Time& time, geometry_msgs::TransformStamped& source2target, const double x,
Expand Down Expand Up @@ -90,6 +93,55 @@ void Controller::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg)
tf_broadcaster_.sendTransform(source2target);
}

void Controller::AssemblyErrorPub(const ros::Time& time)
{
double roll_error, pitch_error, yaw_error, roll_imu, pitch_imu, yaw_imu;
geometry_msgs::TransformStamped tf_source2target, tf_target2imu;
tf2::Transform source2target, target2imu;
Eigen::Matrix3d roll_eigen, pitch_eigen, yaw_eigen, R_eigen;
Eigen::Vector3d error_eigen, assembly_error_eigen;
try
{
tf_source2target = robot_state_.lookupTransform(frame_source_, frame_target_, time);
tf_target2imu = robot_state_.lookupTransform(frame_target_, imu_sensor_.getFrameId(), time);
}
catch (tf2::TransformException& ex)
{
ROS_WARN("%s", ex.what());
}
Comment on lines +108 to +111
tf2::fromMsg(tf_source2target.transform, source2target);
tf2::Matrix3x3(source2target.getRotation()).getRPY(roll_error, pitch_error, yaw_error);
tf2::fromMsg(tf_target2imu.transform, target2imu);
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 +115 to +118
roll_eigen << 1, 0, 0, 0, cos(roll_imu * M_PI / 2), -sin(roll_imu * M_PI / 2), 0, sin(roll_imu * M_PI / 2),
cos(roll_imu * M_PI / 2);
pitch_eigen << cos(pitch_imu * M_PI / 2), 0, sin(pitch_imu * M_PI / 2), 0, 1, 0, -sin(pitch_imu * M_PI / 2), 0,
cos(pitch_imu * M_PI / 2);
yaw_eigen << cos(yaw_imu * M_PI / 2), -sin(yaw_imu * M_PI / 2), 0, sin(yaw_imu * M_PI / 2), cos(yaw_imu * M_PI / 2),
0, 0, 0, 1;
R_eigen = roll_eigen * pitch_eigen * yaw_eigen;
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())
Comment on lines +126 to +130
{
assembly_error_pub_->msg_.header.stamp = time;
assembly_error_pub_->msg_.roll_error = assembly_error_eigen[0];
assembly_error_pub_->msg_.pitch_error = assembly_error_eigen[1];
assembly_error_pub_->msg_.yaw_error = assembly_error_eigen[2];
assembly_error_pub_->unlockAndPublish();
}
else
{
ROS_WARN_THROTTLE(1, "Can't publish assembly error data");
}
}
loop_count_++;
}
} // namespace rm_orientation_controller

PLUGINLIB_EXPORT_CLASS(rm_orientation_controller::Controller, controller_interface::ControllerBase)
Loading