From c6175fe0440728251b2ebf8d66c9141f24278e1e Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Mon, 24 Nov 2025 10:00:30 +0100 Subject: [PATCH 01/14] feat(data_sim): Add boiler plate node --- .../src/g2_2025_data_simulator_node/main.cpp | 13 +++++++ .../nodes/data_simulator.cpp | 37 +++++++++++++++++++ .../nodes/data_simulator.hpp | 19 ++++++++++ 3 files changed, 69 insertions(+) create mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/main.cpp create mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.cpp create mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.hpp 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_data_simulator_node/main.cpp new file mode 100644 index 0000000..556b109 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/main.cpp @@ -0,0 +1,13 @@ +#include "rclcpp/rclcpp.hpp" +#include "nodes/data_simulator.hpp" + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.cpp new file mode 100644 index 0000000..3cac78e --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.cpp @@ -0,0 +1,37 @@ +#include "data_simulator.hpp" + +namespace assignments::three::data_simulator_node { + +DataSimulator::DataSimulator() { + RCLCPP_INFO(this->get_logger(), "DataSimulator node created"); + + imu_publisher_ = this->create_publisher("simulated_imu_data", 10); + RCLCPP_INFO(this->get_logger(), "Created IMU Publisher on topic 'simulated_imu_data'"); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), + 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"; + + RCLCPP_INFO(this->get_logger(), + "Publishing Simulated IMU Data - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]", + 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_data_simulator_node/nodes/data_simulator.hpp b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.hpp new file mode 100644 index 0000000..c76c386 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" + +namespace assignments::three::data_simulator_node { + +class DataSimulator : public rclcpp::Node { +public: + DataSimulator(); + ~DataSimulator(); +private: + rclcpp::Publisher::SharedPtr imu_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + + void publish_imu_data(); +}; + +} // namespace assignments::three::data_simulator_node -- 2.39.5 From 334af2da8206fd295114c87f085277222044ccab Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Mon, 24 Nov 2025 15:20:33 +0100 Subject: [PATCH 02/14] fix(main): Fix main starting wrong node --- src/g2_2025_odometry_pkg/CMakeLists.txt | 13 +++++++++++++ .../src/g2_2025_data_simulator_node/main.cpp | 2 +- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/g2_2025_odometry_pkg/CMakeLists.txt b/src/g2_2025_odometry_pkg/CMakeLists.txt index 9e902f2..f8d467d 100644 --- a/src/g2_2025_odometry_pkg/CMakeLists.txt +++ b/src/g2_2025_odometry_pkg/CMakeLists.txt @@ -10,6 +10,19 @@ find_package(ament_cmake REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +add_executable(data_simulator_node src/g2_2025_data_simulator_node/nodes/data_simulator.cpp src/g2_2025_data_simulator_node/main.cpp) +# add_executable(data_simulator_node src/g2_2025_data_simulator_node/nodes/temp.cpp) + +ament_target_dependencies(data_simulator_node rclcpp sensor_msgs geometry_msgs) + +install(TARGETS + data_simulator_node + DESTINATION lib/${PROJECT_NAME} +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) 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_data_simulator_node/main.cpp index 556b109..b377748 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/main.cpp +++ b/src/g2_2025_odometry_pkg/src/g2_2025_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(); -- 2.39.5 From 6525c6a1fb0fc929f967883e5ae9f012f5b0af8c Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Mon, 24 Nov 2025 15:34:32 +0100 Subject: [PATCH 03/14] fix(data_sim): Add param file, add working implementation --- src/g2_2025_odometry_pkg/config/opt.yaml | 33 ++++ .../nodes/data_simulator.cpp | 157 +++++++++++++++++- .../nodes/data_simulator.hpp | 30 ++++ 3 files changed, 217 insertions(+), 3 deletions(-) create mode 100644 src/g2_2025_odometry_pkg/config/opt.yaml diff --git a/src/g2_2025_odometry_pkg/config/opt.yaml b/src/g2_2025_odometry_pkg/config/opt.yaml new file mode 100644 index 0000000..70419e4 --- /dev/null +++ b/src/g2_2025_odometry_pkg/config/opt.yaml @@ -0,0 +1,33 @@ +data_simulator: + ros__parameters: + publish_rate: 10.0 + linear_x: + num_intervals: 2 + interval_0: + type: "linear" + t_start: 0.0 + t_end: 5.0 + y_start: 0.0 + y_end: 9.81 + interval_1: + type: "linear" + t_start: 10.0 + t_end: 15.0 + y_start: 9.81 + y_end: 0.0 + angular_z: + num_intervals: 1 + interval_0: + type: "constant" + t_start: 2.0 + y_start: 1.57 + angular_x: + num_intervals: 1 + interval_0: + type: "quadratic" + t_start: 3.0 + y_start: 0.785 + t_mid: 4.5 + y_mid: 1.57 + t_end: 6.0 + y_end: 0.0 diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.cpp index 3cac78e..c0e0911 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.cpp +++ b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.cpp @@ -2,14 +2,85 @@ namespace assignments::three::data_simulator_node { -DataSimulator::DataSimulator() { +DataSimulator::DataSimulator() : Node("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(100), + std::chrono::milliseconds(timer_ms), std::bind(&DataSimulator::publish_imu_data, this) ); } @@ -24,14 +95,94 @@ void DataSimulator::publish_imu_data() { 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(), - "Publishing Simulated IMU Data - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]", + "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::data_simulator_node diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.hpp b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.hpp index c76c386..92f4694 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.hpp +++ b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.hpp @@ -2,18 +2,48 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/imu.hpp" +#include +#include +#include namespace assignments::three::data_simulator_node { +enum class SimType { + CONSTANT, + LINEAR, + QUADRATIC +}; + +struct IntervalConfig { + SimType type; + double t_start; + double t_end; + double y_start; + double y_end; + double t_mid; // For quadratic + double y_mid; // For quadratic +}; + class DataSimulator : public rclcpp::Node { public: DataSimulator(); ~DataSimulator(); 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(); + double compute_value(double t, const IntervalConfig& interval); + double get_axis_value(const std::string& axis, double t); + }; } // namespace assignments::three::data_simulator_node -- 2.39.5 From 085bb2f80b1dbb6e52c4ff9878f16480bfe99912 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Mon, 24 Nov 2025 16:12:58 +0100 Subject: [PATCH 04/14] fix(imu_data_simulator): Rename node --- src/g2_2025_odometry_pkg/CMakeLists.txt | 7 +++---- .../src/g2_2025_data_simulator_node/main.cpp | 4 ++-- .../nodes/{data_simulator.cpp => imu_data_simulator.cpp} | 8 ++++---- .../nodes/{data_simulator.hpp => imu_data_simulator.hpp} | 0 4 files changed, 9 insertions(+), 10 deletions(-) rename src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/{data_simulator.cpp => imu_data_simulator.cpp} (97%) rename src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/{data_simulator.hpp => imu_data_simulator.hpp} (100%) diff --git a/src/g2_2025_odometry_pkg/CMakeLists.txt b/src/g2_2025_odometry_pkg/CMakeLists.txt index f8d467d..372a89c 100644 --- a/src/g2_2025_odometry_pkg/CMakeLists.txt +++ b/src/g2_2025_odometry_pkg/CMakeLists.txt @@ -14,13 +14,12 @@ find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -add_executable(data_simulator_node src/g2_2025_data_simulator_node/nodes/data_simulator.cpp src/g2_2025_data_simulator_node/main.cpp) -# add_executable(data_simulator_node src/g2_2025_data_simulator_node/nodes/temp.cpp) +add_executable(imu_data_simulator_node src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp src/g2_2025_imu_data_simulator_node/main.cpp) -ament_target_dependencies(data_simulator_node rclcpp sensor_msgs geometry_msgs) +ament_target_dependencies(imu_data_simulator_node rclcpp sensor_msgs geometry_msgs) install(TARGETS - data_simulator_node + imu_data_simulator_node DESTINATION lib/${PROJECT_NAME} ) 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_data_simulator_node/main.cpp index b377748..1a2a739 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/main.cpp +++ b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/main.cpp @@ -1,10 +1,10 @@ #include "rclcpp/rclcpp.hpp" -#include "nodes/data_simulator.hpp" +#include "nodes/imu_data_simulator.hpp" 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_data_simulator_node/nodes/data_simulator.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.cpp similarity index 97% rename from src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.cpp rename to src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.cpp index c0e0911..88e0f03 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.cpp +++ b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.cpp @@ -1,8 +1,8 @@ -#include "data_simulator.hpp" +#include "imu_data_simulator.hpp" -namespace assignments::three::data_simulator_node { +namespace assignments::three::imu_data_simulator_node { -DataSimulator::DataSimulator() : Node("data_simulator") { +DataSimulator::DataSimulator() : Node("imu_data_simulator") { RCLCPP_INFO(this->get_logger(), "DataSimulator node created"); start_time_ = this->now(); @@ -185,4 +185,4 @@ double DataSimulator::get_axis_value(const std::string& axis, double t) { return last_value; } -} // namespace assignments::three::data_simulator_node +} // namespace assignments::three::imu_data_simulator_node diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.hpp b/src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.hpp similarity index 100% rename from src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.hpp rename to src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.hpp -- 2.39.5 From 6763e9d764b6d6dd5f79876a185e4ae9444864c2 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Mon, 24 Nov 2025 16:37:03 +0100 Subject: [PATCH 05/14] 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 -- 2.39.5 From aef18c137080648c99074ad326c2841f5d655997 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Mon, 24 Nov 2025 16:39:19 +0100 Subject: [PATCH 06/14] fix(debug): Change sim logger to debug logger --- src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp b/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp index 8957cbf..dcc8470 100644 --- a/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp +++ b/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp @@ -60,7 +60,7 @@ void Simulator::load_intervals(rclcpp::Node* node, const std::vectorget_logger(), + RCLCPP_DEBUG(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 -- 2.39.5 From d06a33b90c579d7f77f6a553f38ac6a8fa316543 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Mon, 24 Nov 2025 16:53:27 +0100 Subject: [PATCH 07/14] feat(wheel_data_simulator): Add wheel data simulator --- src/g2_2025_odometry_pkg/CMakeLists.txt | 24 +++++++- src/g2_2025_odometry_pkg/config/opt.yaml | 20 ++++++ .../main.cpp | 13 ++++ .../nodes/wheel_data_simulator.cpp | 61 +++++++++++++++++++ .../nodes/wheel_data_simulator.hpp | 33 ++++++++++ 5 files changed, 150 insertions(+), 1 deletion(-) create mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp create mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp create mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp diff --git a/src/g2_2025_odometry_pkg/CMakeLists.txt b/src/g2_2025_odometry_pkg/CMakeLists.txt index 372a89c..ee39bda 100644 --- a/src/g2_2025_odometry_pkg/CMakeLists.txt +++ b/src/g2_2025_odometry_pkg/CMakeLists.txt @@ -13,13 +13,35 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) -add_executable(imu_data_simulator_node src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp src/g2_2025_imu_data_simulator_node/main.cpp) +add_executable(imu_data_simulator_node + src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp + src/g2_2025_imu_data_simulator_node/main.cpp + src/simulator/Simulator.cpp) + +target_include_directories(imu_data_simulator_node PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_imu_data_simulator_node +) ament_target_dependencies(imu_data_simulator_node rclcpp sensor_msgs geometry_msgs) +add_executable(wheel_data_simulator_node + src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp + src/g2_2025_wheel_data_simulator_node/main.cpp + src/simulator/Simulator.cpp) + +target_include_directories(wheel_data_simulator_node PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_wheel_data_simulator_node +) + +ament_target_dependencies(wheel_data_simulator_node rclcpp std_msgs) + install(TARGETS imu_data_simulator_node + wheel_data_simulator_node DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/g2_2025_odometry_pkg/config/opt.yaml b/src/g2_2025_odometry_pkg/config/opt.yaml index 1f9c221..e414018 100644 --- a/src/g2_2025_odometry_pkg/config/opt.yaml +++ b/src/g2_2025_odometry_pkg/config/opt.yaml @@ -31,3 +31,23 @@ imu_data_simulator: y_mid: 1.57 t_end: 6.0 y_end: 0.0 + +wheel_data_simulator: + ros__parameters: + publish_rate: 10.0 + wheel_fl: + num_intervals: 1 + interval_0: + type: "linear" + t_start: 0.0 + t_end: 10.0 + y_start: 0.0 + y_end: 5.0 + wheel_fr: + num_intervals: 1 + interval_0: + type: "linear" + t_start: 0.0 + t_end: 10.0 + y_start: 0.0 + y_end: 10.0 diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp new file mode 100644 index 0000000..060d515 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp @@ -0,0 +1,13 @@ +#include "rclcpp/rclcpp.hpp" +#include "nodes/wheel_data_simulator.hpp" + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp new file mode 100644 index 0000000..80f6c6f --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp @@ -0,0 +1,61 @@ +#include "wheel_data_simulator.hpp" + +namespace assignments::three::wheel_data_simulator_node { + +DataSimulator::DataSimulator() : Node("wheel_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(); + + wheels_ = { "wheel_fl", "wheel_fr", "wheel_rl", "wheel_rr" }; + + // Simulator loads parameters itself + simulator_ = std::make_unique(this, wheels_); + + wheel_publisher_ = this->create_publisher("simulated_wheel_data", 10); + RCLCPP_INFO(this->get_logger(), "Created wheel Publisher on topic 'simulated_wheel_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_wheel_data, this) + ); +} + +DataSimulator::~DataSimulator() { + RCLCPP_INFO(this->get_logger(), "DataSimulator node destroyed"); +} + +void DataSimulator::publish_wheel_data() { + auto wheel_msg = std::make_shared(); + + // wheel_msg->header.stamp = this->now(); + // wheel_msg->header.frame_id = "wheel_link"; + + // Calculate elapsed time since node start + double elapsed_time = (this->now() - start_time_).seconds(); + + // For now, just log wheel values (adjust based on your actual message type) + wheel_msg->data = { + simulator_->get_axis_value("wheel_fl", elapsed_time), + simulator_->get_axis_value("wheel_fr", elapsed_time), + simulator_->get_axis_value("wheel_rl", elapsed_time), + simulator_->get_axis_value("wheel_rr", elapsed_time) + }; + + RCLCPP_INFO(this->get_logger(), + "t=%.2fs - Wheel Data [%.3f, %.3f, %.3f, %.3f]", + elapsed_time, + wheel_msg->data[0], wheel_msg->data[1], wheel_msg->data[2], wheel_msg->data[3] + ); + + wheel_publisher_->publish(*wheel_msg); +} + +} // namespace assignments::three::wheel_data_simulator_node diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp new file mode 100644 index 0000000..b3f0012 --- /dev/null +++ b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include "simulator/Simulator.hpp" +#include +#include +#include +#include + +namespace assignments::three::wheel_data_simulator_node { + +using assignments::three::Simulator; +using assignments::three::IntervalConfig; +using assignments::three::SimType; + +class DataSimulator : public rclcpp::Node { +public: + DataSimulator(); + ~DataSimulator(); +private: + rclcpp::Publisher::SharedPtr wheel_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Time start_time_; + + std::vector wheels_; + std::unique_ptr simulator_; + + void publish_wheel_data(); + +}; + +} // namespace assignments::three::wheel_data_simulator_node -- 2.39.5 From 2728baf8a5b3b31c2d3194eeaec7ee32700f6ce2 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Wed, 26 Nov 2025 15:20:38 +0100 Subject: [PATCH 08/14] fix(rename): Rename left over axis variables to object variables --- .../nodes/imu_data_simulator.cpp | 12 +++---- .../nodes/wheel_data_simulator.cpp | 11 +++--- .../src/simulator/Simulator.cpp | 35 +++++++++---------- .../src/simulator/Simulator.hpp | 8 ++--- 4 files changed, 31 insertions(+), 35 deletions(-) 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 index 7a9976e..87b5739 100644 --- 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 @@ -42,12 +42,12 @@ void DataSimulator::publish_imu_data() { 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); + imu_msg->linear_acceleration.x = simulator_->get_object_value("linear_x", elapsed_time); + imu_msg->linear_acceleration.y = simulator_->get_object_value("linear_y", elapsed_time); + imu_msg->linear_acceleration.z = simulator_->get_object_value("linear_z", elapsed_time); + imu_msg->angular_velocity.x = simulator_->get_object_value("angular_x", elapsed_time); + imu_msg->angular_velocity.y = simulator_->get_object_value("angular_y", elapsed_time); + imu_msg->angular_velocity.z = simulator_->get_object_value("angular_z", elapsed_time); RCLCPP_INFO(this->get_logger(), "t=%.2fs - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]", diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp index 80f6c6f..1176a0d 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp +++ b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp @@ -35,18 +35,15 @@ DataSimulator::~DataSimulator() { void DataSimulator::publish_wheel_data() { auto wheel_msg = std::make_shared(); - // wheel_msg->header.stamp = this->now(); - // wheel_msg->header.frame_id = "wheel_link"; - // Calculate elapsed time since node start double elapsed_time = (this->now() - start_time_).seconds(); // For now, just log wheel values (adjust based on your actual message type) wheel_msg->data = { - simulator_->get_axis_value("wheel_fl", elapsed_time), - simulator_->get_axis_value("wheel_fr", elapsed_time), - simulator_->get_axis_value("wheel_rl", elapsed_time), - simulator_->get_axis_value("wheel_rr", elapsed_time) + simulator_->get_object_value("wheel_fl", elapsed_time), + simulator_->get_object_value("wheel_fr", elapsed_time), + simulator_->get_object_value("wheel_rl", elapsed_time), + simulator_->get_object_value("wheel_rr", elapsed_time) }; RCLCPP_INFO(this->get_logger(), diff --git a/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp b/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp index dcc8470..e0cb883 100644 --- a/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp +++ b/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp @@ -4,27 +4,26 @@ namespace assignments::three { -Simulator::Simulator(rclcpp::Node* node, const std::vector& axes) { - load_intervals(node, axes); +Simulator::Simulator(rclcpp::Node* node, const std::vector& objects) { + load_intervals(node, objects); } Simulator::~Simulator() { } -void Simulator::load_intervals(rclcpp::Node* node, const std::vector& axes) { +void Simulator::load_intervals(rclcpp::Node* node, const std::vector& objects) { 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()); + for (const auto& object : objects) { + node->declare_parameter(object + ".num_intervals", 0); + int num_intervals = node->get_parameter(object + ".num_intervals").as_int(); + RCLCPP_INFO(node->get_logger(), "Loading %d intervals for object '%s'", num_intervals, object.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); + std::string prefix = object + ".interval_" + std::to_string(i); node->declare_parameter(prefix + ".type", "constant"); node->declare_parameter(prefix + ".t_start", 0.0); @@ -61,13 +60,13 @@ void Simulator::load_intervals(rclcpp::Node* node, const std::vectorget_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, + "Object '%s' Interval %zu: type='%s', t_start=%.6f, t_end=%.6f, y_start=%.6f, y_end=%.6f, t_mid=%.6f, y_mid=%.6f", + object.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; + object_intervals_[object] = intervals; } } @@ -115,14 +114,14 @@ double Simulator::compute_value(double t, const IntervalConfig& interval) { 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 +double Simulator::get_object_value(const std::string& object, double t) { + // Check if object exists in configuration + auto it = object_intervals_.find(object); + if (it == object_intervals_.end()) { + return 0.0; // Default value if object not configured } - // Check each interval for this axis + // Check each interval for this object double last_value = 0.0; for (const auto& interval : it->second) { if (t >= interval.t_start && t <= interval.t_end) { diff --git a/src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp b/src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp index fd56c7b..f0a48c5 100644 --- a/src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp +++ b/src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp @@ -25,18 +25,18 @@ struct IntervalConfig { class Simulator { public: - Simulator(rclcpp::Node* node, const std::vector& axes); + Simulator(rclcpp::Node* node, const std::vector& objects); ~Simulator(); - double get_axis_value(const std::string& axis, double t); + double get_object_value(const std::string& object, double t); private: int max_intervals_; - void load_intervals(rclcpp::Node* node, const std::vector& axes); + void load_intervals(rclcpp::Node* node, const std::vector& objects); double compute_value(double t, const IntervalConfig& interval); - std::map> axis_intervals_; + std::map> object_intervals_; }; } // namespace assignments::three -- 2.39.5 From ca2c08eb102ed77aad03928353a39f82f59d55b0 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Wed, 26 Nov 2025 15:21:02 +0100 Subject: [PATCH 09/14] feat(launch_files): Add launch files --- src/g2_2025_odometry_pkg/CMakeLists.txt | 4 ++++ src/g2_2025_odometry_pkg/launch/imu_sim.launch.xml | 8 ++++++++ src/g2_2025_odometry_pkg/launch/wheel_sim.launch.xml | 8 ++++++++ 3 files changed, 20 insertions(+) create mode 100644 src/g2_2025_odometry_pkg/launch/imu_sim.launch.xml create mode 100644 src/g2_2025_odometry_pkg/launch/wheel_sim.launch.xml diff --git a/src/g2_2025_odometry_pkg/CMakeLists.txt b/src/g2_2025_odometry_pkg/CMakeLists.txt index ee39bda..9defd0d 100644 --- a/src/g2_2025_odometry_pkg/CMakeLists.txt +++ b/src/g2_2025_odometry_pkg/CMakeLists.txt @@ -45,6 +45,10 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME}/ +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/src/g2_2025_odometry_pkg/launch/imu_sim.launch.xml b/src/g2_2025_odometry_pkg/launch/imu_sim.launch.xml new file mode 100644 index 0000000..4203962 --- /dev/null +++ b/src/g2_2025_odometry_pkg/launch/imu_sim.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/g2_2025_odometry_pkg/launch/wheel_sim.launch.xml b/src/g2_2025_odometry_pkg/launch/wheel_sim.launch.xml new file mode 100644 index 0000000..8e82567 --- /dev/null +++ b/src/g2_2025_odometry_pkg/launch/wheel_sim.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + -- 2.39.5 From 9eba61eb6ed914252169ebdc6197dbeed07bf724 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Wed, 26 Nov 2025 16:12:33 +0100 Subject: [PATCH 10/14] fix(simulator): Add exception when two intervals overlap --- .../src/g2_2025_imu_data_simulator_node/main.cpp | 11 ++++++++--- .../src/g2_2025_wheel_data_simulator_node/main.cpp | 11 ++++++++--- .../src/simulator/Simulator.cpp | 14 ++++++++++++++ 3 files changed, 30 insertions(+), 6 deletions(-) diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/main.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/main.cpp index 5203b24..27f8111 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/main.cpp +++ b/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/main.cpp @@ -4,10 +4,15 @@ int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + try { + auto node = std::make_shared(); + rclcpp::spin(node); + } catch (const std::exception& e) { + RCLCPP_ERROR(rclcpp::get_logger("imu_data_simulator"), "Failed to initialize node: %s", e.what()); + rclcpp::shutdown(); + return 1; + } - rclcpp::spin(node); rclcpp::shutdown(); - return 0; } diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp index 060d515..19a744e 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp +++ b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp @@ -4,10 +4,15 @@ int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + try { + auto node = std::make_shared(); + rclcpp::spin(node); + } catch (const std::exception& e) { + RCLCPP_ERROR(rclcpp::get_logger("wheel_data_simulator"), "Failed to initialize node: %s", e.what()); + rclcpp::shutdown(); + return 1; + } - rclcpp::spin(node); rclcpp::shutdown(); - return 0; } diff --git a/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp b/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp index e0cb883..41dab25 100644 --- a/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp +++ b/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp @@ -64,6 +64,20 @@ void Simulator::load_intervals(rclcpp::Node* node, const std::vector other.t_start); + if (overlaps) { + RCLCPP_ERROR(node->get_logger(), + "Object '%s': Interval %zu [%.2f, %.2f] overlaps with Interval %zu [%.2f, %.2f]", + object.c_str(), j, cfg.t_start, cfg.t_end, k, other.t_start, other.t_end + ); + throw std::runtime_error("Overlapping intervals detected for object '" + object + "'"); + } + } } object_intervals_[object] = intervals; -- 2.39.5 From 1a3d92158db50abb7ce5ef7794bdd5eb0e089968 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Wed, 26 Nov 2025 16:13:00 +0100 Subject: [PATCH 11/14] feat(tests): Add gtests for each node and simulator class --- src/g2_2025_odometry_pkg/CMakeLists.txt | 33 ++- src/g2_2025_odometry_pkg/package.xml | 1 + .../test/test_imu_simulator.cpp | 171 ++++++++++++ .../test/test_simulator.cpp | 257 ++++++++++++++++++ .../test/test_wheel_simulator.cpp | 168 ++++++++++++ 5 files changed, 621 insertions(+), 9 deletions(-) create mode 100644 src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp create mode 100644 src/g2_2025_odometry_pkg/test/test_simulator.cpp create mode 100644 src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp diff --git a/src/g2_2025_odometry_pkg/CMakeLists.txt b/src/g2_2025_odometry_pkg/CMakeLists.txt index 9defd0d..274b8f1 100644 --- a/src/g2_2025_odometry_pkg/CMakeLists.txt +++ b/src/g2_2025_odometry_pkg/CMakeLists.txt @@ -50,15 +50,30 @@ install(DIRECTORY launch config ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + + # Simulator unit tests + ament_add_gtest(test_simulator test/test_simulator.cpp src/simulator/Simulator.cpp) + target_include_directories(test_simulator PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src) + ament_target_dependencies(test_simulator rclcpp) + + # IMU simulator integration tests + ament_add_gtest(test_imu_simulator test/test_imu_simulator.cpp + src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp + src/simulator/Simulator.cpp) + target_include_directories(test_imu_simulator PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_imu_data_simulator_node) + ament_target_dependencies(test_imu_simulator rclcpp sensor_msgs geometry_msgs) + + # Wheel simulator integration tests + ament_add_gtest(test_wheel_simulator test/test_wheel_simulator.cpp + src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp + src/simulator/Simulator.cpp) + target_include_directories(test_wheel_simulator PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_wheel_data_simulator_node) + ament_target_dependencies(test_wheel_simulator rclcpp std_msgs) endif() ament_package() diff --git a/src/g2_2025_odometry_pkg/package.xml b/src/g2_2025_odometry_pkg/package.xml index b9e4d06..de60dca 100644 --- a/src/g2_2025_odometry_pkg/package.xml +++ b/src/g2_2025_odometry_pkg/package.xml @@ -11,6 +11,7 @@ ament_lint_auto ament_lint_common + ament_cmake_gtest ament_cmake diff --git a/src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp b/src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp new file mode 100644 index 0000000..894549b --- /dev/null +++ b/src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp @@ -0,0 +1,171 @@ +#include +#include +#include +#include "g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.hpp" +#include + +using namespace assignments::three::data_simulator_node; + +class IMUSimulatorTest : public ::testing::Test { +protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + void setupBasicIMUParams(rclcpp::Node* node) { + node->declare_parameter("max_intervals", 4); + node->declare_parameter("publish_rate", 10.0); + + // Setup linear_x + node->declare_parameter("linear_x.num_intervals", 1); + node->declare_parameter("linear_x.interval_0.type", "constant"); + node->declare_parameter("linear_x.interval_0.t_start", 0.0); + node->declare_parameter("linear_x.interval_0.t_end", 5.0); + node->declare_parameter("linear_x.interval_0.y_start", 1.0); + node->declare_parameter("linear_x.interval_0.y_end", 0.0); + node->declare_parameter("linear_x.interval_0.t_mid", 0.0); + node->declare_parameter("linear_x.interval_0.y_mid", 0.0); + + // Setup other axes with no intervals + for (const auto& axis : { "linear_y", "linear_z", "angular_x", "angular_y", "angular_z" }) { + node->declare_parameter(std::string(axis) + ".num_intervals", 0); + } + } +}; + +// Test node initialization +TEST_F(IMUSimulatorTest, NodeInitialization) { + auto node = std::make_shared("test_imu_init_node"); + setupBasicIMUParams(node.get()); + + // Should not throw + EXPECT_NO_THROW({ + DataSimulator sim; + }); +} + +// Test IMU message publishing +TEST_F(IMUSimulatorTest, MessagePublishing) { + auto test_node = std::make_shared("test_imu_pub_node"); + + sensor_msgs::msg::Imu::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_imu_data", + 10, + [&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) { + received_msg = msg; + } + ); + + auto sim_node = std::make_shared(); + + // Spin a few times to allow publishing + for (int i = 0; i < 5; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + // Check that we received a message + ASSERT_NE(received_msg, nullptr); +} + +// Test IMU message frame_id +TEST_F(IMUSimulatorTest, MessageFrameId) { + auto test_node = std::make_shared("test_imu_frame_node"); + + sensor_msgs::msg::Imu::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_imu_data", + 10, + [&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) { + received_msg = msg; + } + ); + + auto sim_node = std::make_shared(); + + // Spin a few times to allow publishing + for (int i = 0; i < 5; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + ASSERT_NE(received_msg, nullptr); + EXPECT_EQ(received_msg->header.frame_id, "imu_link"); +} + +// Test linear acceleration values +TEST_F(IMUSimulatorTest, LinearAccelerationValues) { + auto test_node = std::make_shared("test_imu_linear_node"); + + sensor_msgs::msg::Imu::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_imu_data", + 10, + [&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) { + if (!received_msg) { // Only capture first message + received_msg = msg; + } + } + ); + + auto sim_node = std::make_shared(); + + // Spin until we receive a message + for (int i = 0; i < 10 && !received_msg; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + ASSERT_NE(received_msg, nullptr); + + // Values should be set according to configuration + // Since we don't control exact timing, just verify structure + EXPECT_TRUE(std::isfinite(received_msg->linear_acceleration.x)); + EXPECT_TRUE(std::isfinite(received_msg->linear_acceleration.y)); + EXPECT_TRUE(std::isfinite(received_msg->linear_acceleration.z)); +} + +// Test angular velocity values +TEST_F(IMUSimulatorTest, AngularVelocityValues) { + auto test_node = std::make_shared("test_imu_angular_node"); + + sensor_msgs::msg::Imu::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_imu_data", + 10, + [&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) { + if (!received_msg) { + received_msg = msg; + } + } + ); + + auto sim_node = std::make_shared(); + + // Spin until we receive a message + for (int i = 0; i < 10 && !received_msg; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + ASSERT_NE(received_msg, nullptr); + + // Values should be set according to configuration + EXPECT_TRUE(std::isfinite(received_msg->angular_velocity.x)); + EXPECT_TRUE(std::isfinite(received_msg->angular_velocity.y)); + EXPECT_TRUE(std::isfinite(received_msg->angular_velocity.z)); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/g2_2025_odometry_pkg/test/test_simulator.cpp b/src/g2_2025_odometry_pkg/test/test_simulator.cpp new file mode 100644 index 0000000..dbcf1a7 --- /dev/null +++ b/src/g2_2025_odometry_pkg/test/test_simulator.cpp @@ -0,0 +1,257 @@ +#include +#include +#include "simulator/Simulator.hpp" +#include + +using namespace assignments::three; + +class SimulatorTest : public ::testing::Test { +protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + // Helper to create a node with parameter overrides + std::shared_ptr createNodeWithParams( + const std::string& name, + const std::vector& params) + { + rclcpp::NodeOptions options; + options.parameter_overrides(params); + return std::make_shared(name, options); + } +}; + +// Test constant interval +TEST_F(SimulatorTest, ConstantInterval) { + auto node = createNodeWithParams("test_constant_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 1), + rclcpp::Parameter("test_channel.interval_0.type", "constant"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); // Test values within interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 0.0), 5.0); + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 5.0); + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 10.0), 5.0); + + // Test value after interval holds + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 5.0); +} + +// Test linear interval +TEST_F(SimulatorTest, LinearInterval) { + auto node = createNodeWithParams("test_linear_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 1), + rclcpp::Parameter("test_channel.interval_0.type", "linear"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); + + // Test linear interpolation + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 0.0), 0.0); + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 5.0); + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 10.0), 10.0); + + // Test value after interval holds + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 10.0); +} + +// Test quadratic interval +TEST_F(SimulatorTest, QuadraticInterval) { + auto node = createNodeWithParams("test_quadratic_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 1), + rclcpp::Parameter("test_channel.interval_0.type", "quadratic"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 10.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); + + // Test quadratic interpolation + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 0.0), 0.0); + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 10.0); + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 10.0), 0.0); + + // Test value after interval holds + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 0.0); +} + +// Test multiple intervals +TEST_F(SimulatorTest, MultipleIntervals) { + auto node = createNodeWithParams("test_multiple_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 2), + // First interval: constant 5.0 from t=0 to t=5 + rclcpp::Parameter("test_channel.interval_0.type", "constant"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0), + // Second interval: constant 10.0 from t=10 to t=15 + rclcpp::Parameter("test_channel.interval_1.type", "constant"), + rclcpp::Parameter("test_channel.interval_1.t_start", 10.0), + rclcpp::Parameter("test_channel.interval_1.t_end", 15.0), + rclcpp::Parameter("test_channel.interval_1.y_start", 10.0), + rclcpp::Parameter("test_channel.interval_1.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_1.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_1.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); + + // Test first interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 2.5), 5.0); + + // Test gap between intervals - should hold value from first interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 7.5), 5.0); + + // Test second interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 12.5), 10.0); + + // Test after all intervals - should hold value from last interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 20.0), 10.0); +} + +// Test value before any interval +TEST_F(SimulatorTest, ValueBeforeIntervals) { + auto node = createNodeWithParams("test_before_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 1), + rclcpp::Parameter("test_channel.interval_0.type", "constant"), + rclcpp::Parameter("test_channel.interval_0.t_start", 10.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 20.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); + + // Before any interval starts, should return 0.0 + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 0.0); +} + +// Test non-existent channel +TEST_F(SimulatorTest, NonExistentChannel) { + auto node = createNodeWithParams("test_nonexistent_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 1), + rclcpp::Parameter("test_channel.interval_0.type", "constant"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); + + // Query non-existent channel should return 0.0 + EXPECT_DOUBLE_EQ(sim.get_object_value("nonexistent_channel", 5.0), 0.0); +} + +// Test overlapping intervals detection +TEST_F(SimulatorTest, OverlappingIntervalsThrowsException) { + auto node = createNodeWithParams("test_overlap_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 2), + // First interval: t=0 to t=10 + rclcpp::Parameter("test_channel.interval_0.type", "constant"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0), + // Second interval: t=5 to t=15 (overlaps with first) + rclcpp::Parameter("test_channel.interval_1.type", "constant"), + rclcpp::Parameter("test_channel.interval_1.t_start", 5.0), + rclcpp::Parameter("test_channel.interval_1.t_end", 15.0), + rclcpp::Parameter("test_channel.interval_1.y_start", 10.0), + rclcpp::Parameter("test_channel.interval_1.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_1.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_1.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + + // Should throw exception due to overlapping intervals + EXPECT_THROW({ + Simulator sim(node.get(), channels); + }, std::runtime_error); +} + +// Test max_intervals limit +TEST_F(SimulatorTest, MaxIntervalsLimit) { + auto node = createNodeWithParams("test_max_intervals_node", { + rclcpp::Parameter("max_intervals", 2), + rclcpp::Parameter("test_channel.num_intervals", 3), + rclcpp::Parameter("test_channel.interval_0.type", "constant"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 1.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0), + rclcpp::Parameter("test_channel.interval_1.type", "constant"), + rclcpp::Parameter("test_channel.interval_1.t_start", 10.0), + rclcpp::Parameter("test_channel.interval_1.t_end", 20.0), + rclcpp::Parameter("test_channel.interval_1.y_start", 2.0), + rclcpp::Parameter("test_channel.interval_1.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_1.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_1.y_mid", 0.0), + rclcpp::Parameter("test_channel.interval_2.type", "constant"), + rclcpp::Parameter("test_channel.interval_2.t_start", 20.0), + rclcpp::Parameter("test_channel.interval_2.t_end", 30.0), + rclcpp::Parameter("test_channel.interval_2.y_start", 3.0), + rclcpp::Parameter("test_channel.interval_2.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_2.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_2.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); + + // Only first 2 intervals should be loaded + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 1.0); // First interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 2.0); // Second interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 25.0), 2.0); // Third interval should not exist, holds second +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp b/src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp new file mode 100644 index 0000000..e7a2674 --- /dev/null +++ b/src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp @@ -0,0 +1,168 @@ +#include +#include +#include +#include "g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp" +#include + +using namespace assignments::three::wheel_data_simulator_node; + +class WheelSimulatorTest : public ::testing::Test { +protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + void setupBasicWheelParams(rclcpp::Node* node) { + node->declare_parameter("max_intervals", 4); + node->declare_parameter("publish_rate", 10.0); + + // Setup wheel_fl + node->declare_parameter("wheel_fl.num_intervals", 1); + node->declare_parameter("wheel_fl.interval_0.type", "linear"); + node->declare_parameter("wheel_fl.interval_0.t_start", 0.0); + node->declare_parameter("wheel_fl.interval_0.t_end", 10.0); + node->declare_parameter("wheel_fl.interval_0.y_start", 0.0); + node->declare_parameter("wheel_fl.interval_0.y_end", 5.0); + node->declare_parameter("wheel_fl.interval_0.t_mid", 0.0); + node->declare_parameter("wheel_fl.interval_0.y_mid", 0.0); + + // Setup other wheels with no intervals + for (const auto& wheel : {"wheel_fr", "wheel_bl", "wheel_br"}) { + node->declare_parameter(std::string(wheel) + ".num_intervals", 0); + } + } +}; + +// Test node initialization +TEST_F(WheelSimulatorTest, NodeInitialization) { + auto node = std::make_shared("test_wheel_init_node"); + setupBasicWheelParams(node.get()); + + // Should not throw + EXPECT_NO_THROW({ + DataSimulator sim; + }); +} + +// Test wheel message publishing +TEST_F(WheelSimulatorTest, WheelDataPublishing) { + auto test_node = std::make_shared("test_wheel_pub_node"); + + std_msgs::msg::Float64MultiArray::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_wheel_data", + 10, + [&received_msg](std_msgs::msg::Float64MultiArray::SharedPtr msg) { + received_msg = msg; + } + ); + + auto sim_node = std::make_shared(); + + // Spin a few times to allow publishing + for (int i = 0; i < 5; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + // Check that we received a message + ASSERT_NE(received_msg, nullptr); +} + +// Test wheel data array size +TEST_F(WheelSimulatorTest, WheelDataArraySize) { + auto test_node = std::make_shared("test_wheel_size_node"); + + std_msgs::msg::Float64MultiArray::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_wheel_data", + 10, + [&received_msg](std_msgs::msg::Float64MultiArray::SharedPtr msg) { + if (!received_msg) { + received_msg = msg; + } + } + ); + + auto sim_node = std::make_shared(); + + // Spin until we receive a message + for (int i = 0; i < 10 && !received_msg; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + ASSERT_NE(received_msg, nullptr); + // Should have 4 wheel values: FL, FR, RL, RR + EXPECT_EQ(received_msg->data.size(), 4); +} + +// Test wheel velocity values are finite +TEST_F(WheelSimulatorTest, ValidVelocityValues) { + auto test_node = std::make_shared("test_wheel_values_node"); + + std_msgs::msg::Float64MultiArray::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_wheel_data", + 10, + [&received_msg](std_msgs::msg::Float64MultiArray::SharedPtr msg) { + if (!received_msg) { + received_msg = msg; + } + } + ); + + auto sim_node = std::make_shared(); + + // Spin until we receive a message + for (int i = 0; i < 10 && !received_msg; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + ASSERT_NE(received_msg, nullptr); + ASSERT_EQ(received_msg->data.size(), 4); + + // All values should be finite + for (const auto& val : received_msg->data) { + EXPECT_TRUE(std::isfinite(val)); + } +} + +// Test multiple wheel messages received +TEST_F(WheelSimulatorTest, MultipleMessagesReceived) { + auto test_node = std::make_shared("test_multi_wheel_node"); + + int msg_count = 0; + auto subscription = test_node->create_subscription( + "simulated_wheel_data", + 10, + [&msg_count](std_msgs::msg::Float64MultiArray::SharedPtr) { + msg_count++; + } + ); + + auto sim_node = std::make_shared(); + + // Spin for a bit to receive messages + for (int i = 0; i < 10; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + // Should have received at least one message + EXPECT_GT(msg_count, 0); +} + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} -- 2.39.5 From 4d8f84c094f8690c9693f3bf0d1d2b8cee23c920 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Wed, 26 Nov 2025 19:01:30 +0100 Subject: [PATCH 12/14] feat(doc): Add node & test documentation --- .../{nodes => classes}/HardwareInterface.md | 0 doc/architecture/classes/Simulator.md | 119 ++++++++++++++++++ doc/architecture/nodes/IMUDataSimulator.md | 64 ++++++++++ doc/architecture/nodes/WheelDataSimulator.md | 64 ++++++++++ doc/tests/IMUDataSimulator.md | 67 ++++++++++ doc/tests/Simulator.md | 98 +++++++++++++++ doc/tests/WheelDataSimulator.md | 73 +++++++++++ 7 files changed, 485 insertions(+) rename doc/architecture/{nodes => classes}/HardwareInterface.md (100%) create mode 100644 doc/architecture/classes/Simulator.md create mode 100644 doc/architecture/nodes/IMUDataSimulator.md create mode 100644 doc/architecture/nodes/WheelDataSimulator.md create mode 100644 doc/tests/IMUDataSimulator.md create mode 100644 doc/tests/Simulator.md create mode 100644 doc/tests/WheelDataSimulator.md diff --git a/doc/architecture/nodes/HardwareInterface.md b/doc/architecture/classes/HardwareInterface.md similarity index 100% rename from doc/architecture/nodes/HardwareInterface.md rename to doc/architecture/classes/HardwareInterface.md diff --git a/doc/architecture/classes/Simulator.md b/doc/architecture/classes/Simulator.md new file mode 100644 index 0000000..95e56c7 --- /dev/null +++ b/doc/architecture/classes/Simulator.md @@ -0,0 +1,119 @@ +# Simulator (`assignments::three::Simulator`) + +The `Simulator` class provides a flexible time-based value generation engine that supports multiple interpolation types. It is used by both the IMU and Wheel data simulator nodes to generate configurable sensor data patterns. + +## Implementation Details + +**Namespace**: `assignments::three` + +**Header**: `simulator/Simulator.hpp` + +### Data Structures + +**SimType Enum** +```cpp +enum class SimType { + CONSTANT, // y = c (constant value) + LINEAR, // y = y0 + (y1-y0) * (t-t0)/(t1-t0) + QUADRATIC // Lagrange interpolation through 3 points +}; +``` + +**IntervalConfig Struct** +```cpp +struct IntervalConfig { + SimType type; // Interpolation type + double t_start; // Interval start time + double t_end; // Interval end time + double y_start; // Start value + double y_end; // End value + double t_mid; // Mid-point time (quadratic only) + double y_mid; // Mid-point value (quadratic only) +}; +``` + +### Constructor +```cpp +Simulator(rclcpp::Node* node, const std::vector& objects) +``` +- Takes a ROS2 node pointer for parameter access +- Takes a list of object/channel names to configure +- Loads interval configurations from ROS2 parameters +- Validates intervals for overlaps (throws `std::runtime_error` if detected) + +## Core Functionality + +**`double get_object_value(const std::string& object, double t)`** +- Returns the simulated value for a given object at time `t` +- If `t` is within an interval, computes the interpolated value +- If `t` is after all intervals, holds the last interval's end value +- If `t` is before all intervals, returns 0.0 +- If object doesn't exist, returns 0.0 + +**`double compute_value(double t, const IntervalConfig& interval)`** (private) +- Computes the interpolated value based on interval type: + - **CONSTANT**: Returns `y_start` + - **LINEAR**: Lagrange interpolation between 2 points + - **QUADRATIC**: Lagrange interpolation through 3 points + +**`void load_intervals(rclcpp::Node* node, const std::vector& objects)`** (private) +- Declares and loads parameters for each object +- Validates that intervals don't overlap +- Respects `max_intervals` limit + +## Parameter Configuration + +For each object, the following parameters are used: + +| Parameter | Type | Description | +|-----------|------|-------------| +| `max_intervals` | int | Global maximum intervals per object | +| `.num_intervals` | int | Number of intervals for this object | +| `.interval_.type` | string | "constant", "linear", or "quadratic" | +| `.interval_.t_start` | double | Interval start time | +| `.interval_.t_end` | double | Interval end time | +| `.interval_.y_start` | double | Value at start | +| `.interval_.y_end` | double | Value at end | +| `.interval_.t_mid` | double | Mid-point time (quadratic) | +| `.interval_.y_mid` | double | Mid-point value (quadratic) | + +## Example Configuration + +```yaml +# Constant acceleration of 5.0 m/s² from t=0 to t=10 +linear_x: + num_intervals: 1 + interval_0: + type: "constant" + t_start: 0.0 + t_end: 10.0 + y_start: 5.0 + +# Linear ramp from 0 to 10 over 5 seconds +wheel_fl: + num_intervals: 1 + interval_0: + type: "linear" + t_start: 0.0 + t_end: 5.0 + y_start: 0.0 + y_end: 10.0 + +# Quadratic curve peaking at t=5 +angular_z: + num_intervals: 1 + interval_0: + type: "quadratic" + t_start: 0.0 + t_end: 10.0 + y_start: 0.0 + y_end: 0.0 + t_mid: 5.0 + y_mid: 3.14 +``` + +## Error Handling + +- Throws `std::runtime_error` if overlapping intervals are detected for the same object +- Logs warning for unknown interval types, defaults to CONSTANT +- Returns 0.0 for non-existent objects (graceful degradation) \ No newline at end of file diff --git a/doc/architecture/nodes/IMUDataSimulator.md b/doc/architecture/nodes/IMUDataSimulator.md new file mode 100644 index 0000000..298bfd1 --- /dev/null +++ b/doc/architecture/nodes/IMUDataSimulator.md @@ -0,0 +1,64 @@ +# IMUDataSimulator (`assignments::three::data_simulator_node`) + +The `IMUDataSimulator` node generates simulated IMU sensor data (`sensor_msgs/msg/Imu`) based on configurable time-varying intervals. It publishes linear acceleration and angular velocity values that can follow constant, linear, or quadratic trajectories over time. + +## Implementation Details + +**Parameters** + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `publish_rate` | double | 10.0 | Publishing frequency in Hz | +| `max_intervals` | int | 4 | Maximum number of intervals per axis | +| `.num_intervals` | int | 0 | Number of intervals for each axis | +| `.interval_.*` | various | - | Interval configuration (see Simulator) | + +**Axes Configured:** +- `linear_x`, `linear_y`, `linear_z` - Linear acceleration axes +- `angular_x`, `angular_y`, `angular_z` - Angular velocity axes + +**Constructor** +```cpp +DataSimulator() +``` +- Initializes ROS2 node with name `imu_data_simulator` +- Creates `Simulator` instance for value generation +- Creates publisher for `simulated_imu_data` topic +- Sets up timer for periodic publishing + +## Core Functionality + +**`void publish_imu_data()`** +- Timer callback invoked at the configured publish rate +- Calculates elapsed time since node start +- Queries `Simulator` for current values of all 6 axes +- Populates IMU message with: + - Header timestamp and frame_id (`imu_link`) + - Linear acceleration (x, y, z) + - Angular velocity (x, y, z) +- Publishes message to topic + +## ROS2 Interface + +**Publications** +- `simulated_imu_data` (sensor_msgs/msg/Imu) + - Publishes simulated IMU data at configured rate + - Frame ID: `imu_link` + +## Usage Example + +```bash +ros2 run g2_2025_odometry_pkg imu_data_simulator_node --ros-args \ + -p publish_rate:=20.0 \ + -p linear_x.num_intervals:=1 \ + -p linear_x.interval_0.type:=constant \ + -p linear_x.interval_0.t_start:=0.0 \ + -p linear_x.interval_0.t_end:=10.0 \ + -p linear_x.interval_0.y_start:=9.81 +``` + +## Dependencies + +- `rclcpp` - ROS2 C++ client library +- `sensor_msgs` - Standard sensor message types +- `Simulator` - Internal simulation engine \ No newline at end of file diff --git a/doc/architecture/nodes/WheelDataSimulator.md b/doc/architecture/nodes/WheelDataSimulator.md new file mode 100644 index 0000000..bbce437 --- /dev/null +++ b/doc/architecture/nodes/WheelDataSimulator.md @@ -0,0 +1,64 @@ +# WheelDataSimulator (`assignments::three::wheel_data_simulator_node`) + +The `WheelDataSimulator` node generates simulated wheel encoder/velocity data (`std_msgs/msg/Float64MultiArray`) based on configurable time-varying intervals. It publishes wheel values for a 4-wheel robot configuration that can follow constant, linear, or quadratic trajectories over time. + +## Implementation Details + +**Parameters** + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `publish_rate` | double | 10.0 | Publishing frequency in Hz | +| `max_intervals` | int | 4 | Maximum number of intervals per wheel | +| `.num_intervals` | int | 0 | Number of intervals for each wheel | +| `.interval_.*` | various | - | Interval configuration (see Simulator) | + +**Wheels Configured:** +- `wheel_fl` - Front Left wheel +- `wheel_fr` - Front Right wheel +- `wheel_rl` - Rear Left wheel +- `wheel_rr` - Rear Right wheel + +**Constructor** +```cpp +DataSimulator() +``` +- Initializes ROS2 node with name `wheel_data_simulator` +- Creates `Simulator` instance for value generation +- Creates publisher for `simulated_wheel_data` topic +- Sets up timer for periodic publishing + +## Core Functionality + +**`void publish_wheel_data()`** +- Timer callback invoked at the configured publish rate +- Calculates elapsed time since node start +- Queries `Simulator` for current values of all 4 wheels +- Populates Float64MultiArray message with wheel values in order: [FL, FR, RL, RR] +- Publishes message to topic + +## ROS2 Interface + +**Publications** +- `simulated_wheel_data` (std_msgs/msg/Float64MultiArray) + - Publishes simulated wheel data at configured rate + - Array contains 4 values: [front_left, front_right, rear_left, rear_right] + +## Usage Example + +```bash +ros2 run g2_2025_odometry_pkg wheel_data_simulator_node --ros-args \ + -p publish_rate:=50.0 \ + -p wheel_fl.num_intervals:=1 \ + -p wheel_fl.interval_0.type:=linear \ + -p wheel_fl.interval_0.t_start:=0.0 \ + -p wheel_fl.interval_0.t_end:=10.0 \ + -p wheel_fl.interval_0.y_start:=0.0 \ + -p wheel_fl.interval_0.y_end:=5.0 +``` + +## Dependencies + +- `rclcpp` - ROS2 C++ client library +- `std_msgs` - Standard message types +- `Simulator` - Internal simulation engine diff --git a/doc/tests/IMUDataSimulator.md b/doc/tests/IMUDataSimulator.md new file mode 100644 index 0000000..bc5ebad --- /dev/null +++ b/doc/tests/IMUDataSimulator.md @@ -0,0 +1,67 @@ +# IMU Data Simulator Unit Tests + +Unit tests for the `DataSimulator` (IMU) node are implemented in `src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp` using Google Test and ROS2 test utilities. The tests validate node initialization, message publishing, and data correctness. + +## Test Cases + +### 1. NodeInitialization + +**Description:** Verifies that the DataSimulator node can be created without errors. + +- **Test Action:** Create DataSimulator instance +- **Expected Result:** No exceptions thrown during construction + +### 2. MessagePublishing + +**Description:** Tests that IMU messages are published on the correct topic. + +- **Test Action:** + - Create subscription to `simulated_imu_data` topic + - Create DataSimulator node + - Spin both nodes for a short period +- **Expected Result:** At least one `sensor_msgs/msg/Imu` message is received + +### 3. MessageFrameId + +**Description:** Verifies that published IMU messages have the correct frame_id. + +- **Test Action:** + - Subscribe to `simulated_imu_data` + - Receive a message +- **Expected Result:** `header.frame_id` equals `"imu_link"` + +### 4. LinearAccelerationValues + +**Description:** Tests that linear acceleration values are valid (finite numbers). + +- **Test Action:** + - Subscribe and receive IMU message + - Check linear_acceleration fields +- **Expected Result:** + - `linear_acceleration.x` is finite + - `linear_acceleration.y` is finite + - `linear_acceleration.z` is finite + +### 5. AngularVelocityValues + +**Description:** Tests that angular velocity values are valid (finite numbers). + +- **Test Action:** + - Subscribe and receive IMU message + - Check angular_velocity fields +- **Expected Result:** + - `angular_velocity.x` is finite + - `angular_velocity.y` is finite + - `angular_velocity.z` is finite + +## Test Infrastructure + +The test class `IMUSimulatorTest` provides: +- Static `SetUpTestSuite()` and `TearDownTestSuite()` for ROS2 init/shutdown +- Helper method `setupBasicIMUParams()` for setting up default test parameters + +## ROS2 Topics Used + +| Topic | Message Type | Direction | +|-------|--------------|-----------| +| `simulated_imu_data` | sensor_msgs/msg/Imu | Published by node under test | diff --git a/doc/tests/Simulator.md b/doc/tests/Simulator.md new file mode 100644 index 0000000..dbffc43 --- /dev/null +++ b/doc/tests/Simulator.md @@ -0,0 +1,98 @@ +# Simulator Unit Tests + +Unit tests for `Simulator` are implemented in `src/g2_2025_odometry_pkg/test/test_simulator.cpp` using Google Test and ROS2 test utilities. The tests validate interval-based value generation with different interpolation types. + +## Test Cases + +### 1. ConstantInterval + +**Description:** Verifies constant interpolation returns the same value throughout the interval. + +- **Test Configuration:** + - Single interval: t=[0, 10], y_start=5.0 + - Type: constant +- **Expected Result:** + - Value is 5.0 at t=0, t=5, t=10 + - Value holds at 5.0 after interval (t=15) + +### 2. LinearInterval + +**Description:** Tests linear interpolation between start and end values. + +- **Test Configuration:** + - Single interval: t=[0, 10], y_start=0.0, y_end=10.0 + - Type: linear +- **Expected Result:** + - Value is 0.0 at t=0 + - Value is 5.0 at t=5 + - Value is 10.0 at t=10 + - Value holds at 10.0 after interval (t=15) + +### 3. QuadraticInterval + +**Description:** Tests quadratic (Lagrange) interpolation through 3 points. + +- **Test Configuration:** + - Single interval: t=[0, 10], y_start=0.0, y_end=0.0, t_mid=5.0, y_mid=10.0 + - Type: quadratic +- **Expected Result:** + - Value is 0.0 at t=0 + - Value is 10.0 at t=5 (peak) + - Value is 0.0 at t=10 + - Value holds at 0.0 after interval (t=15) + +### 4. MultipleIntervals + +**Description:** Tests behavior with multiple non-overlapping intervals and gaps. + +- **Test Configuration:** + - Interval 0: t=[0, 5], constant 5.0 + - Interval 1: t=[10, 15], constant 10.0 + - Gap between t=5 and t=10 +- **Expected Result:** + - Value is 5.0 at t=2.5 (first interval) + - Value holds at 5.0 in gap (t=7.5) + - Value is 10.0 at t=12.5 (second interval) + - Value holds at 10.0 after all intervals (t=20) + +### 5. ValueBeforeIntervals + +**Description:** Tests behavior when querying time before any interval starts. + +- **Test Configuration:** + - Single interval: t=[10, 20] +- **Expected Result:** + - Value is 0.0 at t=5 (before interval starts) + +### 6. NonExistentChannel + +**Description:** Tests graceful handling of queries for unconfigured channels. + +- **Test Action:** Query `get_object_value("nonexistent_channel", 5.0)` +- **Expected Result:** Returns 0.0 + +### 7. OverlappingIntervalsThrowsException + +**Description:** Verifies that overlapping intervals are detected and rejected. + +- **Test Configuration:** + - Interval 0: t=[0, 10] + - Interval 1: t=[5, 15] (overlaps) +- **Expected Result:** Constructor throws `std::runtime_error` + +### 8. MaxIntervalsLimit + +**Description:** Tests that `max_intervals` parameter limits loaded intervals. + +- **Test Configuration:** + - max_intervals=2 + - num_intervals=3 (defines 3 intervals) +- **Expected Result:** + - Only first 2 intervals are loaded + - Third interval values default to holding second interval's end value + +## Test Infrastructure + +The test class `SimulatorTest` provides: +- Static `SetUpTestSuite()` and `TearDownTestSuite()` for ROS2 init/shutdown +- Helper method `createNodeWithParams()` for creating nodes with parameter overrides diff --git a/doc/tests/WheelDataSimulator.md b/doc/tests/WheelDataSimulator.md new file mode 100644 index 0000000..852efa5 --- /dev/null +++ b/doc/tests/WheelDataSimulator.md @@ -0,0 +1,73 @@ +# Wheel Data Simulator Unit Tests + +Unit tests for the `DataSimulator` (Wheel) node are implemented in `src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp` using Google Test and ROS2 test utilities. The tests validate node initialization, message publishing, array structure, and data correctness. + +## Test Cases + +### 1. NodeInitialization + +**Description:** Verifies that the Wheel DataSimulator node can be created without errors. + +- **Test Action:** Create DataSimulator instance +- **Expected Result:** No exceptions thrown during construction + +### 2. WheelDataPublishing + +**Description:** Tests that wheel data messages are published on the correct topic. + +- **Test Action:** + - Create subscription to `simulated_wheel_data` topic + - Create DataSimulator node + - Spin both nodes for a short period +- **Expected Result:** At least one `std_msgs/msg/Float64MultiArray` message is received + +### 3. WheelDataArraySize + +**Description:** Verifies that the published array contains the correct number of wheel values. + +- **Test Action:** + - Subscribe to `simulated_wheel_data` + - Receive a message + - Check array size +- **Expected Result:** `data.size()` equals 4 (FL, FR, RL, RR) + +### 4. ValidVelocityValues + +**Description:** Tests that all wheel velocity values are valid (finite numbers). + +- **Test Action:** + - Subscribe and receive wheel data message + - Check each value in the array +- **Expected Result:** All 4 values are finite numbers + +### 5. MultipleMessagesReceived + +**Description:** Tests that multiple messages are published over time. + +- **Test Action:** + - Subscribe and count received messages + - Spin for a short duration +- **Expected Result:** Message count is greater than 0 + +## Test Infrastructure + +The test class `WheelSimulatorTest` provides: +- Static `SetUpTestSuite()` and `TearDownTestSuite()` for ROS2 init/shutdown +- Helper method `setupBasicWheelParams()` for setting up default test parameters + +## ROS2 Topics Used + +| Topic | Message Type | Direction | +|-------|--------------|-----------| +| `simulated_wheel_data` | std_msgs/msg/Float64MultiArray | Published by node under test | + +## Data Array Format + +The published Float64MultiArray contains wheel values in the following order: + +| Index | Wheel | +|-------|-------| +| 0 | Front Left (FL) | +| 1 | Front Right (FR) | +| 2 | Rear Left (RL) | +| 3 | Rear Right (RR) | -- 2.39.5 From 881346b284b28d5e7a409f9eba060d653d9f82b7 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Fri, 28 Nov 2025 09:28:09 +0100 Subject: [PATCH 13/14] fix(naming): Change all to UpperCamelCase --- doc/tests/Simulator.md | 2 +- src/g2_2025_odometry_pkg/CMakeLists.txt | 14 ++++----- src/g2_2025_odometry_pkg/config/opt.yaml | 31 ++++++++++++++++--- .../g2_2025_imu_data_simulator_node/main.cpp | 2 +- ...ata_simulator.cpp => ImuDataSimulator.cpp} | 4 +-- ...ata_simulator.hpp => ImuDataSimulator.hpp} | 0 .../main.cpp | 2 +- ...a_simulator.cpp => WheelDataSimulator.cpp} | 6 ++-- ...a_simulator.hpp => WheelDataSimulator.hpp} | 0 ...imu_simulator.cpp => TestImuSimulator.cpp} | 2 +- .../{test_simulator.cpp => TestSimulator.cpp} | 0 ...l_simulator.cpp => TestWheelSimulator.cpp} | 2 +- 12 files changed, 43 insertions(+), 22 deletions(-) rename src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/{imu_data_simulator.cpp => ImuDataSimulator.cpp} (95%) rename src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/{imu_data_simulator.hpp => ImuDataSimulator.hpp} (100%) rename src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/{wheel_data_simulator.cpp => WheelDataSimulator.cpp} (91%) rename src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/{wheel_data_simulator.hpp => WheelDataSimulator.hpp} (100%) rename src/g2_2025_odometry_pkg/test/{test_imu_simulator.cpp => TestImuSimulator.cpp} (98%) rename src/g2_2025_odometry_pkg/test/{test_simulator.cpp => TestSimulator.cpp} (100%) rename src/g2_2025_odometry_pkg/test/{test_wheel_simulator.cpp => TestWheelSimulator.cpp} (98%) diff --git a/doc/tests/Simulator.md b/doc/tests/Simulator.md index dbffc43..a75e680 100644 --- a/doc/tests/Simulator.md +++ b/doc/tests/Simulator.md @@ -1,6 +1,6 @@ # Simulator Unit Tests -Unit tests for `Simulator` are implemented in `src/g2_2025_odometry_pkg/test/test_simulator.cpp` using Google Test and ROS2 test utilities. The tests validate interval-based value generation with different interpolation types. +Unit tests for the `Simulator` class are implemented in `src/g2_2025_odometry_pkg/test/test_simulator.cpp` using Google Test and ROS2 test utilities. The tests validate interval-based value generation with different interpolation types. ## Test Cases diff --git a/src/g2_2025_odometry_pkg/CMakeLists.txt b/src/g2_2025_odometry_pkg/CMakeLists.txt index 274b8f1..e98170a 100644 --- a/src/g2_2025_odometry_pkg/CMakeLists.txt +++ b/src/g2_2025_odometry_pkg/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) add_executable(imu_data_simulator_node - src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp + src/g2_2025_imu_data_simulator_node/nodes/ImuDataSimulator.cpp src/g2_2025_imu_data_simulator_node/main.cpp src/simulator/Simulator.cpp) @@ -28,7 +28,7 @@ target_include_directories(imu_data_simulator_node PRIVATE ament_target_dependencies(imu_data_simulator_node rclcpp sensor_msgs geometry_msgs) add_executable(wheel_data_simulator_node - src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp + src/g2_2025_wheel_data_simulator_node/nodes/WheelDataSimulator.cpp src/g2_2025_wheel_data_simulator_node/main.cpp src/simulator/Simulator.cpp) @@ -53,13 +53,13 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) # Simulator unit tests - ament_add_gtest(test_simulator test/test_simulator.cpp src/simulator/Simulator.cpp) + ament_add_gtest(test_simulator test/TestSimulator.cpp src/simulator/Simulator.cpp) target_include_directories(test_simulator PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src) ament_target_dependencies(test_simulator rclcpp) # IMU simulator integration tests - ament_add_gtest(test_imu_simulator test/test_imu_simulator.cpp - src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp + ament_add_gtest(test_imu_simulator test/TestImuSimulator.cpp + src/g2_2025_imu_data_simulator_node/nodes/ImuDataSimulator.cpp src/simulator/Simulator.cpp) target_include_directories(test_imu_simulator PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src @@ -67,8 +67,8 @@ if(BUILD_TESTING) ament_target_dependencies(test_imu_simulator rclcpp sensor_msgs geometry_msgs) # Wheel simulator integration tests - ament_add_gtest(test_wheel_simulator test/test_wheel_simulator.cpp - src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp + ament_add_gtest(test_wheel_simulator test/TestWheelSimulator.cpp + src/g2_2025_wheel_data_simulator_node/nodes/WheelDataSimulator.cpp src/simulator/Simulator.cpp) target_include_directories(test_wheel_simulator PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src diff --git a/src/g2_2025_odometry_pkg/config/opt.yaml b/src/g2_2025_odometry_pkg/config/opt.yaml index e414018..4b4dcee 100644 --- a/src/g2_2025_odometry_pkg/config/opt.yaml +++ b/src/g2_2025_odometry_pkg/config/opt.yaml @@ -1,8 +1,9 @@ imu_data_simulator: ros__parameters: + max_intervals: 4 publish_rate: 10.0 linear_x: - num_intervals: 2 + num_intervals: 4 interval_0: type: "linear" t_start: 0.0 @@ -11,10 +12,22 @@ imu_data_simulator: y_end: 9.81 interval_1: type: "linear" - t_start: 10.0 - t_end: 15.0 + t_start: 7.5 + t_end: 12.5 y_start: 9.81 y_end: 0.0 + interval_2: + type: "linear" + t_start: 15.0 + t_end: 20.0 + y_start: 0.0 + y_end: -9.81 + interval_3: + type: "linear" + t_start: 22.5 + t_end: 25.0 + y_start: -9.81 + y_end: 0.0 angular_z: num_intervals: 1 interval_0: @@ -22,15 +35,23 @@ imu_data_simulator: t_start: 2.0 y_start: 1.57 angular_x: - num_intervals: 1 + num_intervals: 2 interval_0: type: "quadratic" t_start: 3.0 - y_start: 0.785 + y_start: 0.0 t_mid: 4.5 y_mid: 1.57 t_end: 6.0 y_end: 0.0 + interval_1: + type: "quadratic" + t_start: 7.0 + y_start: 0.0 + t_mid: 8.5 + y_mid: -1.57 + t_end: 10.0 + y_end: 0.0 wheel_data_simulator: ros__parameters: diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/main.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/main.cpp index 27f8111..fa566cb 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/main.cpp +++ b/src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/main.cpp @@ -1,5 +1,5 @@ #include "rclcpp/rclcpp.hpp" -#include "nodes/imu_data_simulator.hpp" +#include "nodes/ImuDataSimulator.hpp" int main(int argc, char *argv[]) { rclcpp::init(argc, argv); 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/ImuDataSimulator.cpp similarity index 95% rename from src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp rename to src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/ImuDataSimulator.cpp index 87b5739..1765ea3 100644 --- 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/ImuDataSimulator.cpp @@ -1,9 +1,9 @@ -#include "imu_data_simulator.hpp" +#include "ImuDataSimulator.hpp" namespace assignments::three::data_simulator_node { DataSimulator::DataSimulator() : Node("imu_data_simulator") { - RCLCPP_INFO(this->get_logger(), "DataSimulator node created"); + RCLCPP_INFO(this->get_logger(), "ImuDataSimulator node created"); start_time_ = this->now(); 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/ImuDataSimulator.hpp similarity index 100% rename from src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.hpp rename to src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/ImuDataSimulator.hpp diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp index 19a744e..325c831 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp +++ b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp @@ -1,5 +1,5 @@ #include "rclcpp/rclcpp.hpp" -#include "nodes/wheel_data_simulator.hpp" +#include "nodes/WheelDataSimulator.hpp" int main(int argc, char *argv[]) { rclcpp::init(argc, argv); diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/WheelDataSimulator.cpp similarity index 91% rename from src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp rename to src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/WheelDataSimulator.cpp index 1176a0d..1dfe73d 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp +++ b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/WheelDataSimulator.cpp @@ -1,9 +1,9 @@ -#include "wheel_data_simulator.hpp" +#include "WheelDataSimulator.hpp" namespace assignments::three::wheel_data_simulator_node { DataSimulator::DataSimulator() : Node("wheel_data_simulator") { - RCLCPP_INFO(this->get_logger(), "DataSimulator node created"); + RCLCPP_INFO(this->get_logger(), "WheelDataSimulator node created"); start_time_ = this->now(); @@ -29,7 +29,7 @@ DataSimulator::DataSimulator() : Node("wheel_data_simulator") { } DataSimulator::~DataSimulator() { - RCLCPP_INFO(this->get_logger(), "DataSimulator node destroyed"); + RCLCPP_INFO(this->get_logger(), "WheelDataSimulator node destroyed"); } void DataSimulator::publish_wheel_data() { diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/WheelDataSimulator.hpp similarity index 100% rename from src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp rename to src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/WheelDataSimulator.hpp diff --git a/src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp b/src/g2_2025_odometry_pkg/test/TestImuSimulator.cpp similarity index 98% rename from src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp rename to src/g2_2025_odometry_pkg/test/TestImuSimulator.cpp index 894549b..5125f48 100644 --- a/src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp +++ b/src/g2_2025_odometry_pkg/test/TestImuSimulator.cpp @@ -1,7 +1,7 @@ #include #include #include -#include "g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.hpp" +#include "g2_2025_imu_data_simulator_node/nodes/ImuDataSimulator.hpp" #include using namespace assignments::three::data_simulator_node; diff --git a/src/g2_2025_odometry_pkg/test/test_simulator.cpp b/src/g2_2025_odometry_pkg/test/TestSimulator.cpp similarity index 100% rename from src/g2_2025_odometry_pkg/test/test_simulator.cpp rename to src/g2_2025_odometry_pkg/test/TestSimulator.cpp diff --git a/src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp b/src/g2_2025_odometry_pkg/test/TestWheelSimulator.cpp similarity index 98% rename from src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp rename to src/g2_2025_odometry_pkg/test/TestWheelSimulator.cpp index e7a2674..b511fc5 100644 --- a/src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp +++ b/src/g2_2025_odometry_pkg/test/TestWheelSimulator.cpp @@ -1,7 +1,7 @@ #include #include #include -#include "g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp" +#include "g2_2025_wheel_data_simulator_node/nodes/WheelDataSimulator.hpp" #include using namespace assignments::three::wheel_data_simulator_node; -- 2.39.5 From 45f5397d58f956b144f005fac7d2810df5cfa4ee Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Fri, 28 Nov 2025 09:34:34 +0100 Subject: [PATCH 14/14] fix(formatting): Fix simple formatting errors --- src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp | 4 +--- src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp | 7 +++---- src/g2_2025_odometry_pkg/test/TestSimulator.cpp | 2 +- 3 files changed, 5 insertions(+), 8 deletions(-) diff --git a/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp b/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp index 41dab25..fc1a3f5 100644 --- a/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp +++ b/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp @@ -1,8 +1,6 @@ #include "Simulator.hpp" -#include -namespace assignments::three -{ +namespace assignments::three { Simulator::Simulator(rclcpp::Node* node, const std::vector& objects) { load_intervals(node, objects); diff --git a/src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp b/src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp index f0a48c5..a64a84d 100644 --- a/src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp +++ b/src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp @@ -3,9 +3,9 @@ #include #include #include +#include -namespace assignments::three -{ +namespace assignments::three { enum class SimType { CONSTANT, @@ -32,11 +32,10 @@ public: private: int max_intervals_; + std::map> object_intervals_; void load_intervals(rclcpp::Node* node, const std::vector& objects); double compute_value(double t, const IntervalConfig& interval); - - std::map> object_intervals_; }; } // namespace assignments::three diff --git a/src/g2_2025_odometry_pkg/test/TestSimulator.cpp b/src/g2_2025_odometry_pkg/test/TestSimulator.cpp index dbcf1a7..ecf3cf5 100644 --- a/src/g2_2025_odometry_pkg/test/TestSimulator.cpp +++ b/src/g2_2025_odometry_pkg/test/TestSimulator.cpp @@ -248,7 +248,7 @@ TEST_F(SimulatorTest, MaxIntervalsLimit) { // Only first 2 intervals should be loaded EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 1.0); // First interval EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 2.0); // Second interval - EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 25.0), 2.0); // Third interval should not exist, holds second + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 25.0), 2.0); // Third interval should not exist } int main(int argc, char** argv) { -- 2.39.5