diff --git a/src/g2_2025_odometry_pkg/CMakeLists.txt b/src/g2_2025_odometry_pkg/CMakeLists.txt index 0a47718..ed91084 100644 --- a/src/g2_2025_odometry_pkg/CMakeLists.txt +++ b/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} ) diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/main.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/main.cpp index a8b6562..3a5cfbe 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/main.cpp +++ b/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(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; - } + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; } diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.cpp index be8a80c..dafa1e7 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.cpp +++ b/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("wheel_radius", 0.0); - lx_ = this->declare_parameter("lx", 0.0); - ly_ = this->declare_parameter("ly", 0.0); + radius_ = this->declare_parameter("wheel_radius", 0.2); + lx_ = this->declare_parameter("lx", 0.3); + ly_ = this->declare_parameter("ly", 0.6); - odom_publisher_ = this->create_publisher("wheel_odometry", 10); + wheel_encoder_sim_subscriber_ = this->create_subscription( + "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("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 diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.hpp b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.hpp index a8c6a0d..da0e1e8 100644 --- a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.hpp +++ b/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 +#include +#include +#include +#include -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::SharedPtr odom_publisher_; + rclcpp::Publisher::SharedPtr position_publisher; + rclcpp::Subscription::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 diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp new file mode 100644 index 0000000..80f6c6f --- /dev/null +++ b/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(this, wheels_); + + wheel_publisher_ = this->create_publisher("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(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(); + + // 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 diff --git a/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp b/src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp new file mode 100644 index 0000000..b3f0012 --- /dev/null +++ b/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 +#include +#include +#include + +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::SharedPtr wheel_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Time start_time_; + + std::vector wheels_; + std::unique_ptr simulator_; + + void publish_wheel_data(); + +}; + +} // namespace assignments::three::wheel_data_simulator_node