diff --git a/src/g2_2025_odometry_pkg/CMakeLists.txt b/src/g2_2025_odometry_pkg/CMakeLists.txt index ed91084..949e397 100644 --- a/src/g2_2025_odometry_pkg/CMakeLists.txt +++ b/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 diff --git a/src/g2_2025_odometry_pkg/launch/odom.launch.xml b/src/g2_2025_odometry_pkg/launch/odom.launch.xml new file mode 100644 index 0000000..6bcc6f7 --- /dev/null +++ b/src/g2_2025_odometry_pkg/launch/odom.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + 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 dafa1e7..5706a1c 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 @@ -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("wheel_radius", 0.2); lx_ = this->declare_parameter("lx", 0.3); ly_ = this->declare_parameter("ly", 0.6); + x_ = this->declare_parameter("initial_x", 0.0); + y_ = this->declare_parameter("initial_y", 0.0); + theta_ = this->declare_parameter("initial_theta", 0.0); + time_elapsed_ = this->declare_parameter("time_elapsed", 1.0); - wheel_encoder_sim_subscriber_ = this->create_subscription( + 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( "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]; + if (msg->data.size() >= 4) { + 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; + // //wheel data placeholder + // wfl_ = 1.0; + // wfr_ = 0.5; + // wrl_ = 0.5; + // wrr_ = 1.0; + + position_publisher_ = this->create_publisher("car_position", 10); - 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"); + + RCLCPP_INFO(this->get_logger(), "Subscribed to 'simulated_wheel_data', publishing to 'car_position'"); } -double WheelPositionApproximator::V_x() { - double vx = radius_ * (wfl + wfr + wrl + wrr) / 4.0; - return vx; +double WheelPositionApproximator::calculate_V_x() +{ + return radius_ * (wfl_ + wfr_ + wrl_ + wrr_) / 4.0; } -double WheelPositionApproximator::V_y() { - double vy = radius_ * (-wfl + wfr + wrl - wrr) / 4.0; - return vy; +double WheelPositionApproximator::calculate_V_y() +{ + return radius_ * (-wfl_ + wfr_ + wrl_ - wrr_) / 4.0; } -double WheelPositionApproximator::theata_z() { - double sum = lx_ + ly_; - double theta = radius_ * (-wfl + wfr - wrl + wrr) / (4.0 * sum); - return theta; +double WheelPositionApproximator::calculate_V_theta() +{ + return radius_ * (-wfl_ + wfr_ - wrl_ + wrr_) / (4.0 * (lx_ + ly_)); } -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_); +void WheelPositionApproximator::calculate_position() +{ + double vx = calculate_V_x(); + double vy = calculate_V_y(); + double vtheta = calculate_V_theta(); - x_ = V_x_ * time_elapsed + initial_x; - y_ = V_y_ * time_elapsed + initial_y; - theta_ = V_theta_ * time_elapsed + theta_; + theta_ += vtheta * time_elapsed_; + 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_; - initial_x = x_; - initial_y = y_; - initial_theta = theta_; - RCLCPP_INFO(this->get_logger(), "Position - x: %.2f, y: %.2f, theta: %.2f", x_, y_, theta_); + x_ += delta_x; + y_ += delta_y; + 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.w = 1.0; - position_publisher->publish(odom_msg); + odom_msg.pose.pose.orientation.z = std::sin(theta_ / 2.0); + odom_msg.pose.pose.orientation.w = std::cos(theta_ / 2.0); + position_publisher_->publish(odom_msg); } -} // namespace g2_2025_odometry_pkg + +} // namespace assignments::three::wheel_position_approximator_node 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 da0e1e8..bce99d8 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,11 +1,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav_msgs/msg/odometry.hpp" -#include -#include -#include -#include -#include - +#include +#include "std_msgs/msg/float64_multi_array.hpp" 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::SharedPtr position_publisher; - rclcpp::Subscription::SharedPtr wheel_encoder_sim_subscriber_; + void calculate_position(); + + double calculate_V_x(); + double calculate_V_y(); + double calculate_V_theta(); + + rclcpp::Publisher::SharedPtr position_publisher_; + rclcpp::Subscription::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 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; + double wfl_; + double wfr_; + double wrl_; + double wrr_; + double x_; + double y_; + double theta_; }; -} // namespace g2_2025_odometry_pkg + +} // namespace assignments::three::wheel_position_approximator_node