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