feat(wheel_data_simulator): Add wheel data simulator

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}
 )
 

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

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<assignments::three::wheel_data_simulator_node::DataSimulator>();
+
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+
+    return 0;
+}

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<Simulator>(this, wheels_);
+
+    wheel_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("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<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(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<std_msgs::msg::Float64MultiArray>();
+
+    // 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

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 <vector>
+#include <string>
+#include <map>
+#include <memory>
+
+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<std_msgs::msg::Float64MultiArray>::SharedPtr wheel_publisher_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Time start_time_;
+
+    std::vector<std::string> wheels_;
+    std::unique_ptr<Simulator> simulator_;
+
+    void publish_wheel_data();
+
+};
+
+} // namespace assignments::three::wheel_data_simulator_node