feat(wheel_pos_aprox): linked to the sim and refined calcs

src/g2_2025_odometry_pkg/CMakeLists.txt

@@ -22,6 +22,8 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -107,19 +109,37 @@ install(DIRECTORY launch config
   DESTINATION share/${PROJECT_NAME}/
 )
 
-add_executable(odometry_node src/odometry_node.cpp)
+# add_executable(odometry_node src/odometry_node.cpp)
 
-ament_target_dependencies(odometry_node rclcpp nav_msgs geometry_msgs)
-install(TARGETS
-  odometry_node
-  DESTINATION lib/${PROJECT_NAME}
-)
+# ament_target_dependencies(odometry_node rclcpp nav_msgs geometry_msgs)
+# install(TARGETS
+#   odometry_node
+#   DESTINATION lib/${PROJECT_NAME}
+# )
 
-add_executable(wheel_position_approximator_node src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.cpp)
+add_executable(wheel_position_approximator_node
+ src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.cpp
+ src/g2_2025_wheel_approximator_node/main.cpp
+ )
 
 ament_target_dependencies(wheel_position_approximator_node rclcpp nav_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
   wheel_position_approximator_node
+  wheel_data_simulator_node
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/main.cpp

@@ -1,14 +1,11 @@
 #include "rclcpp/rclcpp.hpp"
-#include "WheelPositionApproximator.hpp"
+#include "nodes/WheelPositionApproximator.hpp"
 
-namespace g2_2025_odometry_pkg
+int main(int argc, char **argv)
 {
-    int main(int argc, char **argv)
-    {
-        rclcpp::init(argc, argv);
-        auto node = std::make_shared<WheelPositionApproximator>();
-        rclcpp::spin(node);
-        rclcpp::shutdown();
-        return 0;
-    }
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<assignments::three::wheel_position_approximator_node::WheelPositionApproximator>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
 }

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

@@ -1,77 +1,93 @@
 #include "WheelPositionApproximator.hpp"
 
-
-// wheel position approximator node implementation
-// data in from simulated wheel encoders to approximate robot position
-//are: wheel radius, lx, ly, time.
-// wfl, wfr, wrl, wrr
-// outputs: vx, vy, theta
-// integrate over time to get position
-// position = integral of velocity over time + initial position for both x and y
-
-// data in from simulated imu to approximate robot orientation
-// are: vx, vy, vtheta and wheel radius
-// integrate over time to get orientation
-// orientation = integral of angular velocity over time + initial orientation
-
-namespace g2_2025_odometry_pkg {
+namespace assignments::three::wheel_position_approximator_node
+{
 WheelPositionApproximator::WheelPositionApproximator() : Node("wheel_position_approximator")
 {
-    r_ = this->declare_parameter<double>("wheel_radius", 0.0);
-    lx_ = this->declare_parameter<double>("lx", 0.0);
-    ly_ = this->declare_parameter<double>("ly", 0.0);
+    radius_ = this->declare_parameter<double>("wheel_radius", 0.2);
+    lx_ = this->declare_parameter<double>("lx", 0.3);
+    ly_ = this->declare_parameter<double>("ly", 0.6);
 
-    odom_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>("wheel_odometry", 10);
+    wheel_encoder_sim_subscriber_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
+        "simulated_wheel_data",
+        10,
+        [this](const std_msgs::msg::Float64MultiArray::SharedPtr msg) {
+            wfl = msg->data[0];
+            wfr = msg->data[1];
+            wrl = msg->data[2];
+            wrr = msg->data[3];
+        }
+    );
+    RCLCPP_INFO(this->get_logger(), "Subscribed to topic 'simulated_wheel_data'");
+
+    //test values
+    // wfl = 1.0;
+    // wfr = 0.4;
+    // wrl = 1.0;
+    // wrr = 0.4;
+
+    position_publisher = this->create_publisher<nav_msgs::msg::Odometry>("car_position", 10);
     timer_ = this->create_wall_timer(
         std::chrono::milliseconds(100),
         std::bind(&WheelPositionApproximator::publish_odometry, this)
     );
+    RCLCPP_INFO(this->get_logger(), "Publisher to topic 'car_position' initialized");
 }
 
 
 
-void WheelPositionApproximator::V_x(){
-
-    double vx = r_ * ( /*wfl + wfr + wrl + wrr*/ 0 ) / 4.0;
-
-
+double WheelPositionApproximator::V_x() {
+    double vx = radius_ * (wfl + wfr + wrl + wrr) / 4.0;
+    return vx;
 }
 
-void WheelPositionApproximator::V_y(){
-    double vy = r_ * ( /*-wfl + wfr + wrl - wrr*/ 0 ) / 4.0;
-
-
-}
-void WheelPositionApproximator::V_theta(){
-
-    double theta = r_ * ( /*-wfl + wfr - wrl + wrr*/ 0 ) / (4.0 * (lx_ + ly_));
-
+double WheelPositionApproximator::V_y() {
+    double vy = radius_ * (-wfl + wfr + wrl - wrr) / 4.0;
+    return vy;
 }
 
-void WheelPositionApproximator::postion_calc(){
-    // integrate vx, vy, theta over time to get position
-    // position = integral of velocity over time + initial position for both x and y
+double WheelPositionApproximator::theata_z() {
+    double sum = lx_ + ly_;
+    double theta = radius_ * (-wfl + wfr - wrl + wrr) / (4.0 * sum);
+    return theta;
+}
+
+void WheelPositionApproximator::postion_calc() {
+    V_x_ = V_x();
+    V_y_ = V_y();
+    V_theta_ = theata_z();
+    // RCLCPP_INFO(this->get_logger(), "Vx: %.2f, Vy: %.2f, Vtheta: %.2f", V_x_, V_y_, V_theta_);
+
+    x_ = V_x_ * time_elapsed + initial_x;
+    y_ = V_y_ * time_elapsed + initial_y;
+    theta_ = V_theta_ * time_elapsed + theta_;
+
+    initial_x = x_;
+    initial_y = y_;
+    initial_theta = theta_;
+    RCLCPP_INFO(this->get_logger(), "Position - x: %.2f, y: %.2f, theta: %.2f", x_, y_, theta_);
+
 }
 
 void WheelPositionApproximator::publish_odometry()
 {
+    postion_calc();
+
     auto odom_msg = nav_msgs::msg::Odometry();
     odom_msg.header.stamp = this->now();
     odom_msg.header.frame_id = "odom";
     odom_msg.child_frame_id = "base_link";
 
 
-    odom_msg.pose.pose.position.x = 0.0;
-    odom_msg.pose.pose.position.y = 0.0;
+    odom_msg.pose.pose.position.x = x_;
+    odom_msg.pose.pose.position.y = y_;
     odom_msg.pose.pose.position.z = 0.0;
 
     odom_msg.pose.pose.orientation.x = 0.0;
     odom_msg.pose.pose.orientation.y = 0.0;
-    odom_msg.pose.pose.orientation.z = 0.0;
+    odom_msg.pose.pose.orientation.z = theta_;
     odom_msg.pose.pose.orientation.w = 1.0;
-
-    // Publish the odometry message
-    odom_publisher_->publish(odom_msg);
+    position_publisher->publish(odom_msg);
 
 }
 }  // namespace g2_2025_odometry_pkg

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

@@ -1,23 +1,50 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/odometry.hpp"
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include <vector>
+#include <string>
+#include <map>
+#include <memory>
 
-namespace g2_2025_odometry_pkg
+
+namespace assignments::three::wheel_position_approximator_node
 {
     class WheelPositionApproximator : public rclcpp::Node
     {
     public:
         WheelPositionApproximator();
-        void V_x();
-        void V_y();
-        void V_theta();
+        double V_x();
+        double V_y();
+        double theata_z();
         void postion_calc();
     private:
         void publish_odometry();
-        rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
+        rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr position_publisher;
+        rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr wheel_encoder_sim_subscriber_;
         rclcpp::TimerBase::SharedPtr timer_;
-        double r_;
+        double initial_x = 0.0;
+        double initial_y = 0.0;
+        double initial_theta = 0.0;
+        double time_elapsed = 1;
+
+        double radius_;
         double lx_;
         double ly_;
 
+        double V_x_;
+        double V_y_;
+        double V_theta_;
+
+        double wfl = 0.0;
+        double wfr = 0.0;
+        double wrl = 0.0;
+        double wrr = 0.0;
+
+
+        // Position state
+        double x_ = 0.0;
+        double y_ = 0.0;
+        double theta_ = 0.0;
+
     };
 }  // namespace g2_2025_odometry_pkg

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