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

fix(data_sim): Add param file, add working implementation

Browse Source
This commit is contained in:
Vincent Kompjoeteraar Winter
2025-11-24 15:34:32 +01:00
parent 334af2da82
commit 6525c6a1fb
3 changed files with 217 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
src/g2_2025_odometry_pkg/config/opt.yaml Normal file
View File

@@ -0,0 +1,33 @@
data_simulator:
ros__parameters:
publish_rate: 10.0
linear_x:
num_intervals: 2
interval_0:
type: "linear"
t_start: 0.0
t_end: 5.0
y_start: 0.0
y_end: 9.81
interval_1:
type: "linear"
t_start: 10.0
t_end: 15.0
y_start: 9.81
y_end: 0.0
angular_z:
num_intervals: 1
interval_0:
type: "constant"
t_start: 2.0
y_start: 1.57
angular_x:
num_intervals: 1
interval_0:
type: "quadratic"
t_start: 3.0
y_start: 0.785
t_mid: 4.5
y_mid: 1.57
t_end: 6.0
y_end: 0.0

157
src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.cpp
View File

@@ -2,14 +2,85 @@
namespace assignments::three::data_simulator_node {
DataSimulator::DataSimulator() {
DataSimulator::DataSimulator() : Node("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(100),
std::chrono::milliseconds(timer_ms),
std::bind(&DataSimulator::publish_imu_data, this)
);
}
@@ -24,14 +95,94 @@ void DataSimulator::publish_imu_data() {
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(),
"Publishing Simulated IMU Data - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]",
"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::data_simulator_node

30
src/g2_2025_odometry_pkg/src/g2_2025_data_simulator_node/nodes/data_simulator.hpp
View File

@@ -2,18 +2,48 @@
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include <vector>
#include <string>
#include <map>
namespace assignments::three::data_simulator_node {
enum class SimType {
CONSTANT,
LINEAR,
QUADRATIC
};
struct IntervalConfig {
SimType type;
double t_start;
double t_end;
double y_start;
double y_end;
double t_mid; // For quadratic
double y_mid; // For quadratic
};
class DataSimulator : public rclcpp::Node {
public:
DataSimulator();
~DataSimulator();
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();
double compute_value(double t, const IntervalConfig& interval);
double get_axis_value(const std::string& axis, double t);
};
} // namespace assignments::three::data_simulator_node