feat(launch&test): merged launchfile with sim & added test for wheel pos aprox + fixs

src/g2_2025_odometry_pkg/CMakeLists.txt

@@ -138,7 +138,7 @@ target_include_directories(wheel_data_simulator_node PRIVATE
 ament_target_dependencies(wheel_data_simulator_node rclcpp std_msgs)
 
 install(
-    DIRECTORY launch
+    DIRECTORY launch config
     DESTINATION share/${PROJECT_NAME}/
 )
 

src/g2_2025_odometry_pkg/launch/odom.launch.xml

@@ -1,9 +0,0 @@
-<launch>
-    <let name="yaml_path" value="$(find-pkg-share g2_2025_odometry_pkg)/config/opt.yaml" />
-    <node pkg="g2_2025_odometry_pkg" exec="wheel_data_simulator_node">
-        <param name="yaml_path" value="$(var yaml_path)" />
-    </node>
-
-    <node pkg="g2_2025_odometry_pkg" exec="wheel_position_approximator_node" />
-
-</launch>

src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.cpp

@@ -1,4 +1,5 @@
 #include "WheelPositionApproximator.hpp"
+#include <cmath>
 
 namespace assignments::three::wheel_position_approximator_node
 {
@@ -75,6 +76,12 @@ double WheelPositionApproximator::calculate_V_theta()
     return radius_ * (-wfl_ + wfr_ - wrl_ + wrr_) / (4.0 * (lx_ + ly_));
 }
 
+double WheelPositionApproximator::normalize_angle(double angle) {
+    while (angle > M_PI) angle -= 2.0 * M_PI;
+    while (angle < -M_PI) angle += 2.0 * M_PI;
+    return angle;
+}
+
 void WheelPositionApproximator::calculate_position()
 {
     double vx = calculate_V_x();
@@ -82,13 +89,17 @@ void WheelPositionApproximator::calculate_position()
     double vtheta = calculate_V_theta();
 
     theta_ += vtheta * time_elapsed_;
+
+    theta_ = normalize_angle(theta_);
+
     double delta_x = (vx * std::cos(theta_) - vy * std::sin(theta_)) * time_elapsed_;
     double delta_y = (vx * std::sin(theta_) + vy * std::cos(theta_)) * time_elapsed_;
 
     x_ += delta_x;
     y_ += delta_y;
 
-    RCLCPP_INFO(this->get_logger(), "Position - x: %.3f, y: %.3f, theta: %.3f rad", x_, y_, theta_);
+    RCLCPP_INFO(this->get_logger(), "Position - x: %.3f, y: %.3f, theta: %.3f rad (%.1f deg)",
+                x_, y_, theta_, theta_ * 180.0 / M_PI);
 }
 
 void WheelPositionApproximator::publish_odometry()

src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.hpp

@@ -17,6 +17,7 @@ namespace assignments::three::wheel_position_approximator_node
         double calculate_V_x();
         double calculate_V_y();
         double calculate_V_theta();
+        double normalize_angle(double angle);
 
         rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr position_publisher_;
         rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr wheel_encoder_subscriber_;

src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp

@@ -35,18 +35,15 @@ DataSimulator::~DataSimulator() {
 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)
+        simulator_->get_object_value("wheel_fl", elapsed_time),
+        simulator_->get_object_value("wheel_fr", elapsed_time),
+        simulator_->get_object_value("wheel_rl", elapsed_time),
+        simulator_->get_object_value("wheel_rr", elapsed_time)
     };
 
     RCLCPP_INFO(this->get_logger(),

src/g2_2025_odometry_pkg/test/WheelPositionApproximator.test.cpp

@@ -0,0 +1,270 @@
+#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();
+}