From 9d8a1fc71054f32c9c3f2e6c44ff1aa68869cec3 Mon Sep 17 00:00:00 2001 From: Wessel Tip Date: Fri, 28 Nov 2025 20:14:33 +0100 Subject: [PATCH] chore: Remove copy-pasted nodes --- src/g2_2025_odometry_pkg/config/opt.yaml | 63 -------- .../launch/wheel_sim.launch.xml | 13 -- .../main.cpp | 13 -- .../nodes/wheel_data_simulator.cpp | 58 -------- .../nodes/wheel_data_simulator.hpp | 33 ----- .../src/simulator/Simulator.cpp | 140 ------------------ .../src/simulator/Simulator.hpp | 42 ------ 7 files changed, 362 deletions(-) delete mode 100644 src/g2_2025_odometry_pkg/config/opt.yaml delete mode 100644 src/g2_2025_odometry_pkg/launch/wheel_sim.launch.xml delete mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp delete mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp delete mode 100644 src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp delete mode 100644 src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp delete mode 100644 src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp diff --git a/src/g2_2025_odometry_pkg/config/opt.yaml b/src/g2_2025_odometry_pkg/config/opt.yaml deleted file mode 100644 index 61c9678..0000000 --- a/src/g2_2025_odometry_pkg/config/opt.yaml +++ /dev/null @@ -1,63 +0,0 @@ -imu_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 - -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 - -wheel_position_approximator: - ros__parameters: - wheel_radius: 0.2 - lx: 0.3 - ly: 0.6 - time_elapsed: 1.0 - initial_x: 0.0 - initial_y: 0.0 - initial_theta: 0.0 diff --git a/src/g2_2025_odometry_pkg/launch/wheel_sim.launch.xml b/src/g2_2025_odometry_pkg/launch/wheel_sim.launch.xml deleted file mode 100644 index 4249fe2..0000000 --- a/src/g2_2025_odometry_pkg/launch/wheel_sim.launch.xml +++ /dev/null @@ -1,13 +0,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 deleted file mode 100644 index 060d515..0000000 --- a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#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 deleted file mode 100644 index 1176a0d..0000000 --- a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#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(); - - // 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_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(), - "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 deleted file mode 100644 index b3f0012..0000000 --- a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp +++ /dev/null @@ -1,33 +0,0 @@ -#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 diff --git a/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp b/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp deleted file mode 100644 index e0cb883..0000000 --- a/src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp +++ /dev/null @@ -1,140 +0,0 @@ -#include "Simulator.hpp" -#include - -namespace assignments::three -{ - -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& objects) { - node->declare_parameter("max_intervals", 4); - max_intervals_ = node->get_parameter("max_intervals").as_int(); - - 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 = object + ".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_DEBUG(node->get_logger(), - "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 - ); - } - - object_intervals_[object] = 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_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 object - 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/simulator/Simulator.hpp b/src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp deleted file mode 100644 index f0a48c5..0000000 --- a/src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once -#include -#include -#include -#include - -namespace assignments::three -{ - -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 Simulator { -public: - Simulator(rclcpp::Node* node, const std::vector& objects); - ~Simulator(); - - double get_object_value(const std::string& object, double t); - -private: - int max_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