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

chore: Remove copy-pasted nodes

Browse Source
This commit is contained in:
Wessel T
2025-11-28 20:14:33 +01:00
parent 81dc66c597
commit 9d8a1fc710
7 changed files with 0 additions and 362 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,63 +0,0 @@
imu_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
wheel_data_simulator:
ros__parameters:
publish_rate: 10.0
wheel_fl:
num_intervals: 1
interval_0:
type: "linear"
t_start: 0.0
t_end: 10.0
y_start: 0.0
y_end: 5.0
wheel_fr:
num_intervals: 1
interval_0:
type: "linear"
t_start: 0.0
t_end: 10.0
y_start: 0.0
y_end: 10.0
wheel_position_approximator:
ros__parameters:
wheel_radius: 0.2
lx: 0.3
ly: 0.6
time_elapsed: 1.0
initial_x: 0.0
initial_y: 0.0
initial_theta: 0.0

13
src/g2_2025_odometry_pkg/launch/wheel_sim.launch.xml
View File

@@ -1,13 +0,0 @@
<launch>
<arg name="params_file" default="$(find-pkg-share g2_2025_odometry_pkg)/config/opt.yaml"/>
<node pkg="g2_2025_odometry_pkg" exec="wheel_data_simulator_node" name="wheel_data_simulator" output="screen">
<param from="$(var params_file)"/>
</node>
<node pkg="g2_2025_odometry_pkg" exec="wheel_position_approximator_node" name="wheel_position_approximator" output="screen">
<param from="$(var params_file)"/>
</node>
</launch>

13
src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/main.cpp
View File

@@ -1,13 +0,0 @@
#include "rclcpp/rclcpp.hpp"
#include "nodes/wheel_data_simulator.hpp"
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
auto node = std::make_shared<assignments::three::wheel_data_simulator_node::DataSimulator>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

58
src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.cpp
View File

@@ -1,58 +0,0 @@
#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<Simulator>(this, wheels_);
wheel_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("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<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_wheel_data, this)
);
}
DataSimulator::~DataSimulator() {
RCLCPP_INFO(this->get_logger(), "DataSimulator node destroyed");
}
void DataSimulator::publish_wheel_data() {
auto wheel_msg = std::make_shared<std_msgs::msg::Float64MultiArray>();
// 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_object_value("wheel_fl", elapsed_time),
simulator_->get_object_value("wheel_fr", elapsed_time),
simulator_->get_object_value("wheel_rl", elapsed_time),
simulator_->get_object_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

33
src/g2_2025_odometry_pkg/src/g2_2025_wheel_data_simulator_node/nodes/wheel_data_simulator.hpp
View File

@@ -1,33 +0,0 @@
#pragma once
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "simulator/Simulator.hpp"
#include <vector>
#include <string>
#include <map>
#include <memory>
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<std_msgs::msg::Float64MultiArray>::SharedPtr wheel_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time start_time_;
std::vector<std::string> wheels_;
std::unique_ptr<Simulator> simulator_;
void publish_wheel_data();
};
} // namespace assignments::three::wheel_data_simulator_node

140
src/g2_2025_odometry_pkg/src/simulator/Simulator.cpp
View File

@@ -1,140 +0,0 @@
#include "Simulator.hpp"
#include <algorithm>
namespace assignments::three
{
Simulator::Simulator(rclcpp::Node* node, const std::vector<std::string>& objects) {
load_intervals(node, objects);
}
Simulator::~Simulator() {
}
void Simulator::load_intervals(rclcpp::Node* node, const std::vector<std::string>& objects) {
node->declare_parameter("max_intervals", 4);
max_intervals_ = node->get_parameter("max_intervals").as_int();
for (const auto& object : objects) {
node->declare_parameter(object + ".num_intervals", 0);
int num_intervals = node->get_parameter(object + ".num_intervals").as_int();
RCLCPP_INFO(node->get_logger(), "Loading %d intervals for object '%s'", num_intervals, object.c_str());
std::vector<IntervalConfig> intervals;
for (int i = 0; i < std::min(num_intervals, max_intervals_); i++) {
std::string prefix = object + ".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_DEBUG(node->get_logger(),
"Object '%s' Interval %zu: type='%s', t_start=%.6f, t_end=%.6f, y_start=%.6f, y_end=%.6f, t_mid=%.6f, y_mid=%.6f",
object.c_str(), j, type_str,
cfg.t_start, cfg.t_end, cfg.y_start, cfg.y_end, cfg.t_mid, cfg.y_mid
);
}
object_intervals_[object] = 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_object_value(const std::string& object, double t) {
// Check if object exists in configuration
auto it = object_intervals_.find(object);
if (it == object_intervals_.end()) {
return 0.0; // Default value if object not configured
}
// Check each interval for this object
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

42
src/g2_2025_odometry_pkg/src/simulator/Simulator.hpp
View File

@@ -1,42 +0,0 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <vector>
#include <string>
#include <map>
namespace assignments::three
{
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 Simulator {
public:
Simulator(rclcpp::Node* node, const std::vector<std::string>& objects);
~Simulator();
double get_object_value(const std::string& object, double t);
private:
int max_intervals_;
void load_intervals(rclcpp::Node* node, const std::vector<std::string>& objects);
double compute_value(double t, const IntervalConfig& interval);
std::map<std::string, std::vector<IntervalConfig>> object_intervals_;
};
} // namespace assignments::three