fix(data_sim): Add param file, add working implementation

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

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<IntervalConfig> 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<sensor_msgs::msg::Imu>("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<int>(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

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 <vector>
+#include <string>
+#include <map>
 
 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<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
     rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Time start_time_;
+
+    std::vector<std::string> axes_;
+    std::map<std::string, std::vector<IntervalConfig>> 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