feat(wheel_pos_aprox): added nodes and updated cmakelists

src/g2_2025_odometry_pkg/CMakeLists.txt

@@ -20,6 +20,9 @@ FetchContent_MakeAvailable(tomlplusplus)
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 
@@ -104,6 +107,22 @@ install(DIRECTORY launch config
   DESTINATION share/${PROJECT_NAME}/
 )
 
+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}
+)
+
+add_executable(wheel_position_approximator_node src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.cpp)
+
+ament_target_dependencies(wheel_position_approximator_node rclcpp nav_msgs geometry_msgs)
+install(TARGETS
+  wheel_position_approximator_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
 

src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/main.cpp

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

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

@@ -0,0 +1,77 @@
+#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 {
+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);
+
+    odom_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>("wheel_odometry", 10);
+    timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(100),
+        std::bind(&WheelPositionApproximator::publish_odometry, this)
+    );
+}
+
+
+
+void WheelPositionApproximator::V_x(){
+
+    double vx = r_ * ( /*wfl + wfr + wrl + wrr*/ 0 ) / 4.0;
+
+
+}
+
+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_));
+
+}
+
+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
+}
+
+void WheelPositionApproximator::publish_odometry()
+{
+    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.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.w = 1.0;
+
+    // Publish the odometry message
+    odom_publisher_->publish(odom_msg);
+
+}
+}  // namespace g2_2025_odometry_pkg

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

@@ -0,0 +1,23 @@
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+
+namespace g2_2025_odometry_pkg
+{
+    class WheelPositionApproximator : public rclcpp::Node
+    {
+    public:
+        WheelPositionApproximator();
+        void V_x();
+        void V_y();
+        void V_theta();
+        void postion_calc();
+    private:
+        void publish_odometry();
+        rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
+        rclcpp::TimerBase::SharedPtr timer_;
+        double r_;
+        double lx_;
+        double ly_;
+
+    };
+}  // namespace g2_2025_odometry_pkg