fix(wheel_pos_aprox): fixed clacs & launch file

src/g2_2025_odometry_pkg/CMakeLists.txt

@@ -137,6 +137,11 @@ target_include_directories(wheel_data_simulator_node PRIVATE
 
 ament_target_dependencies(wheel_data_simulator_node rclcpp std_msgs)
 
+install(
+    DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME}/
+)
+
 install(TARGETS
   wheel_position_approximator_node
   wheel_data_simulator_node

src/g2_2025_odometry_pkg/launch/odom.launch.xml

@@ -0,0 +1,9 @@
+<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

@@ -2,76 +2,100 @@
 
 namespace assignments::three::wheel_position_approximator_node
 {
-WheelPositionApproximator::WheelPositionApproximator() : Node("wheel_position_approximator")
+
+WheelPositionApproximator::WheelPositionApproximator()
+    : Node("wheel_position_approximator"),
+    radius_(0.0),
+    lx_(0.0),
+    ly_(0.0),
+    time_elapsed_(1.0),
+    wfl_(0.0),
+    wfr_(0.0),
+    wrl_(0.0),
+    wrr_(0.0),
+    x_(0.0),
+    y_(0.0),
+    theta_(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);
+    x_ = this->declare_parameter<double>("initial_x", 0.0);
+    y_ = this->declare_parameter<double>("initial_y", 0.0);
+    theta_ = this->declare_parameter<double>("initial_theta", 0.0);
+    time_elapsed_ = this->declare_parameter<double>("time_elapsed", 1.0);
 
-    wheel_encoder_sim_subscriber_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
+    RCLCPP_INFO(this->get_logger(), "WheelPositionApproximator node started");
+    RCLCPP_INFO(this->get_logger(), "Parameters - radius: %.2f, lx: %.2f, ly: %.2f, dt: %.2f",
+                radius_, lx_, ly_, time_elapsed_);
+
+    wheel_encoder_subscriber_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
         "simulated_wheel_data",
         10,
         [this](const std_msgs::msg::Float64MultiArray::SharedPtr msg) {
-            wfl = msg->data[0];
+            if (msg->data.size() >= 4) {
-            wfr = msg->data[1];
+                wfl_ = msg->data[0];
-            wrl = msg->data[2];
+                wfr_ = msg->data[1];
-            wrr = msg->data[3];
+                wrl_ = msg->data[2];
+                wrr_ = msg->data[3];
+            }
         }
     );
-    RCLCPP_INFO(this->get_logger(), "Subscribed to topic 'simulated_wheel_data'");
 
-    //test values
+    // //wheel data placeholder
-    // wfl = 1.0;
+    // wfl_ = 1.0;
-    // wfr = 0.4;
+    // wfr_ = 0.5;
-    // wrl = 1.0;
+    // wrl_ = 0.5;
-    // wrr = 0.4;
+    // wrr_ = 1.0;
+
+    position_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>("car_position", 10);
 
-    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");
+
+    RCLCPP_INFO(this->get_logger(), "Subscribed to 'simulated_wheel_data', publishing to 'car_position'");
 }
 
 
 
-double WheelPositionApproximator::V_x() {
+double WheelPositionApproximator::calculate_V_x()
-    double vx = radius_ * (wfl + wfr + wrl + wrr) / 4.0;
+{
-    return vx;
+    return radius_ * (wfl_ + wfr_ + wrl_ + wrr_) / 4.0;
 }
 
-double WheelPositionApproximator::V_y() {
+double WheelPositionApproximator::calculate_V_y()
-    double vy = radius_ * (-wfl + wfr + wrl - wrr) / 4.0;
+{
-    return vy;
+    return radius_ * (-wfl_ + wfr_ + wrl_ - wrr_) / 4.0;
 }
 
-double WheelPositionApproximator::theata_z() {
+double WheelPositionApproximator::calculate_V_theta()
-    double sum = lx_ + ly_;
+{
-    double theta = radius_ * (-wfl + wfr - wrl + wrr) / (4.0 * sum);
+    return radius_ * (-wfl_ + wfr_ - wrl_ + wrr_) / (4.0 * (lx_ + ly_));
-    return theta;
 }
 
-void WheelPositionApproximator::postion_calc() {
+void WheelPositionApproximator::calculate_position()
-    V_x_ = V_x();
+{
-    V_y_ = V_y();
+    double vx = calculate_V_x();
-    V_theta_ = theata_z();
+    double vy = calculate_V_y();
-    // RCLCPP_INFO(this->get_logger(), "Vx: %.2f, Vy: %.2f, Vtheta: %.2f", V_x_, V_y_, V_theta_);
+    double vtheta = calculate_V_theta();
 
-    x_ = V_x_ * time_elapsed + initial_x;
+    theta_ += vtheta * time_elapsed_;
-    y_ = V_y_ * time_elapsed + initial_y;
+    double delta_x = (vx * std::cos(theta_) - vy * std::sin(theta_)) * time_elapsed_;
-    theta_ = V_theta_ * time_elapsed + theta_;
+    double delta_y = (vx * std::sin(theta_) + vy * std::cos(theta_)) * time_elapsed_;
 
-    initial_x = x_;
+    x_ += delta_x;
-    initial_y = y_;
+    y_ += delta_y;
-    initial_theta = theta_;
-    RCLCPP_INFO(this->get_logger(), "Position - x: %.2f, y: %.2f, theta: %.2f", x_, y_, theta_);
 
+    RCLCPP_INFO(this->get_logger(), "Position - x: %.3f, y: %.3f, theta: %.3f rad", x_, y_, theta_);
 }
 
 void WheelPositionApproximator::publish_odometry()
 {
-    postion_calc();
+    //tip: probeer plotjuggler om data te visualiseren ;p
+
+    calculate_position();
 
     auto odom_msg = nav_msgs::msg::Odometry();
     odom_msg.header.stamp = this->now();
@@ -85,9 +109,10 @@ void WheelPositionApproximator::publish_odometry()
 
     odom_msg.pose.pose.orientation.x = 0.0;
     odom_msg.pose.pose.orientation.y = 0.0;
-    odom_msg.pose.pose.orientation.z = theta_;
+    odom_msg.pose.pose.orientation.z = std::sin(theta_ / 2.0);
-    odom_msg.pose.pose.orientation.w = 1.0;
+    odom_msg.pose.pose.orientation.w = std::cos(theta_ / 2.0);
-    position_publisher->publish(odom_msg);
 
+    position_publisher_->publish(odom_msg);
 }
-}  // namespace g2_2025_odometry_pkg
+
+}  // namespace assignments::three::wheel_position_approximator_node

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

@@ -1,11 +1,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/odometry.hpp"
-#include <std_msgs/msg/float64_multi_array.hpp>
+#include <cmath>
-#include <vector>
+#include "std_msgs/msg/float64_multi_array.hpp"
-#include <string>
-#include <map>
-#include <memory>
-
 
 namespace assignments::three::wheel_position_approximator_node
 {
@@ -13,38 +9,32 @@ namespace assignments::three::wheel_position_approximator_node
     {
     public:
         WheelPositionApproximator();
-        double V_x();
+
-        double V_y();
-        double theata_z();
-        void postion_calc();
     private:
         void publish_odometry();
-        rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr position_publisher;
+        void calculate_position();
-        rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr wheel_encoder_sim_subscriber_;
+
+        double calculate_V_x();
+        double calculate_V_y();
+        double calculate_V_theta();
+
+        rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr position_publisher_;
+        rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr wheel_encoder_subscriber_;
         rclcpp::TimerBase::SharedPtr timer_;
-        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 time_elapsed_;
 
-        double V_x_;
+        double wfl_;
-        double V_y_;
+        double wfr_;
-        double V_theta_;
+        double wrl_;
-
+        double wrr_;
-        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;
 
+        double x_;
+        double y_;
+        double theta_;
     };
-}  // namespace g2_2025_odometry_pkg
+
+}  // namespace assignments::three::wheel_position_approximator_node