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

fix(rename & split): Rename node and split the simulator functionality into seperate class

Browse Source
This commit is contained in:
Vincent Kompjoeteraar Winter
2025-11-24 16:37:03 +01:00
parent 085bb2f80b
commit 6763e9d764
7 changed files with 245 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/g2_2025_odometry_pkg/config/opt.yaml
View File

@@ -1,4 +1,4 @@
data_simulator:
imu_data_simulator:
ros__parameters:
publish_rate: 10.0
linear_x:

188
src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.cpp
View File

@@ -1,188 +0,0 @@
#include "imu_data_simulator.hpp"
namespace assignments::three::imu_data_simulator_node {
DataSimulator::DataSimulator() : Node("imu_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();
this->declare_parameter("max_intervals", 4);
max_intervals_ = this->get_parameter("max_intervals").as_int();
axes_ = { "linear_x", "linear_y", "linear_z", "angular_x", "angular_y", "angular_z" };
for (const auto& axis : axes_) {
// Declare number of intervals
this->declare_parameter(axis + ".num_intervals", 0);
int num_intervals = this->get_parameter(axis + ".num_intervals").as_int();
RCLCPP_INFO(this->get_logger(), "Loading %d intervals for axis '%s'", num_intervals, axis.c_str());
std::vector<IntervalConfig> intervals;
// Load up to 4 intervals
for (int i = 0; i < std::min(num_intervals, max_intervals_); i++) {
std::string prefix = axis + ".interval_" + std::to_string(i);
this->declare_parameter(prefix + ".type", "constant");
this->declare_parameter(prefix + ".t_start", 0.0);
this->declare_parameter(prefix + ".t_end", 0.0);
this->declare_parameter(prefix + ".y_start", 0.0);
this->declare_parameter(prefix + ".y_end", 0.0);
this->declare_parameter(prefix + ".t_mid", 0.0);
this->declare_parameter(prefix + ".y_mid", 0.0);
IntervalConfig config;
std::string type_str = this->get_parameter(prefix + ".type").as_string();
if (type_str == "constant") {
config.type = SimType::CONSTANT;
} else if (type_str == "linear") {
config.type = SimType::LINEAR;
} else if (type_str == "quadratic") {
config.type = SimType::QUADRATIC;
} else {
RCLCPP_WARN(this->get_logger(), "Unknown interval type '%s', defaulting to 'constant'", type_str.c_str());
config.type = SimType::CONSTANT;
}
config.t_start = this->get_parameter(prefix + ".t_start").as_double();
config.t_end = this->get_parameter(prefix + ".t_end").as_double();
config.y_start = this->get_parameter(prefix + ".y_start").as_double();
config.y_end = this->get_parameter(prefix + ".y_end").as_double();
config.t_mid = this->get_parameter(prefix + ".t_mid").as_double();
config.y_mid = this->get_parameter(prefix + ".y_mid").as_double();
intervals.push_back(config);
}
// Log loaded intervals
for (size_t j = 0; j < intervals.size(); ++j) {
const auto& cfg = intervals[j];
const char* type_str = (cfg.type == SimType::CONSTANT) ? "constant" :
(cfg.type == SimType::LINEAR) ? "linear" : "quadratic";
RCLCPP_INFO(this->get_logger(),
"Axis '%s' Interval %zu: type='%s', t_start=%.6f, t_end=%.6f, y_start=%.6f, y_end=%.6f, t_mid=%.6f, y_mid=%.6f",
axis.c_str(), j, type_str,
cfg.t_start, cfg.t_end, cfg.y_start, cfg.y_end, cfg.t_mid, cfg.y_mid
);
}
axis_intervals_[axis] = intervals;
}
imu_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("simulated_imu_data", 10);
RCLCPP_INFO(this->get_logger(), "Created IMU Publisher on topic 'simulated_imu_data'");
// Use publish_rate parameter
int timer_ms = static_cast<int>(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_imu_data, this)
);
}
DataSimulator::~DataSimulator() {
RCLCPP_INFO(this->get_logger(), "DataSimulator node destroyed");
}
void DataSimulator::publish_imu_data() {
auto imu_msg = std::make_shared<sensor_msgs::msg::Imu>();
imu_msg->header.stamp = this->now();
imu_msg->header.frame_id = "imu_link";
// Calculate elapsed time since node start
double elapsed_time = (this->now() - start_time_).seconds();
// Get values for each axis
imu_msg->linear_acceleration.x = get_axis_value("linear_x", elapsed_time);
imu_msg->linear_acceleration.y = get_axis_value("linear_y", elapsed_time);
imu_msg->linear_acceleration.z = get_axis_value("linear_z", elapsed_time);
imu_msg->angular_velocity.x = get_axis_value("angular_x", elapsed_time);
imu_msg->angular_velocity.y = get_axis_value("angular_y", elapsed_time);
imu_msg->angular_velocity.z = get_axis_value("angular_z", elapsed_time);
RCLCPP_INFO(this->get_logger(),
"t=%.2fs - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]",
elapsed_time,
imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z,
imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z
);
imu_publisher_->publish(*imu_msg);
}
double DataSimulator::compute_value(double t, const IntervalConfig& interval) {
// Check if time is within interval
if (t < interval.t_start || t > interval.t_end) {
return 0.0;
}
switch (interval.type) {
case SimType::CONSTANT:
// Constant: y = c
return interval.y_start;
case SimType::LINEAR: {
// Linear interpolation: y = y0 + (y1 - y0) * (t - t0) / (t1 - t0)
double t0 = interval.t_start;
double t1 = interval.t_end;
double y0 = interval.y_start;
double y1 = interval.y_end;
double L0 = (t - t1) / (t0 - t1);
double L1 = (t - t0) / (t1 - t0);
return y0 * L0 + y1 * L1;
// if (t1 - t0 == 0.0) return y0;
// return y0 + (y1 - y0) * (t - t0) / (t1 - t0);
}
case SimType::QUADRATIC: {
// Quadratic using Lagrange interpolation through 3 points
double t0 = interval.t_start;
double t1 = interval.t_mid;
double t2 = interval.t_end;
double y0 = interval.y_start;
double y1 = interval.y_mid;
double y2 = interval.y_end;
double L0 = ((t - t1) * (t - t2)) / ((t0 - t1) * (t0 - t2));
double L1 = ((t - t0) * (t - t2)) / ((t1 - t0) * (t1 - t2));
double L2 = ((t - t0) * (t - t1)) / ((t2 - t0) * (t2 - t1));
return y0 * L0 + y1 * L1 + y2 * L2;
}
}
return 0.0;
}
double DataSimulator::get_axis_value(const std::string& axis, double t) {
// Check if axis exists in configuration
auto it = axis_intervals_.find(axis);
if (it == axis_intervals_.end()) {
return 0.0; // Default value if axis not configured
}
// Check each interval for this axis
double last_value = 0.0;
for (const auto& interval : it->second) {
if (t >= interval.t_start && t <= interval.t_end) {
// Currently in this interval
return compute_value(t, interval);
} else if (t > interval.t_end) {
// Past this interval - remember its end value for holding
last_value = compute_value(interval.t_end, interval);
}
}
// Not in any interval - return last known value (or 0.0 if before all intervals)
return last_value;
}
} // namespace assignments::three::imu_data_simulator_node

2
src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/main.cpp → src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/main.cpp
View File

@@ -4,7 +4,7 @@
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
auto node = std::make_shared<assignments::three::imu_data_simulator_node::DataSimulator>();
auto node = std::make_shared<assignments::three::data_simulator_node::DataSimulator>();
rclcpp::spin(node);
rclcpp::shutdown();

62
src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.cpp Normal file
View File

@@ -0,0 +1,62 @@
#include "imu_data_simulator.hpp"
namespace assignments::three::data_simulator_node {
DataSimulator::DataSimulator() : Node("imu_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();
axes_ = { "linear_x", "linear_y", "linear_z", "angular_x", "angular_y", "angular_z" };
// Simulator loads parameters itself
simulator_ = std::make_unique<Simulator>(this, axes_);
imu_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("simulated_imu_data", 10);
RCLCPP_INFO(this->get_logger(), "Created IMU Publisher on topic 'simulated_imu_data'");
// Use publish_rate parameter
int timer_ms = static_cast<int>(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_imu_data, this)
);
}
DataSimulator::~DataSimulator() {
RCLCPP_INFO(this->get_logger(), "DataSimulator node destroyed");
}
void DataSimulator::publish_imu_data() {
auto imu_msg = std::make_shared<sensor_msgs::msg::Imu>();
imu_msg->header.stamp = this->now();
imu_msg->header.frame_id = "imu_link";
// Calculate elapsed time since node start
double elapsed_time = (this->now() - start_time_).seconds();
// Get values for each axis
imu_msg->linear_acceleration.x = simulator_->get_axis_value("linear_x", elapsed_time);
imu_msg->linear_acceleration.y = simulator_->get_axis_value("linear_y", elapsed_time);
imu_msg->linear_acceleration.z = simulator_->get_axis_value("linear_z", elapsed_time);
imu_msg->angular_velocity.x = simulator_->get_axis_value("angular_x", elapsed_time);
imu_msg->angular_velocity.y = simulator_->get_axis_value("angular_y", elapsed_time);
imu_msg->angular_velocity.z = simulator_->get_axis_value("angular_z", elapsed_time);
RCLCPP_INFO(this->get_logger(),
"t=%.2fs - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]",
elapsed_time,
imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z,
imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z
);
imu_publisher_->publish(*imu_msg);
}
} // namespace assignments::three::data_simulator_node

28
src/g2_2025_odometry_pkg/src/g2_2025_imu_data_simulator_node/nodes/imu_data_simulator.hpp Normal file
View File

@@ -0,0 +1,28 @@
#pragma once
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "simulator/Simulator.hpp"
namespace assignments::three::data_simulator_node {
using assignments::three::Simulator;
class DataSimulator : public rclcpp::Node {
public:
DataSimulator();
~DataSimulator();
private:
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time start_time_;
std::vector<std::string> axes_;
std::unique_ptr<Simulator> simulator_;
void publish_imu_data();
};
} // namespace assignments::three::data_simulator_node

141
src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp Normal file
View File

@@ -0,0 +1,141 @@
#include "Simulator.hpp"
#include <algorithm>
namespace assignments::three
{
Simulator::Simulator(rclcpp::Node* node, const std::vector<std::string>& axes) {
load_intervals(node, axes);
}
Simulator::~Simulator() {
}
void Simulator::load_intervals(rclcpp::Node* node, const std::vector<std::string>& axes) {
node->declare_parameter("max_intervals", 4);
max_intervals_ = node->get_parameter("max_intervals").as_int();
for (const auto& axis : axes) {
node->declare_parameter(axis + ".num_intervals", 0);
int num_intervals = node->get_parameter(axis + ".num_intervals").as_int();
RCLCPP_INFO(node->get_logger(), "Loading %d intervals for axis '%s'", num_intervals, axis.c_str());
std::vector<IntervalConfig> intervals;
for (int i = 0; i < std::min(num_intervals, max_intervals_); i++) {
std::string prefix = axis + ".interval_" + std::to_string(i);
node->declare_parameter(prefix + ".type", "constant");
node->declare_parameter(prefix + ".t_start", 0.0);
node->declare_parameter(prefix + ".t_end", 0.0);
node->declare_parameter(prefix + ".y_start", 0.0);
node->declare_parameter(prefix + ".y_end", 0.0);
node->declare_parameter(prefix + ".t_mid", 0.0);
node->declare_parameter(prefix + ".y_mid", 0.0);
IntervalConfig config;
std::string type_str = node->get_parameter(prefix + ".type").as_string();
if (type_str == "constant") {
config.type = SimType::CONSTANT;
} else if (type_str == "linear") {
config.type = SimType::LINEAR;
} else if (type_str == "quadratic") {
config.type = SimType::QUADRATIC;
} else {
RCLCPP_WARN(node->get_logger(), "Unknown interval type '%s', defaulting to 'constant'", type_str.c_str());
config.type = SimType::CONSTANT;
}
config.t_start = node->get_parameter(prefix + ".t_start").as_double();
config.t_end = node->get_parameter(prefix + ".t_end").as_double();
config.y_start = node->get_parameter(prefix + ".y_start").as_double();
config.y_end = node->get_parameter(prefix + ".y_end").as_double();
config.t_mid = node->get_parameter(prefix + ".t_mid").as_double();
config.y_mid = node->get_parameter(prefix + ".y_mid").as_double();
intervals.push_back(config);
}
for (size_t j = 0; j < intervals.size(); ++j) {
const auto& cfg = intervals[j];
const char* type_str = (cfg.type == SimType::CONSTANT) ? "constant" :
(cfg.type == SimType::LINEAR) ? "linear" : "quadratic";
RCLCPP_INFO(node->get_logger(),
"Axis '%s' Interval %zu: type='%s', t_start=%.6f, t_end=%.6f, y_start=%.6f, y_end=%.6f, t_mid=%.6f, y_mid=%.6f",
axis.c_str(), j, type_str,
cfg.t_start, cfg.t_end, cfg.y_start, cfg.y_end, cfg.t_mid, cfg.y_mid
);
}
axis_intervals_[axis] = intervals;
}
}
double Simulator::compute_value(double t, const IntervalConfig& interval) {
// Check if time is within interval
if (t < interval.t_start || t > interval.t_end) {
return 0.0;
}
switch (interval.type) {
case SimType::CONSTANT:
// Constant: y = c
return interval.y_start;
case SimType::LINEAR: {
// Linear interpolation: y = y0 + (y1 - y0) * (t - t0) / (t1 - t0)
double t0 = interval.t_start;
double t1 = interval.t_end;
double y0 = interval.y_start;
double y1 = interval.y_end;
double L0 = (t - t1) / (t0 - t1);
double L1 = (t - t0) / (t1 - t0);
return y0 * L0 + y1 * L1;
}
case SimType::QUADRATIC: {
// Quadratic using Lagrange interpolation through 3 points
double t0 = interval.t_start;
double t1 = interval.t_mid;
double t2 = interval.t_end;
double y0 = interval.y_start;
double y1 = interval.y_mid;
double y2 = interval.y_end;
double L0 = ((t - t1) * (t - t2)) / ((t0 - t1) * (t0 - t2));
double L1 = ((t - t0) * (t - t2)) / ((t1 - t0) * (t1 - t2));
double L2 = ((t - t0) * (t - t1)) / ((t2 - t0) * (t2 - t1));
return y0 * L0 + y1 * L1 + y2 * L2;
}
}
return 0.0;
}
double Simulator::get_axis_value(const std::string& axis, double t) {
// Check if axis exists in configuration
auto it = axis_intervals_.find(axis);
if (it == axis_intervals_.end()) {
return 0.0; // Default value if axis not configured
}
// Check each interval for this axis
double last_value = 0.0;
for (const auto& interval : it->second) {
if (t >= interval.t_start && t <= interval.t_end) {
// Currently in this interval
return compute_value(t, interval);
} else if (t > interval.t_end) {
// Past this interval - remember its end value for holding
last_value = compute_value(interval.t_end, interval);
}
}
// Not in any interval - return last known value (or 0.0 if before all intervals)
return last_value;
}
} // namespace assignments::three

31
src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/imu_data_simulator.hpp → src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp
View File

@@ -1,12 +1,11 @@
#pragma once
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include <rclcpp/rclcpp.hpp>
#include <vector>
#include <string>
#include <map>
namespace assignments::three::data_simulator_node {
namespace assignments::three
{
enum class SimType {
CONSTANT,
@@ -24,26 +23,20 @@ struct IntervalConfig {
double y_mid; // For quadratic
};
class DataSimulator : public rclcpp::Node {
class Simulator {
public:
DataSimulator();
~DataSimulator();
Simulator(rclcpp::Node* node, const std::vector<std::string>& axes);
~Simulator();
double get_axis_value(const std::string& axis, double t);
private:
int max_intervals_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time start_time_;
std::vector<std::string> axes_;
std::map<std::string, std::vector<IntervalConfig>> axis_intervals_;
void publish_imu_data();
void parse_type();
void load_intervals(rclcpp::Node* node, const std::vector<std::string>& axes);
double compute_value(double t, const IntervalConfig& interval);
double get_axis_value(const std::string& axis, double t);
std::map<std::string, std::vector<IntervalConfig>> axis_intervals_;
};
} // namespace assignments::three::data_simulator_node
} // namespace assignments::three