From 6763e9d764b6d6dd5f79876a185e4ae9444864c2 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Mon, 24 Nov 2025 16:37:03 +0100 Subject: [PATCH] fix(rename & split): Rename node and split the simulator functionality into seperate class --- src/g2_2025_odometry_pkg/config/opt.yaml | 2 +- .../nodes/imu_data_simulator.cpp | 188 ------------------ .../main.cpp | 2 +- .../nodes/imu_data_simulator.cpp | 62 ++++++ .../nodes/imu_data_simulator.hpp | 28 +++ .../src/simulator/Simulator.cpp | 141 +++++++++++++ .../Simulator.hpp} | 31 ++- 7 files changed, 245 insertions(+), 209 deletions(-) delete mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.cpp rename src/g2_2025_odometry_pkg/src/{g2_2025_data_simulator_node => g2_2025_imu_data_simulator_node}/main.cpp (67%) create mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp create mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.hpp create mode 100644 src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp rename src/g2_2025_odometry_pkg/src/{g2_2025_data_simulator_node/nodes/imu_data_simulator.hpp => simulator/Simulator.hpp} (54%) diff --git a/src/g2_2025_odometry_pkg/config/opt.yaml b/src/g2_2025_odometry_pkg/config/opt.yaml index 70419e4..1f9c221 100644 --- a/src/g2_2025_odometry_pkg/config/opt.yaml +++ b/src/g2_2025_odometry_pkg/config/opt.yaml @@ -1,4 +1,4 @@ -data_simulator: +imu_data_simulator: ros__parameters: publish_rate: 10.0 linear_x: diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.cpp deleted file mode 100644 index 88e0f03..0000000 --- a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.cpp +++ /dev/null @@ -1,188 +0,0 @@ -#include "imu_data_simulator.hpp" - -namespace assignments::three::imu_data_simulator_node { - -DataSimulator::DataSimulator() : Node("imu_data_simulator") { - RCLCPP_INFO(this->get_logger(), "DataSimulator node created"); - - start_time_ = this->now(); - - this->declare_parameter("publish_rate", 10.0); - double rate = this->get_parameter("publish_rate").as_double(); - this->declare_parameter("max_intervals", 4); - max_intervals_ = this->get_parameter("max_intervals").as_int(); - - axes_ = { "linear_x", "linear_y", "linear_z", "angular_x", "angular_y", "angular_z" }; - - for (const auto& axis : axes_) { - // Declare number of intervals - this->declare_parameter(axis + ".num_intervals", 0); - int num_intervals = this->get_parameter(axis + ".num_intervals").as_int(); - - RCLCPP_INFO(this->get_logger(), "Loading %d intervals for axis '%s'", num_intervals, axis.c_str()); - - std::vector intervals; - - // Load up to 4 intervals - for (int i = 0; i < std::min(num_intervals, max_intervals_); i++) { - std::string prefix = axis + ".interval_" + std::to_string(i); - - this->declare_parameter(prefix + ".type", "constant"); - this->declare_parameter(prefix + ".t_start", 0.0); - this->declare_parameter(prefix + ".t_end", 0.0); - this->declare_parameter(prefix + ".y_start", 0.0); - this->declare_parameter(prefix + ".y_end", 0.0); - this->declare_parameter(prefix + ".t_mid", 0.0); - this->declare_parameter(prefix + ".y_mid", 0.0); - - IntervalConfig config; - std::string type_str = this->get_parameter(prefix + ".type").as_string(); - if (type_str == "constant") { - config.type = SimType::CONSTANT; - } else if (type_str == "linear") { - config.type = SimType::LINEAR; - } else if (type_str == "quadratic") { - config.type = SimType::QUADRATIC; - } else { - RCLCPP_WARN(this->get_logger(), "Unknown interval type '%s', defaulting to 'constant'", type_str.c_str()); - config.type = SimType::CONSTANT; - } - config.t_start = this->get_parameter(prefix + ".t_start").as_double(); - config.t_end = this->get_parameter(prefix + ".t_end").as_double(); - config.y_start = this->get_parameter(prefix + ".y_start").as_double(); - config.y_end = this->get_parameter(prefix + ".y_end").as_double(); - config.t_mid = this->get_parameter(prefix + ".t_mid").as_double(); - config.y_mid = this->get_parameter(prefix + ".y_mid").as_double(); - - intervals.push_back(config); - } - - // Log loaded intervals - for (size_t j = 0; j < intervals.size(); ++j) { - const auto& cfg = intervals[j]; - const char* type_str = (cfg.type == SimType::CONSTANT) ? "constant" : - (cfg.type == SimType::LINEAR) ? "linear" : "quadratic"; - RCLCPP_INFO(this->get_logger(), - "Axis '%s' Interval %zu: type='%s', t_start=%.6f, t_end=%.6f, y_start=%.6f, y_end=%.6f, t_mid=%.6f, y_mid=%.6f", - axis.c_str(), j, type_str, - cfg.t_start, cfg.t_end, cfg.y_start, cfg.y_end, cfg.t_mid, cfg.y_mid - ); - } - - axis_intervals_[axis] = intervals; - } - - imu_publisher_ = this->create_publisher("simulated_imu_data", 10); - RCLCPP_INFO(this->get_logger(), "Created IMU Publisher on topic 'simulated_imu_data'"); - - // Use publish_rate parameter - int timer_ms = static_cast(1000.0 / rate); - RCLCPP_INFO(this->get_logger(), "Publishing at %.1f Hz (every %d ms)", rate, timer_ms); - - timer_ = this->create_wall_timer( - std::chrono::milliseconds(timer_ms), - std::bind(&DataSimulator::publish_imu_data, this) - ); -} - -DataSimulator::~DataSimulator() { - RCLCPP_INFO(this->get_logger(), "DataSimulator node destroyed"); -} - -void DataSimulator::publish_imu_data() { - auto imu_msg = std::make_shared(); - - imu_msg->header.stamp = this->now(); - imu_msg->header.frame_id = "imu_link"; - - // Calculate elapsed time since node start - double elapsed_time = (this->now() - start_time_).seconds(); - - // Get values for each axis - imu_msg->linear_acceleration.x = get_axis_value("linear_x", elapsed_time); - imu_msg->linear_acceleration.y = get_axis_value("linear_y", elapsed_time); - imu_msg->linear_acceleration.z = get_axis_value("linear_z", elapsed_time); - imu_msg->angular_velocity.x = get_axis_value("angular_x", elapsed_time); - imu_msg->angular_velocity.y = get_axis_value("angular_y", elapsed_time); - imu_msg->angular_velocity.z = get_axis_value("angular_z", elapsed_time); - - RCLCPP_INFO(this->get_logger(), - "t=%.2fs - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]", - elapsed_time, - imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z, - imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z - ); - - imu_publisher_->publish(*imu_msg); -} - -double DataSimulator::compute_value(double t, const IntervalConfig& interval) { - // Check if time is within interval - if (t < interval.t_start || t > interval.t_end) { - return 0.0; - } - - switch (interval.type) { - case SimType::CONSTANT: - // Constant: y = c - return interval.y_start; - - case SimType::LINEAR: { - // Linear interpolation: y = y0 + (y1 - y0) * (t - t0) / (t1 - t0) - double t0 = interval.t_start; - double t1 = interval.t_end; - double y0 = interval.y_start; - double y1 = interval.y_end; - - double L0 = (t - t1) / (t0 - t1); - double L1 = (t - t0) / (t1 - t0); - - return y0 * L0 + y1 * L1; - // if (t1 - t0 == 0.0) return y0; - // return y0 + (y1 - y0) * (t - t0) / (t1 - t0); - } - - case SimType::QUADRATIC: { - // Quadratic using Lagrange interpolation through 3 points - double t0 = interval.t_start; - double t1 = interval.t_mid; - double t2 = interval.t_end; - double y0 = interval.y_start; - double y1 = interval.y_mid; - double y2 = interval.y_end; - - double L0 = ((t - t1) * (t - t2)) / ((t0 - t1) * (t0 - t2)); - double L1 = ((t - t0) * (t - t2)) / ((t1 - t0) * (t1 - t2)); - double L2 = ((t - t0) * (t - t1)) / ((t2 - t0) * (t2 - t1)); - - return y0 * L0 + y1 * L1 + y2 * L2; - } - } - - return 0.0; -} - -double DataSimulator::get_axis_value(const std::string& axis, double t) { - // Check if axis exists in configuration - auto it = axis_intervals_.find(axis); - if (it == axis_intervals_.end()) { - return 0.0; // Default value if axis not configured - } - - // Check each interval for this axis - double last_value = 0.0; - for (const auto& interval : it->second) { - if (t >= interval.t_start && t <= interval.t_end) { - // Currently in this interval - return compute_value(t, interval); - } else if (t > interval.t_end) { - // Past this interval - remember its end value for holding - last_value = compute_value(interval.t_end, interval); - } - } - - // Not in any interval - return last known value (or 0.0 if before all intervals) - return last_value; -} - -} // namespace assignments::three::imu_data_simulator_node diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/main.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/main.cpp similarity index 67% rename from src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/main.cpp rename to src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/main.cpp index 1a2a739..5203b24 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/main.cpp +++ b/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/main.cpp @@ -4,7 +4,7 @@ int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::spin(node); rclcpp::shutdown(); diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp new file mode 100644 index 0000000..7a9976e --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp @@ -0,0 +1,62 @@ +#include "imu_data_simulator.hpp" + +namespace assignments::three::data_simulator_node { + +DataSimulator::DataSimulator() : Node("imu_data_simulator") { + RCLCPP_INFO(this->get_logger(), "DataSimulator node created"); + + start_time_ = this->now(); + + this->declare_parameter("publish_rate", 10.0); + double rate = this->get_parameter("publish_rate").as_double(); + + axes_ = { "linear_x", "linear_y", "linear_z", "angular_x", "angular_y", "angular_z" }; + + // Simulator loads parameters itself + simulator_ = std::make_unique(this, axes_); + + imu_publisher_ = this->create_publisher("simulated_imu_data", 10); + RCLCPP_INFO(this->get_logger(), "Created IMU Publisher on topic 'simulated_imu_data'"); + + // Use publish_rate parameter + int timer_ms = static_cast(1000.0 / rate); + RCLCPP_INFO(this->get_logger(), "Publishing at %.1f Hz (every %d ms)", rate, timer_ms); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(timer_ms), + std::bind(&DataSimulator::publish_imu_data, this) + ); +} + +DataSimulator::~DataSimulator() { + RCLCPP_INFO(this->get_logger(), "DataSimulator node destroyed"); +} + +void DataSimulator::publish_imu_data() { + auto imu_msg = std::make_shared(); + + imu_msg->header.stamp = this->now(); + imu_msg->header.frame_id = "imu_link"; + + // Calculate elapsed time since node start + double elapsed_time = (this->now() - start_time_).seconds(); + + // Get values for each axis + imu_msg->linear_acceleration.x = simulator_->get_axis_value("linear_x", elapsed_time); + imu_msg->linear_acceleration.y = simulator_->get_axis_value("linear_y", elapsed_time); + imu_msg->linear_acceleration.z = simulator_->get_axis_value("linear_z", elapsed_time); + imu_msg->angular_velocity.x = simulator_->get_axis_value("angular_x", elapsed_time); + imu_msg->angular_velocity.y = simulator_->get_axis_value("angular_y", elapsed_time); + imu_msg->angular_velocity.z = simulator_->get_axis_value("angular_z", elapsed_time); + + RCLCPP_INFO(this->get_logger(), + "t=%.2fs - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]", + elapsed_time, + imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z, + imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z + ); + + imu_publisher_->publish(*imu_msg); +} + +} // namespace assignments::three::data_simulator_node diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.hpp b/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.hpp new file mode 100644 index 0000000..a66d4f7 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "simulator/Simulator.hpp" + + +namespace assignments::three::data_simulator_node { + +using assignments::three::Simulator; + +class DataSimulator : public rclcpp::Node { +public: + DataSimulator(); + ~DataSimulator(); +private: + rclcpp::Publisher::SharedPtr imu_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Time start_time_; + + std::vector axes_; + std::unique_ptr simulator_; + + void publish_imu_data(); + +}; + +} // namespace assignments::three::data_simulator_node diff --git a/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp b/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp new file mode 100644 index 0000000..8957cbf --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp @@ -0,0 +1,141 @@ +#include "Simulator.hpp" +#include + +namespace assignments::three +{ + +Simulator::Simulator(rclcpp::Node* node, const std::vector& axes) { + load_intervals(node, axes); +} + +Simulator::~Simulator() { +} + +void Simulator::load_intervals(rclcpp::Node* node, const std::vector& axes) { + node->declare_parameter("max_intervals", 4); + max_intervals_ = node->get_parameter("max_intervals").as_int(); + + for (const auto& axis : axes) { + node->declare_parameter(axis + ".num_intervals", 0); + int num_intervals = node->get_parameter(axis + ".num_intervals").as_int(); + + RCLCPP_INFO(node->get_logger(), "Loading %d intervals for axis '%s'", num_intervals, axis.c_str()); + + std::vector intervals; + + for (int i = 0; i < std::min(num_intervals, max_intervals_); i++) { + std::string prefix = axis + ".interval_" + std::to_string(i); + + node->declare_parameter(prefix + ".type", "constant"); + node->declare_parameter(prefix + ".t_start", 0.0); + node->declare_parameter(prefix + ".t_end", 0.0); + node->declare_parameter(prefix + ".y_start", 0.0); + node->declare_parameter(prefix + ".y_end", 0.0); + node->declare_parameter(prefix + ".t_mid", 0.0); + node->declare_parameter(prefix + ".y_mid", 0.0); + + IntervalConfig config; + std::string type_str = node->get_parameter(prefix + ".type").as_string(); + if (type_str == "constant") { + config.type = SimType::CONSTANT; + } else if (type_str == "linear") { + config.type = SimType::LINEAR; + } else if (type_str == "quadratic") { + config.type = SimType::QUADRATIC; + } else { + RCLCPP_WARN(node->get_logger(), "Unknown interval type '%s', defaulting to 'constant'", type_str.c_str()); + config.type = SimType::CONSTANT; + } + config.t_start = node->get_parameter(prefix + ".t_start").as_double(); + config.t_end = node->get_parameter(prefix + ".t_end").as_double(); + config.y_start = node->get_parameter(prefix + ".y_start").as_double(); + config.y_end = node->get_parameter(prefix + ".y_end").as_double(); + config.t_mid = node->get_parameter(prefix + ".t_mid").as_double(); + config.y_mid = node->get_parameter(prefix + ".y_mid").as_double(); + + intervals.push_back(config); + } + + for (size_t j = 0; j < intervals.size(); ++j) { + const auto& cfg = intervals[j]; + const char* type_str = (cfg.type == SimType::CONSTANT) ? "constant" : + (cfg.type == SimType::LINEAR) ? "linear" : "quadratic"; + RCLCPP_INFO(node->get_logger(), + "Axis '%s' Interval %zu: type='%s', t_start=%.6f, t_end=%.6f, y_start=%.6f, y_end=%.6f, t_mid=%.6f, y_mid=%.6f", + axis.c_str(), j, type_str, + cfg.t_start, cfg.t_end, cfg.y_start, cfg.y_end, cfg.t_mid, cfg.y_mid + ); + } + + axis_intervals_[axis] = intervals; + } +} + +double Simulator::compute_value(double t, const IntervalConfig& interval) { + // Check if time is within interval + if (t < interval.t_start || t > interval.t_end) { + return 0.0; + } + + switch (interval.type) { + case SimType::CONSTANT: + // Constant: y = c + return interval.y_start; + + case SimType::LINEAR: { + // Linear interpolation: y = y0 + (y1 - y0) * (t - t0) / (t1 - t0) + double t0 = interval.t_start; + double t1 = interval.t_end; + double y0 = interval.y_start; + double y1 = interval.y_end; + + double L0 = (t - t1) / (t0 - t1); + double L1 = (t - t0) / (t1 - t0); + + return y0 * L0 + y1 * L1; + } + + case SimType::QUADRATIC: { + // Quadratic using Lagrange interpolation through 3 points + double t0 = interval.t_start; + double t1 = interval.t_mid; + double t2 = interval.t_end; + double y0 = interval.y_start; + double y1 = interval.y_mid; + double y2 = interval.y_end; + + double L0 = ((t - t1) * (t - t2)) / ((t0 - t1) * (t0 - t2)); + double L1 = ((t - t0) * (t - t2)) / ((t1 - t0) * (t1 - t2)); + double L2 = ((t - t0) * (t - t1)) / ((t2 - t0) * (t2 - t1)); + + return y0 * L0 + y1 * L1 + y2 * L2; + } + } + + return 0.0; +} + +double Simulator::get_axis_value(const std::string& axis, double t) { + // Check if axis exists in configuration + auto it = axis_intervals_.find(axis); + if (it == axis_intervals_.end()) { + return 0.0; // Default value if axis not configured + } + + // Check each interval for this axis + double last_value = 0.0; + for (const auto& interval : it->second) { + if (t >= interval.t_start && t <= interval.t_end) { + // Currently in this interval + return compute_value(t, interval); + } else if (t > interval.t_end) { + // Past this interval - remember its end value for holding + last_value = compute_value(interval.t_end, interval); + } + } + + // Not in any interval - return last known value (or 0.0 if before all intervals) + return last_value; +} + +} // namespace assignments::three diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.hpp b/src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp similarity index 54% rename from src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.hpp rename to src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp index 92f4694..fd56c7b 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.hpp +++ b/src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp @@ -1,12 +1,11 @@ #pragma once - -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/imu.hpp" +#include #include #include #include -namespace assignments::three::data_simulator_node { +namespace assignments::three +{ enum class SimType { CONSTANT, @@ -24,26 +23,20 @@ struct IntervalConfig { double y_mid; // For quadratic }; -class DataSimulator : public rclcpp::Node { +class Simulator { public: - DataSimulator(); - ~DataSimulator(); + Simulator(rclcpp::Node* node, const std::vector& axes); + ~Simulator(); + + double get_axis_value(const std::string& axis, double t); + private: int max_intervals_; - rclcpp::Publisher::SharedPtr imu_publisher_; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Time start_time_; - - std::vector axes_; - std::map> axis_intervals_; - - - void publish_imu_data(); - void parse_type(); + void load_intervals(rclcpp::Node* node, const std::vector& axes); double compute_value(double t, const IntervalConfig& interval); - double get_axis_value(const std::string& axis, double t); + std::map> axis_intervals_; }; -} // namespace assignments::three::data_simulator_node +} // namespace assignments::three