Skip to content

refactor(eagleye_rt): convert all nodes to class-based rclcpp::Node #366

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

Open
rsasaki0109 wants to merge 2 commits into
base: develop-ros2
Choose a base branch
from
Open

refactor(eagleye_rt): convert all nodes to class-based rclcpp::Node #366

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
5b26806
refactor(eagleye_rt): convert all nodes to class-based rclcpp::Node p…
rsasaki0109 Apr 4, 2026
6122426
fix: zero-initialize status structs in all class-based nodes
rsasaki0109 Apr 6, 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
2 changes: 1 addition & 1 deletion eagleye_rt/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

if($ENV{ROS_DISTRO} STREQUAL "galactic")
if("$ENV{ROS_DISTRO}" STREQUAL "galactic")
add_definitions(-DROS_DISTRO_GALACTIC)
endif()

Expand Down
118 changes: 65 additions & 53 deletions eagleye_rt/src/angular_velocity_offset_stop_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,71 +32,83 @@
#include "eagleye_coordinate/eagleye_coordinate.hpp"
#include "eagleye_navigation/eagleye_navigation.hpp"

static geometry_msgs::msg::TwistStamped velocity;
rclcpp::Publisher<eagleye_msgs::msg::AngularVelocityOffset>::SharedPtr pub;
static eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop;
static sensor_msgs::msg::Imu imu;
class AngularVelocityOffsetStopNode : public rclcpp::Node
{
public:
AngularVelocityOffsetStopNode() : Node("eagleye_angular_velocity_offset_stop")
{
std::string subscribe_twist_topic_name = "vehicle/twist";

struct AngularVelocityOffsetStopParameter angular_velocity_offset_stop_parameter;
struct AngularVelocityOffsetStopStatus angular_velocity_offset_stop_status;
std::string yaml_file;
this->declare_parameter("yaml_file", yaml_file);
this->get_parameter("yaml_file", yaml_file);
std::cout << "yaml_file: " << yaml_file << std::endl;

void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg)
{
velocity = *msg;
}
try
{
YAML::Node conf = YAML::LoadFile(yaml_file);

void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
{
imu.header = msg->header;
imu.orientation = msg->orientation;
imu.orientation_covariance = msg->orientation_covariance;
imu.angular_velocity = msg->angular_velocity;
imu.angular_velocity_covariance = msg->angular_velocity_covariance;
imu.linear_acceleration = msg->linear_acceleration;
imu.linear_acceleration_covariance = msg->linear_acceleration_covariance;
angular_velocity_offset_stop.header = msg->header;
angular_velocity_offset_stop_estimate(velocity, imu, angular_velocity_offset_stop_parameter, &angular_velocity_offset_stop_status, &angular_velocity_offset_stop);
pub->publish(angular_velocity_offset_stop);
}
angular_velocity_offset_stop_parameter_.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as<double>();
angular_velocity_offset_stop_parameter_.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as<double>();
angular_velocity_offset_stop_parameter_.estimated_interval = conf["/**"]["ros__parameters"]["angular_velocity_offset_stop"]["estimated_interval"].as<double>();
angular_velocity_offset_stop_parameter_.outlier_threshold = conf["/**"]["ros__parameters"]["angular_velocity_offset_stop"]["outlier_threshold"].as<double>();

int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("eagleye_angular_velocity_offset_stop");
std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl;
std::cout << "imu_rate " << angular_velocity_offset_stop_parameter_.imu_rate << std::endl;
std::cout << "stop_judgment_threshold " << angular_velocity_offset_stop_parameter_.stop_judgment_threshold << std::endl;
std::cout << "estimated_minimum_interval " << angular_velocity_offset_stop_parameter_.estimated_interval << std::endl;
std::cout << "outlier_threshold " << angular_velocity_offset_stop_parameter_.outlier_threshold << std::endl;
}
catch (YAML::Exception& e)
{
std::cerr << "\033[1;31mangular_velocity_offset_stop Node YAML Error: " << e.msg << "\033[0m" << std::endl;
exit(3);
}

std::string subscribe_twist_topic_name = "vehicle/twist";
sub1_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
subscribe_twist_topic_name, 1000,
std::bind(&AngularVelocityOffsetStopNode::velocityCallback, this, std::placeholders::_1));
sub2_ = this->create_subscription<sensor_msgs::msg::Imu>(
"imu/data_tf_converted", 1000,
std::bind(&AngularVelocityOffsetStopNode::imuCallback, this, std::placeholders::_1));
pub_ = this->create_publisher<eagleye_msgs::msg::AngularVelocityOffset>("angular_velocity_offset_stop", 1000);
}

std::string yaml_file;
node->declare_parameter("yaml_file",yaml_file);
node->get_parameter("yaml_file",yaml_file);
std::cout << "yaml_file: " << yaml_file << std::endl;
private:
geometry_msgs::msg::TwistStamped velocity_;
eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop_;
sensor_msgs::msg::Imu imu_;

try
{
YAML::Node conf = YAML::LoadFile(yaml_file);
struct AngularVelocityOffsetStopParameter angular_velocity_offset_stop_parameter_;
struct AngularVelocityOffsetStopStatus angular_velocity_offset_stop_status_ = {};

angular_velocity_offset_stop_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as<double>();
angular_velocity_offset_stop_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as<double>();
angular_velocity_offset_stop_parameter.estimated_interval = conf["/**"]["ros__parameters"]["angular_velocity_offset_stop"]["estimated_interval"].as<double>();
angular_velocity_offset_stop_parameter.outlier_threshold = conf["/**"]["ros__parameters"]["angular_velocity_offset_stop"]["outlier_threshold"].as<double>();
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub1_;
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub2_;
rclcpp::Publisher<eagleye_msgs::msg::AngularVelocityOffset>::SharedPtr pub_;

std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl;
std::cout << "imu_rate " << angular_velocity_offset_stop_parameter.imu_rate << std::endl;
std::cout << "stop_judgment_threshold " << angular_velocity_offset_stop_parameter.stop_judgment_threshold << std::endl;
std::cout << "estimated_minimum_interval " << angular_velocity_offset_stop_parameter.estimated_interval << std::endl;
std::cout << "outlier_threshold " << angular_velocity_offset_stop_parameter.outlier_threshold << std::endl;
}
catch (YAML::Exception& e)
void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg)
{
std::cerr << "\033[1;31mangular_velocity_offset_stop Node YAML Error: " << e.msg << "\033[0m" << std::endl;
exit(3);
velocity_ = *msg;
}

auto sub1 = node->create_subscription<geometry_msgs::msg::TwistStamped>(subscribe_twist_topic_name, 1000, velocity_callback);
auto sub2 = node->create_subscription<sensor_msgs::msg::Imu>("imu/data_tf_converted", 1000, imu_callback);
pub = node->create_publisher<eagleye_msgs::msg::AngularVelocityOffset>("angular_velocity_offset_stop", 1000);

rclcpp::spin(node);
void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
{
imu_.header = msg->header;
imu_.orientation = msg->orientation;
imu_.orientation_covariance = msg->orientation_covariance;
imu_.angular_velocity = msg->angular_velocity;
imu_.angular_velocity_covariance = msg->angular_velocity_covariance;
imu_.linear_acceleration = msg->linear_acceleration;
imu_.linear_acceleration_covariance = msg->linear_acceleration_covariance;
angular_velocity_offset_stop_.header = msg->header;
angular_velocity_offset_stop_estimate(velocity_, imu_, angular_velocity_offset_stop_parameter_, &angular_velocity_offset_stop_status_, &angular_velocity_offset_stop_);
pub_->publish(angular_velocity_offset_stop_);
}
};

int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<AngularVelocityOffsetStopNode>());
return 0;
}
141 changes: 80 additions & 61 deletions eagleye_rt/src/correction_imu.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,79 +32,98 @@
#include "eagleye_coordinate/eagleye_coordinate.hpp"
#include "eagleye_navigation/eagleye_navigation.hpp"

rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub;
static eagleye_msgs::msg::YawrateOffset yaw_rate_offset;
static eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop;
static eagleye_msgs::msg::AccXOffset acc_x_offset;
static eagleye_msgs::msg::AccXScaleFactor acc_x_scale_factor;
static sensor_msgs::msg::Imu imu;

static sensor_msgs::msg::Imu correction_imu;


void yaw_rate_offset_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg)
{
yaw_rate_offset = *msg;
}

void angular_velocity_offset_stop_callback(const eagleye_msgs::msg::AngularVelocityOffset::ConstSharedPtr msg)
{
angular_velocity_offset_stop = *msg;
}

void acc_x_offset_callback(const eagleye_msgs::msg::AccXOffset::ConstSharedPtr msg)
{
acc_x_offset = *msg;
}

void acc_x_scale_factor_callback(const eagleye_msgs::msg::AccXScaleFactor::ConstSharedPtr msg)
{
acc_x_scale_factor = *msg;
}

void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
class CorrectionImuNode : public rclcpp::Node
{
imu = *msg;
public:
CorrectionImuNode() : Node("eagleye_correction_imu")
{
sub1_ = this->create_subscription<eagleye_msgs::msg::YawrateOffset>(
"yaw_rate_offset_2nd", rclcpp::QoS(10),
std::bind(&CorrectionImuNode::yawRateOffsetCallback, this, std::placeholders::_1));
sub2_ = this->create_subscription<eagleye_msgs::msg::AngularVelocityOffset>(
"angular_velocity_offset_stop", rclcpp::QoS(10),
std::bind(&CorrectionImuNode::angularVelocityOffsetStopCallback, this, std::placeholders::_1));
sub3_ = this->create_subscription<eagleye_msgs::msg::AccXOffset>(
"acc_x_offset", rclcpp::QoS(10),
std::bind(&CorrectionImuNode::accXOffsetCallback, this, std::placeholders::_1));
sub4_ = this->create_subscription<eagleye_msgs::msg::AccXScaleFactor>(
"acc_x_scale_factor", rclcpp::QoS(10),
std::bind(&CorrectionImuNode::accXScaleFactorCallback, this, std::placeholders::_1));
sub5_ = this->create_subscription<sensor_msgs::msg::Imu>(
"imu/data_tf_converted", 1000,
std::bind(&CorrectionImuNode::imuCallback, this, std::placeholders::_1));
pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/data_corrected", rclcpp::QoS(10));
}

correction_imu.header = imu.header;
correction_imu.orientation = imu.orientation;
correction_imu.orientation_covariance = imu.orientation_covariance;
correction_imu.angular_velocity_covariance = imu.angular_velocity_covariance;
correction_imu.linear_acceleration_covariance = imu.linear_acceleration_covariance;
private:
eagleye_msgs::msg::YawrateOffset yaw_rate_offset_;
eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop_;
eagleye_msgs::msg::AccXOffset acc_x_offset_;
eagleye_msgs::msg::AccXScaleFactor acc_x_scale_factor_;
sensor_msgs::msg::Imu imu_;
sensor_msgs::msg::Imu correction_imu_;

rclcpp::Subscription<eagleye_msgs::msg::YawrateOffset>::SharedPtr sub1_;
rclcpp::Subscription<eagleye_msgs::msg::AngularVelocityOffset>::SharedPtr sub2_;
rclcpp::Subscription<eagleye_msgs::msg::AccXOffset>::SharedPtr sub3_;
rclcpp::Subscription<eagleye_msgs::msg::AccXScaleFactor>::SharedPtr sub4_;
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub5_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_;

void yawRateOffsetCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg)
{
yaw_rate_offset_ = *msg;
}

if (acc_x_offset.status.enabled_status == true && acc_x_scale_factor.status.enabled_status)
void angularVelocityOffsetStopCallback(const eagleye_msgs::msg::AngularVelocityOffset::ConstSharedPtr msg)
{
correction_imu.linear_acceleration.x = imu.linear_acceleration.x * acc_x_scale_factor.acc_x_scale_factor + acc_x_offset.acc_x_offset;
correction_imu.linear_acceleration.y = imu.linear_acceleration.y;
correction_imu.linear_acceleration.z = imu.linear_acceleration.z;
angular_velocity_offset_stop_ = *msg;
}
else

void accXOffsetCallback(const eagleye_msgs::msg::AccXOffset::ConstSharedPtr msg)
{
correction_imu.linear_acceleration.x = imu.linear_acceleration.x;
correction_imu.linear_acceleration.y = imu.linear_acceleration.y;
correction_imu.linear_acceleration.z = imu.linear_acceleration.z;
acc_x_offset_ = *msg;
}

correction_imu.angular_velocity.x = imu.angular_velocity.x + angular_velocity_offset_stop.angular_velocity_offset.x;
correction_imu.angular_velocity.y = imu.angular_velocity.y + angular_velocity_offset_stop.angular_velocity_offset.y;
correction_imu.angular_velocity.z = -1 * (imu.angular_velocity.z + angular_velocity_offset_stop.angular_velocity_offset.z);
void accXScaleFactorCallback(const eagleye_msgs::msg::AccXScaleFactor::ConstSharedPtr msg)
{
acc_x_scale_factor_ = *msg;
}

pub->publish(correction_imu);
}
void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
{
imu_ = *msg;

correction_imu_.header = imu_.header;
correction_imu_.orientation = imu_.orientation;
correction_imu_.orientation_covariance = imu_.orientation_covariance;
correction_imu_.angular_velocity_covariance = imu_.angular_velocity_covariance;
correction_imu_.linear_acceleration_covariance = imu_.linear_acceleration_covariance;

if (acc_x_offset_.status.enabled_status == true && acc_x_scale_factor_.status.enabled_status)
{
correction_imu_.linear_acceleration.x = imu_.linear_acceleration.x * acc_x_scale_factor_.acc_x_scale_factor + acc_x_offset_.acc_x_offset;
correction_imu_.linear_acceleration.y = imu_.linear_acceleration.y;
correction_imu_.linear_acceleration.z = imu_.linear_acceleration.z;
}
else
{
correction_imu_.linear_acceleration.x = imu_.linear_acceleration.x;
correction_imu_.linear_acceleration.y = imu_.linear_acceleration.y;
correction_imu_.linear_acceleration.z = imu_.linear_acceleration.z;
}

correction_imu_.angular_velocity.x = imu_.angular_velocity.x + angular_velocity_offset_stop_.angular_velocity_offset.x;
correction_imu_.angular_velocity.y = imu_.angular_velocity.y + angular_velocity_offset_stop_.angular_velocity_offset.y;
correction_imu_.angular_velocity.z = -1 * (imu_.angular_velocity.z + angular_velocity_offset_stop_.angular_velocity_offset.z);

pub_->publish(correction_imu_);
}
};

int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("eagleye_correction_imu");

auto sub1 = node->create_subscription<eagleye_msgs::msg::YawrateOffset>("yaw_rate_offset_2nd", rclcpp::QoS(10), yaw_rate_offset_callback); //ros::TransportHints().tcpNoDelay()
auto sub2 = node->create_subscription<eagleye_msgs::msg::AngularVelocityOffset>("angular_velocity_offset_stop", rclcpp::QoS(10), angular_velocity_offset_stop_callback); //ros::TransportHints().tcpNoDelay()
auto sub3 = node->create_subscription<eagleye_msgs::msg::AccXOffset>("acc_x_offset", rclcpp::QoS(10), acc_x_offset_callback); //ros::TransportHints().tcpNoDelay()
auto sub4 = node->create_subscription<eagleye_msgs::msg::AccXScaleFactor>("acc_x_scale_factor", rclcpp::QoS(10), acc_x_scale_factor_callback); //ros::TransportHints().tcpNoDelay()
auto sub5 = node->create_subscription<sensor_msgs::msg::Imu>("imu/data_tf_converted", 1000, imu_callback); //ros::TransportHints().tcpNoDelay()
pub = node->create_publisher<sensor_msgs::msg::Imu>("imu/data_corrected", rclcpp::QoS(10));

rclcpp::spin(node);

rclcpp::spin(std::make_shared<CorrectionImuNode>());
return 0;
}
Loading
Loading