ros2-assignments/src/g2_2025_odometry_pkg/test/WheelPositionApproximator.test.cpp

#include <gtest/gtest.h>
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <cmath>
#include <memory>

#include "../src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.hpp"

using namespace assignments::three::wheel_position_approximator_node;

class WheelPositionApproximatorTest : public ::testing::Test {
protected:
    void SetUp() override {
        rclcpp::init(0, nullptr);
    }

    void TearDown() override {
        rclcpp::shutdown();
    }

    // Helper to create node - parameters must be set via ROS parameter server or launch
    std::shared_ptr<WheelPositionApproximator> createNode()
    {
        return std::make_shared<WheelPositionApproximator>();
    }

    // Helper to publish wheel data
    void publishWheelData(
        rclcpp::Node::SharedPtr node,
        double wfl, double wfr, double wrl, double wrr)
    {
        auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>(
            "simulated_wheel_data", 10);

        auto msg = std_msgs::msg::Float64MultiArray();
        msg.data = {wfl, wfr, wrl, wrr};

        // Give time for subscription to be established
        rclcpp::sleep_for(std::chrono::milliseconds(100));

        publisher->publish(msg);

        // Spin to process the message
        rclcpp::spin_some(node);
        rclcpp::sleep_for(std::chrono::milliseconds(100));
    }
};// Test 1: Node Creation and Initialization
TEST_F(WheelPositionApproximatorTest, NodeCreation) {
    auto node = createNode();
    ASSERT_NE(node, nullptr);
    EXPECT_EQ(node->get_name(), std::string("wheel_position_approximator"));
}

// Test 2: Parameter Loading with Defaults
TEST_F(WheelPositionApproximatorTest, ParameterLoading) {
    auto node = createNode();

    // Check default parameters
    EXPECT_DOUBLE_EQ(node->get_parameter("wheel_radius").as_double(), 0.2);
    EXPECT_DOUBLE_EQ(node->get_parameter("lx").as_double(), 0.3);
    EXPECT_DOUBLE_EQ(node->get_parameter("ly").as_double(), 0.6);
    EXPECT_DOUBLE_EQ(node->get_parameter("time_elapsed").as_double(), 1.0);
    EXPECT_DOUBLE_EQ(node->get_parameter("initial_x").as_double(), 0.0);
    EXPECT_DOUBLE_EQ(node->get_parameter("initial_y").as_double(), 0.0);
    EXPECT_DOUBLE_EQ(node->get_parameter("initial_theta").as_double(), 0.0);
}

// Test 3: Publisher Creation
TEST_F(WheelPositionApproximatorTest, PublisherCreation) {
    auto node = createNode();

    auto topic_names = node->get_topic_names_and_types();
    bool found_car_position = false;

    for (const auto& [topic, types] : topic_names) {
        if (topic == "/car_position") {
            found_car_position = true;
            EXPECT_EQ(types.size(), 1);
            EXPECT_EQ(types[0], "nav_msgs/msg/Odometry");
        }
    }

    EXPECT_TRUE(found_car_position);
}

// Test 4: Subscriber Creation
TEST_F(WheelPositionApproximatorTest, SubscriberCreation) {
    auto node = createNode();

    auto topic_names = node->get_topic_names_and_types();
    bool found_wheel_data = false;

    for (const auto& [topic, types] : topic_names) {
        if (topic == "/simulated_wheel_data") {
            found_wheel_data = true;
        }
    }

    EXPECT_TRUE(found_wheel_data);
}

// Test 5: Odometry Message Publishing
TEST_F(WheelPositionApproximatorTest, OdometryMessagePublishing) {
    auto node = createNode();    bool message_received = false;
    nav_msgs::msg::Odometry::SharedPtr received_msg;

    auto subscription = node->create_subscription<nav_msgs::msg::Odometry>(
        "car_position", 10,
        [&](nav_msgs::msg::Odometry::SharedPtr msg) {
            message_received = true;
            received_msg = msg;
        });

    // Wait for timer to trigger and publish
    for (int i = 0; i < 20 && !message_received; ++i) {
        rclcpp::spin_some(node);
        rclcpp::sleep_for(std::chrono::milliseconds(50));
    }

    EXPECT_TRUE(message_received);
    if (message_received) {
        EXPECT_EQ(received_msg->header.frame_id, "odom");
        EXPECT_EQ(received_msg->child_frame_id, "base_link");
    }
}

// Test 6: Straight Line Movement (All wheels same speed)
TEST_F(WheelPositionApproximatorTest, StraightLineMovement) {
    auto node = createNode();

    bool message_received = false;
    nav_msgs::msg::Odometry::SharedPtr received_msg;

    auto subscription = node->create_subscription<nav_msgs::msg::Odometry>(
        "car_position", 10,
        [&](nav_msgs::msg::Odometry::SharedPtr msg) {
            message_received = true;
            received_msg = msg;
        });

    // Publish wheel data - all wheels same speed (straight line)
    publishWheelData(node, 1.0, 1.0, 1.0, 1.0);

    // Wait for odometry message
    for (int i = 0; i < 20 && !message_received; ++i) {
        rclcpp::spin_some(node);
        rclcpp::sleep_for(std::chrono::milliseconds(50));
    }

    EXPECT_TRUE(message_received);
    if (message_received) {
        // With all wheels at 1.0 rad/s, vx should be 0.2 m/s
        // After 1 second at timer rate, position should have moved in x
        // Note: exact values depend on timing, so we check movement occurred
        EXPECT_GT(std::abs(received_msg->pose.pose.position.x), 0.0);
        // vy should be 0 (no lateral movement)
        EXPECT_NEAR(received_msg->pose.pose.position.y, 0.0, 0.01);
        // theta should be 0 (no rotation)
        EXPECT_NEAR(received_msg->pose.pose.orientation.z, 0.0, 0.01);
    }
}

// Test 7: Rotation (Differential wheel speeds)
TEST_F(WheelPositionApproximatorTest, RotationalMovement) {
    auto node = createNode();

    bool message_received = false;
    nav_msgs::msg::Odometry::SharedPtr received_msg;

    auto subscription = node->create_subscription<nav_msgs::msg::Odometry>(
        "car_position", 10,
        [&](nav_msgs::msg::Odometry::SharedPtr msg) {
            message_received = true;
            received_msg = msg;
        });

    // Publish wheel data - left slower than right (turning left)
    publishWheelData(node, 0.5, 2.0, 0.5, 2.0);

    // Wait for odometry message
    for (int i = 0; i < 20 && !message_received; ++i) {
        rclcpp::spin_some(node);
        rclcpp::sleep_for(std::chrono::milliseconds(50));
    }

    EXPECT_TRUE(message_received);
    if (message_received) {
        // Should have some rotation (non-zero theta in quaternion)
        double qz = received_msg->pose.pose.orientation.z;
        double qw = received_msg->pose.pose.orientation.w;
        double theta_from_quat = 2.0 * std::atan2(qz, qw);
        EXPECT_GT(std::abs(theta_from_quat), 0.0);
    }
}

// Test 8: Zero Wheel Velocities (Stationary)
TEST_F(WheelPositionApproximatorTest, StationaryRobot) {
    auto node = createNode();

    bool message_received = false;
    nav_msgs::msg::Odometry::SharedPtr received_msg;

    auto subscription = node->create_subscription<nav_msgs::msg::Odometry>(
        "car_position", 10,
        [&](nav_msgs::msg::Odometry::SharedPtr msg) {
            message_received = true;
            received_msg = msg;
        });

    // Publish zero wheel velocities
    publishWheelData(node, 0.0, 0.0, 0.0, 0.0);

    // Wait for odometry message
    for (int i = 0; i < 20 && !message_received; ++i) {
        rclcpp::spin_some(node);
        rclcpp::sleep_for(std::chrono::milliseconds(50));
    }

    EXPECT_TRUE(message_received);
    if (message_received) {
        // Position should remain near origin (default initial values)
        EXPECT_NEAR(received_msg->pose.pose.position.x, 0.0, 0.1);
        EXPECT_NEAR(received_msg->pose.pose.position.y, 0.0, 0.1);
    }
}

// Test 9: Quaternion Validity
TEST_F(WheelPositionApproximatorTest, QuaternionValidity) {
    auto node = createNode();

    bool message_received = false;
    nav_msgs::msg::Odometry::SharedPtr received_msg;

    auto subscription = node->create_subscription<nav_msgs::msg::Odometry>(
        "car_position", 10,
        [&](nav_msgs::msg::Odometry::SharedPtr msg) {
            message_received = true;
            received_msg = msg;
        });

    publishWheelData(node, 1.0, 2.0, 1.0, 2.0);

    for (int i = 0; i < 20 && !message_received; ++i) {
        rclcpp::spin_some(node);
        rclcpp::sleep_for(std::chrono::milliseconds(50));
    }

    EXPECT_TRUE(message_received);
    if (message_received) {
        // Check quaternion is normalized
        double qx = received_msg->pose.pose.orientation.x;
        double qy = received_msg->pose.pose.orientation.y;
        double qz = received_msg->pose.pose.orientation.z;
        double qw = received_msg->pose.pose.orientation.w;

        double norm = std::sqrt(qx*qx + qy*qy + qz*qz + qw*qw);
        EXPECT_NEAR(norm, 1.0, 0.001);

        // For 2D rotation, x and y should be 0
        EXPECT_DOUBLE_EQ(qx, 0.0);
        EXPECT_DOUBLE_EQ(qy, 0.0);
    }
}

// Main function
int main(int argc, char **argv) {
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}