inholland/ros2-assignments
3
1
Fork 0
generated from wessel/boilerplate
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(wheel_pos_aprox): fixed clacs & launch file

Browse Source
This commit is contained in:
Mohammad Khalaf
2025-11-25 19:31:01 +01:00
committed by Wessel Tip
parent 1a9254a53e
commit 68d7100de3
4 changed files with 101 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/g2_2025_odometry_pkg/CMakeLists.txt
View File

@@ -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

9
src/g2_2025_odometry_pkg/launch/odom.launch.xml Normal file
View File

@@ -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>

107
src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.cpp
View File

@@ -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];
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<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 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

52
src/g2_2025_odometry_pkg/src/g2_2025_wheel_approximator_node/nodes/WheelPositionApproximator.hpp
View File

@@ -1,11 +1,7 @@
#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>
#include <cmath>
#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<nav_msgs::msg::Odometry>::SharedPtr position_publisher;
rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr wheel_encoder_sim_subscriber_;
void calculate_position();
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 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