From d06a33b90c579d7f77f6a553f38ac6a8fa316543 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Mon, 24 Nov 2025 16:53:27 +0100 Subject: [PATCH] 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