diff --git a/src/g2_2025_odometry_pkg/CMakeLists.txt b/src/g2_2025_odometry_pkg/CMakeLists.txt index 9defd0d..274b8f1 100644 --- a/src/g2_2025_odometry_pkg/CMakeLists.txt +++ b/src/g2_2025_odometry_pkg/CMakeLists.txt @@ -50,15 +50,30 @@ install(DIRECTORY launch config ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + + # Simulator unit tests + ament_add_gtest(test_simulator test/test_simulator.cpp src/simulator/Simulator.cpp) + target_include_directories(test_simulator PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src) + ament_target_dependencies(test_simulator rclcpp) + + # IMU simulator integration tests + ament_add_gtest(test_imu_simulator test/test_imu_simulator.cpp + src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp + src/simulator/Simulator.cpp) + target_include_directories(test_imu_simulator PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_imu_data_simulator_node) + ament_target_dependencies(test_imu_simulator rclcpp sensor_msgs geometry_msgs) + + # Wheel simulator integration tests + ament_add_gtest(test_wheel_simulator test/test_wheel_simulator.cpp + src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp + src/simulator/Simulator.cpp) + target_include_directories(test_wheel_simulator PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_wheel_data_simulator_node) + ament_target_dependencies(test_wheel_simulator rclcpp std_msgs) endif() ament_package() diff --git a/src/g2_2025_odometry_pkg/package.xml b/src/g2_2025_odometry_pkg/package.xml index b9e4d06..de60dca 100644 --- a/src/g2_2025_odometry_pkg/package.xml +++ b/src/g2_2025_odometry_pkg/package.xml @@ -11,6 +11,7 @@ ament_lint_auto ament_lint_common + ament_cmake_gtest ament_cmake diff --git a/src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp b/src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp new file mode 100644 index 0000000..894549b --- /dev/null +++ b/src/g2_2025_odometry_pkg/test/test_imu_simulator.cpp @@ -0,0 +1,171 @@ +#include +#include +#include +#include "g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.hpp" +#include + +using namespace assignments::three::data_simulator_node; + +class IMUSimulatorTest : public ::testing::Test { +protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + void setupBasicIMUParams(rclcpp::Node* node) { + node->declare_parameter("max_intervals", 4); + node->declare_parameter("publish_rate", 10.0); + + // Setup linear_x + node->declare_parameter("linear_x.num_intervals", 1); + node->declare_parameter("linear_x.interval_0.type", "constant"); + node->declare_parameter("linear_x.interval_0.t_start", 0.0); + node->declare_parameter("linear_x.interval_0.t_end", 5.0); + node->declare_parameter("linear_x.interval_0.y_start", 1.0); + node->declare_parameter("linear_x.interval_0.y_end", 0.0); + node->declare_parameter("linear_x.interval_0.t_mid", 0.0); + node->declare_parameter("linear_x.interval_0.y_mid", 0.0); + + // Setup other axes with no intervals + for (const auto& axis : { "linear_y", "linear_z", "angular_x", "angular_y", "angular_z" }) { + node->declare_parameter(std::string(axis) + ".num_intervals", 0); + } + } +}; + +// Test node initialization +TEST_F(IMUSimulatorTest, NodeInitialization) { + auto node = std::make_shared("test_imu_init_node"); + setupBasicIMUParams(node.get()); + + // Should not throw + EXPECT_NO_THROW({ + DataSimulator sim; + }); +} + +// Test IMU message publishing +TEST_F(IMUSimulatorTest, MessagePublishing) { + auto test_node = std::make_shared("test_imu_pub_node"); + + sensor_msgs::msg::Imu::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_imu_data", + 10, + [&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) { + received_msg = msg; + } + ); + + auto sim_node = std::make_shared(); + + // Spin a few times to allow publishing + for (int i = 0; i < 5; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + // Check that we received a message + ASSERT_NE(received_msg, nullptr); +} + +// Test IMU message frame_id +TEST_F(IMUSimulatorTest, MessageFrameId) { + auto test_node = std::make_shared("test_imu_frame_node"); + + sensor_msgs::msg::Imu::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_imu_data", + 10, + [&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) { + received_msg = msg; + } + ); + + auto sim_node = std::make_shared(); + + // Spin a few times to allow publishing + for (int i = 0; i < 5; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + ASSERT_NE(received_msg, nullptr); + EXPECT_EQ(received_msg->header.frame_id, "imu_link"); +} + +// Test linear acceleration values +TEST_F(IMUSimulatorTest, LinearAccelerationValues) { + auto test_node = std::make_shared("test_imu_linear_node"); + + sensor_msgs::msg::Imu::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_imu_data", + 10, + [&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) { + if (!received_msg) { // Only capture first message + received_msg = msg; + } + } + ); + + auto sim_node = std::make_shared(); + + // Spin until we receive a message + for (int i = 0; i < 10 && !received_msg; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + ASSERT_NE(received_msg, nullptr); + + // Values should be set according to configuration + // Since we don't control exact timing, just verify structure + EXPECT_TRUE(std::isfinite(received_msg->linear_acceleration.x)); + EXPECT_TRUE(std::isfinite(received_msg->linear_acceleration.y)); + EXPECT_TRUE(std::isfinite(received_msg->linear_acceleration.z)); +} + +// Test angular velocity values +TEST_F(IMUSimulatorTest, AngularVelocityValues) { + auto test_node = std::make_shared("test_imu_angular_node"); + + sensor_msgs::msg::Imu::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_imu_data", + 10, + [&received_msg](sensor_msgs::msg::Imu::SharedPtr msg) { + if (!received_msg) { + received_msg = msg; + } + } + ); + + auto sim_node = std::make_shared(); + + // Spin until we receive a message + for (int i = 0; i < 10 && !received_msg; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + ASSERT_NE(received_msg, nullptr); + + // Values should be set according to configuration + EXPECT_TRUE(std::isfinite(received_msg->angular_velocity.x)); + EXPECT_TRUE(std::isfinite(received_msg->angular_velocity.y)); + EXPECT_TRUE(std::isfinite(received_msg->angular_velocity.z)); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/g2_2025_odometry_pkg/test/test_simulator.cpp b/src/g2_2025_odometry_pkg/test/test_simulator.cpp new file mode 100644 index 0000000..dbcf1a7 --- /dev/null +++ b/src/g2_2025_odometry_pkg/test/test_simulator.cpp @@ -0,0 +1,257 @@ +#include +#include +#include "simulator/Simulator.hpp" +#include + +using namespace assignments::three; + +class SimulatorTest : public ::testing::Test { +protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + // Helper to create a node with parameter overrides + std::shared_ptr createNodeWithParams( + const std::string& name, + const std::vector& params) + { + rclcpp::NodeOptions options; + options.parameter_overrides(params); + return std::make_shared(name, options); + } +}; + +// Test constant interval +TEST_F(SimulatorTest, ConstantInterval) { + auto node = createNodeWithParams("test_constant_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 1), + rclcpp::Parameter("test_channel.interval_0.type", "constant"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); // Test values within interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 0.0), 5.0); + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 5.0); + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 10.0), 5.0); + + // Test value after interval holds + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 5.0); +} + +// Test linear interval +TEST_F(SimulatorTest, LinearInterval) { + auto node = createNodeWithParams("test_linear_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 1), + rclcpp::Parameter("test_channel.interval_0.type", "linear"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); + + // Test linear interpolation + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 0.0), 0.0); + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 5.0); + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 10.0), 10.0); + + // Test value after interval holds + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 10.0); +} + +// Test quadratic interval +TEST_F(SimulatorTest, QuadraticInterval) { + auto node = createNodeWithParams("test_quadratic_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 1), + rclcpp::Parameter("test_channel.interval_0.type", "quadratic"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 10.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); + + // Test quadratic interpolation + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 0.0), 0.0); + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 10.0); + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 10.0), 0.0); + + // Test value after interval holds + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 0.0); +} + +// Test multiple intervals +TEST_F(SimulatorTest, MultipleIntervals) { + auto node = createNodeWithParams("test_multiple_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 2), + // First interval: constant 5.0 from t=0 to t=5 + rclcpp::Parameter("test_channel.interval_0.type", "constant"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0), + // Second interval: constant 10.0 from t=10 to t=15 + rclcpp::Parameter("test_channel.interval_1.type", "constant"), + rclcpp::Parameter("test_channel.interval_1.t_start", 10.0), + rclcpp::Parameter("test_channel.interval_1.t_end", 15.0), + rclcpp::Parameter("test_channel.interval_1.y_start", 10.0), + rclcpp::Parameter("test_channel.interval_1.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_1.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_1.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); + + // Test first interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 2.5), 5.0); + + // Test gap between intervals - should hold value from first interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 7.5), 5.0); + + // Test second interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 12.5), 10.0); + + // Test after all intervals - should hold value from last interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 20.0), 10.0); +} + +// Test value before any interval +TEST_F(SimulatorTest, ValueBeforeIntervals) { + auto node = createNodeWithParams("test_before_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 1), + rclcpp::Parameter("test_channel.interval_0.type", "constant"), + rclcpp::Parameter("test_channel.interval_0.t_start", 10.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 20.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); + + // Before any interval starts, should return 0.0 + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 0.0); +} + +// Test non-existent channel +TEST_F(SimulatorTest, NonExistentChannel) { + auto node = createNodeWithParams("test_nonexistent_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 1), + rclcpp::Parameter("test_channel.interval_0.type", "constant"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); + + // Query non-existent channel should return 0.0 + EXPECT_DOUBLE_EQ(sim.get_object_value("nonexistent_channel", 5.0), 0.0); +} + +// Test overlapping intervals detection +TEST_F(SimulatorTest, OverlappingIntervalsThrowsException) { + auto node = createNodeWithParams("test_overlap_node", { + rclcpp::Parameter("max_intervals", 4), + rclcpp::Parameter("test_channel.num_intervals", 2), + // First interval: t=0 to t=10 + rclcpp::Parameter("test_channel.interval_0.type", "constant"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 5.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0), + // Second interval: t=5 to t=15 (overlaps with first) + rclcpp::Parameter("test_channel.interval_1.type", "constant"), + rclcpp::Parameter("test_channel.interval_1.t_start", 5.0), + rclcpp::Parameter("test_channel.interval_1.t_end", 15.0), + rclcpp::Parameter("test_channel.interval_1.y_start", 10.0), + rclcpp::Parameter("test_channel.interval_1.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_1.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_1.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + + // Should throw exception due to overlapping intervals + EXPECT_THROW({ + Simulator sim(node.get(), channels); + }, std::runtime_error); +} + +// Test max_intervals limit +TEST_F(SimulatorTest, MaxIntervalsLimit) { + auto node = createNodeWithParams("test_max_intervals_node", { + rclcpp::Parameter("max_intervals", 2), + rclcpp::Parameter("test_channel.num_intervals", 3), + rclcpp::Parameter("test_channel.interval_0.type", "constant"), + rclcpp::Parameter("test_channel.interval_0.t_start", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_end", 10.0), + rclcpp::Parameter("test_channel.interval_0.y_start", 1.0), + rclcpp::Parameter("test_channel.interval_0.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_0.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_0.y_mid", 0.0), + rclcpp::Parameter("test_channel.interval_1.type", "constant"), + rclcpp::Parameter("test_channel.interval_1.t_start", 10.0), + rclcpp::Parameter("test_channel.interval_1.t_end", 20.0), + rclcpp::Parameter("test_channel.interval_1.y_start", 2.0), + rclcpp::Parameter("test_channel.interval_1.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_1.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_1.y_mid", 0.0), + rclcpp::Parameter("test_channel.interval_2.type", "constant"), + rclcpp::Parameter("test_channel.interval_2.t_start", 20.0), + rclcpp::Parameter("test_channel.interval_2.t_end", 30.0), + rclcpp::Parameter("test_channel.interval_2.y_start", 3.0), + rclcpp::Parameter("test_channel.interval_2.y_end", 0.0), + rclcpp::Parameter("test_channel.interval_2.t_mid", 0.0), + rclcpp::Parameter("test_channel.interval_2.y_mid", 0.0) + }); + + std::vector channels = { "test_channel" }; + Simulator sim(node.get(), channels); + + // Only first 2 intervals should be loaded + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 5.0), 1.0); // First interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 15.0), 2.0); // Second interval + EXPECT_DOUBLE_EQ(sim.get_object_value("test_channel", 25.0), 2.0); // Third interval should not exist, holds second +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp b/src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp new file mode 100644 index 0000000..e7a2674 --- /dev/null +++ b/src/g2_2025_odometry_pkg/test/test_wheel_simulator.cpp @@ -0,0 +1,168 @@ +#include +#include +#include +#include "g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp" +#include + +using namespace assignments::three::wheel_data_simulator_node; + +class WheelSimulatorTest : public ::testing::Test { +protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + void setupBasicWheelParams(rclcpp::Node* node) { + node->declare_parameter("max_intervals", 4); + node->declare_parameter("publish_rate", 10.0); + + // Setup wheel_fl + node->declare_parameter("wheel_fl.num_intervals", 1); + node->declare_parameter("wheel_fl.interval_0.type", "linear"); + node->declare_parameter("wheel_fl.interval_0.t_start", 0.0); + node->declare_parameter("wheel_fl.interval_0.t_end", 10.0); + node->declare_parameter("wheel_fl.interval_0.y_start", 0.0); + node->declare_parameter("wheel_fl.interval_0.y_end", 5.0); + node->declare_parameter("wheel_fl.interval_0.t_mid", 0.0); + node->declare_parameter("wheel_fl.interval_0.y_mid", 0.0); + + // Setup other wheels with no intervals + for (const auto& wheel : {"wheel_fr", "wheel_bl", "wheel_br"}) { + node->declare_parameter(std::string(wheel) + ".num_intervals", 0); + } + } +}; + +// Test node initialization +TEST_F(WheelSimulatorTest, NodeInitialization) { + auto node = std::make_shared("test_wheel_init_node"); + setupBasicWheelParams(node.get()); + + // Should not throw + EXPECT_NO_THROW({ + DataSimulator sim; + }); +} + +// Test wheel message publishing +TEST_F(WheelSimulatorTest, WheelDataPublishing) { + auto test_node = std::make_shared("test_wheel_pub_node"); + + std_msgs::msg::Float64MultiArray::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_wheel_data", + 10, + [&received_msg](std_msgs::msg::Float64MultiArray::SharedPtr msg) { + received_msg = msg; + } + ); + + auto sim_node = std::make_shared(); + + // Spin a few times to allow publishing + for (int i = 0; i < 5; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + // Check that we received a message + ASSERT_NE(received_msg, nullptr); +} + +// Test wheel data array size +TEST_F(WheelSimulatorTest, WheelDataArraySize) { + auto test_node = std::make_shared("test_wheel_size_node"); + + std_msgs::msg::Float64MultiArray::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_wheel_data", + 10, + [&received_msg](std_msgs::msg::Float64MultiArray::SharedPtr msg) { + if (!received_msg) { + received_msg = msg; + } + } + ); + + auto sim_node = std::make_shared(); + + // Spin until we receive a message + for (int i = 0; i < 10 && !received_msg; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + ASSERT_NE(received_msg, nullptr); + // Should have 4 wheel values: FL, FR, RL, RR + EXPECT_EQ(received_msg->data.size(), 4); +} + +// Test wheel velocity values are finite +TEST_F(WheelSimulatorTest, ValidVelocityValues) { + auto test_node = std::make_shared("test_wheel_values_node"); + + std_msgs::msg::Float64MultiArray::SharedPtr received_msg; + auto subscription = test_node->create_subscription( + "simulated_wheel_data", + 10, + [&received_msg](std_msgs::msg::Float64MultiArray::SharedPtr msg) { + if (!received_msg) { + received_msg = msg; + } + } + ); + + auto sim_node = std::make_shared(); + + // Spin until we receive a message + for (int i = 0; i < 10 && !received_msg; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + ASSERT_NE(received_msg, nullptr); + ASSERT_EQ(received_msg->data.size(), 4); + + // All values should be finite + for (const auto& val : received_msg->data) { + EXPECT_TRUE(std::isfinite(val)); + } +} + +// Test multiple wheel messages received +TEST_F(WheelSimulatorTest, MultipleMessagesReceived) { + auto test_node = std::make_shared("test_multi_wheel_node"); + + int msg_count = 0; + auto subscription = test_node->create_subscription( + "simulated_wheel_data", + 10, + [&msg_count](std_msgs::msg::Float64MultiArray::SharedPtr) { + msg_count++; + } + ); + + auto sim_node = std::make_shared(); + + // Spin for a bit to receive messages + for (int i = 0; i < 10; i++) { + rclcpp::spin_some(sim_node); + rclcpp::spin_some(test_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + // Should have received at least one message + EXPECT_GT(msg_count, 0); +} + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}