Fri May 2 04:14:19 PM CEST 2025
This commit is contained in:
parent
310b6bfccf
commit
1ed67d4a7d
2 changed files with 171 additions and 53 deletions
|
|
@ -10,11 +10,12 @@
|
|||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
constexpr int RUN_DURATION_SECONDS = 100;
|
||||
constexpr bool DEBUG = true;
|
||||
constexpr int RUN_DURATION_SECONDS = 20;
|
||||
constexpr bool DEBUG = false;
|
||||
|
||||
// A generic sensor node that publishes at a given rate
|
||||
class SensorNode : public rclcpp::Node {
|
||||
class SensorNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
SensorNode(std::string const &name,
|
||||
std::string const &topic,
|
||||
|
|
@ -23,22 +24,24 @@ public:
|
|||
{
|
||||
publisher_ = create_publisher<std_msgs::msg::String>(topic, 10);
|
||||
auto period = std::chrono::milliseconds(1000 / rate_hz);
|
||||
timer_ = create_wall_timer(period, [this, name]() {
|
||||
timer_ = create_wall_timer(period, [this, name]()
|
||||
{
|
||||
auto msg = std_msgs::msg::String();
|
||||
msg.data = name + std::string(" publishes");
|
||||
if (DEBUG) {
|
||||
RCLCPP_INFO(get_logger(), "%s", msg.data.c_str());
|
||||
}
|
||||
publisher_->publish(msg);
|
||||
});
|
||||
publisher_->publish(msg); });
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
};
|
||||
|
||||
// A generic single-input processing node with a fixed delay
|
||||
class ProcessingNode : public rclcpp::Node {
|
||||
class ProcessingNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
ProcessingNode(std::string const &name,
|
||||
std::string const &input_topic,
|
||||
|
|
@ -48,8 +51,11 @@ public:
|
|||
{
|
||||
subscription_ = create_subscription<std_msgs::msg::String>(
|
||||
input_topic, 10,
|
||||
[this, name, output_topic, delay_ms](std_msgs::msg::String::SharedPtr msg) {
|
||||
RCLCPP_INFO(get_logger(), "%s processing: %s", name.c_str(), msg->data.c_str());
|
||||
[this, name, output_topic, delay_ms](std_msgs::msg::String::SharedPtr msg)
|
||||
{
|
||||
if (DEBUG) {
|
||||
RCLCPP_INFO(get_logger(), "%s received: %s", name.c_str(), msg->data.c_str());
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
|
||||
auto out = std_msgs::msg::String();
|
||||
out.data = msg->data + " -> " + name;
|
||||
|
|
@ -57,13 +63,15 @@ public:
|
|||
});
|
||||
publisher_ = create_publisher<std_msgs::msg::String>(output_topic, 10);
|
||||
}
|
||||
private:
|
||||
|
||||
protected:
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
|
||||
};
|
||||
|
||||
// A processing node that fuses multiple inputs
|
||||
class MultiInputProcessingNode : public rclcpp::Node {
|
||||
class MultiInputProcessingNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
MultiInputProcessingNode(std::string const &name,
|
||||
std::vector<std::string> const &input_topics,
|
||||
|
|
@ -71,12 +79,15 @@ public:
|
|||
int delay_ms)
|
||||
: Node(name)
|
||||
{
|
||||
for (auto & topic : input_topics) {
|
||||
for (auto &topic : input_topics)
|
||||
{
|
||||
auto sub = create_subscription<std_msgs::msg::String>(
|
||||
topic, 10,
|
||||
[this, name, delay_ms](std_msgs::msg::String::SharedPtr msg) {
|
||||
[this, name, delay_ms](std_msgs::msg::String::SharedPtr msg)
|
||||
{
|
||||
// simple fusion: log each arrival
|
||||
if (DEBUG) {
|
||||
if (DEBUG)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "%s fusing input: %s", name.c_str(), msg->data.c_str());
|
||||
}
|
||||
});
|
||||
|
|
@ -84,67 +95,174 @@ public:
|
|||
}
|
||||
publisher_ = create_publisher<std_msgs::msg::String>(output_topic, 10);
|
||||
// Also publish fused result periodically
|
||||
timer_ = create_wall_timer(50ms, [this, name, delay_ms]() {
|
||||
timer_ = create_wall_timer(50ms, [this, name, delay_ms]()
|
||||
{
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
|
||||
auto out = std_msgs::msg::String();
|
||||
out.data = name + std::string(" fused");
|
||||
if (DEBUG) {
|
||||
RCLCPP_INFO(get_logger(), "%s publishes fused message", name.c_str());
|
||||
}
|
||||
publisher_->publish(out);
|
||||
});
|
||||
publisher_->publish(out); });
|
||||
}
|
||||
private:
|
||||
|
||||
protected:
|
||||
std::vector<rclcpp::Subscription<std_msgs::msg::String>::SharedPtr> subscriptions_;
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
};
|
||||
|
||||
int main(int argc, char ** argv) {
|
||||
class CameraNode : public SensorNode
|
||||
{
|
||||
public:
|
||||
CameraNode(std::string const &name, std::string const &topic, int rate_hz) : SensorNode(name, topic, rate_hz) {}
|
||||
};
|
||||
|
||||
class DebayerNode : public ProcessingNode
|
||||
{
|
||||
public:
|
||||
DebayerNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {}
|
||||
};
|
||||
|
||||
class RadiometricNode : public ProcessingNode
|
||||
{
|
||||
public:
|
||||
RadiometricNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {
|
||||
// Overwrite the subscription callback
|
||||
subscription_ = create_subscription<std_msgs::msg::String>(
|
||||
input_topic, 10,
|
||||
[this, name, output_topic, delay_ms](std_msgs::msg::String::SharedPtr msg)
|
||||
{
|
||||
// Custom logic for RadiometricNode
|
||||
if (DEBUG) {
|
||||
RCLCPP_INFO(get_logger(), "%s received: %s", name.c_str(), msg->data.c_str());
|
||||
}
|
||||
|
||||
// random: 1 in 5 it will be 10s instead of delay_ms
|
||||
if (rand() % 5 == 0) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10000));
|
||||
} else {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
|
||||
}
|
||||
|
||||
// Custom transformation of data
|
||||
auto out = std_msgs::msg::String();
|
||||
out.data = msg->data + " -> Radiometric processing complete";
|
||||
|
||||
// Publish the processed message
|
||||
publisher_->publish(out);
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
class GeometricNode : public ProcessingNode
|
||||
{
|
||||
public:
|
||||
GeometricNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {}
|
||||
};
|
||||
|
||||
class MappingNode : public ProcessingNode
|
||||
{
|
||||
public:
|
||||
MappingNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {}
|
||||
};
|
||||
|
||||
class SmokeClassifierNode : public ProcessingNode
|
||||
{
|
||||
public:
|
||||
SmokeClassifierNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {}
|
||||
};
|
||||
|
||||
class GPSNode : public SensorNode
|
||||
{
|
||||
public:
|
||||
GPSNode(std::string const &name, std::string const &topic, int rate_hz) : SensorNode(name, topic, rate_hz) {}
|
||||
};
|
||||
|
||||
class IMUNode : public SensorNode
|
||||
{
|
||||
public:
|
||||
IMUNode(std::string const &name, std::string const &topic, int rate_hz) : SensorNode(name, topic, rate_hz) {}
|
||||
};
|
||||
|
||||
class BaroNode : public SensorNode
|
||||
{
|
||||
public:
|
||||
BaroNode(std::string const &name, std::string const &topic, int rate_hz) : SensorNode(name, topic, rate_hz) {}
|
||||
};
|
||||
|
||||
class FusionNode : public MultiInputProcessingNode
|
||||
{
|
||||
public:
|
||||
FusionNode(std::string const &name, std::vector<std::string> const &input_topics, std::string const &output_topic, int delay_ms) : MultiInputProcessingNode(name, input_topics, output_topic, delay_ms) {}
|
||||
};
|
||||
|
||||
class LidarNode : public SensorNode
|
||||
{
|
||||
public:
|
||||
LidarNode(std::string const &name, std::string const &topic, int rate_hz) : SensorNode(name, topic, rate_hz) {}
|
||||
};
|
||||
|
||||
class CommandNode : public SensorNode
|
||||
{
|
||||
public:
|
||||
CommandNode(std::string const &name, std::string const &topic, int rate_hz) : SensorNode(name, topic, rate_hz) {}
|
||||
};
|
||||
|
||||
class FlightManagementNode : public MultiInputProcessingNode
|
||||
{
|
||||
public:
|
||||
FlightManagementNode(std::string const &name, std::vector<std::string> const &input_topics, std::string const &output_topic, int delay_ms) : MultiInputProcessingNode(name, input_topics, output_topic, delay_ms) {}
|
||||
};
|
||||
|
||||
class ControlNode : public ProcessingNode
|
||||
{
|
||||
public:
|
||||
ControlNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {}
|
||||
};
|
||||
|
||||
class TelemetryNode : public MultiInputProcessingNode
|
||||
{
|
||||
public:
|
||||
TelemetryNode(std::string const &name, std::vector<std::string> const &input_topics, std::string const &output_topic, int delay_ms) : MultiInputProcessingNode(name, input_topics, output_topic, delay_ms) {}
|
||||
};
|
||||
|
||||
class RadioNode : public ProcessingNode
|
||||
{
|
||||
public:
|
||||
RadioNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {}
|
||||
};
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::executors::MultiThreadedExecutor executor;
|
||||
|
||||
// Chain 1: Image2Ground Mapping
|
||||
class CameraNode : public SensorNode { public: CameraNode(std::string const &name, std::string const &topic, int rate_hz) : SensorNode(name, topic, rate_hz) {}};
|
||||
auto camera = std::make_shared<CameraNode>("camera_node", "/camera/raw", 30);
|
||||
class DebayerNode : public ProcessingNode { public: DebayerNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {}};
|
||||
auto debayer = std::make_shared<DebayerNode>("debayer_node", "/camera/raw", "/camera/debayered", 15);
|
||||
class RadiometricNode : public ProcessingNode { public: RadiometricNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {}};
|
||||
auto radiometric = std::make_shared<RadiometricNode>("radiometric_node", "/camera/debayered", "/camera/radiometric", 20);
|
||||
class GeometricNode : public ProcessingNode { public: GeometricNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {}};
|
||||
auto geometric = std::make_shared<GeometricNode>("geometric_node", "/camera/radiometric", "/camera/geometric", 25);
|
||||
class MappingNode : public ProcessingNode { public: MappingNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {}};
|
||||
auto mapping = std::make_shared<MappingNode>("mapping_node", "/camera/geometric", "/camera/mapped", 30);
|
||||
|
||||
// Chain 2: Semantic Scheduler Adaptation
|
||||
class SmokeClassifierNode : public ProcessingNode { public: SmokeClassifierNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {}};
|
||||
auto smoke = std::make_shared<SmokeClassifierNode>("smoke_classifier_node", "/camera/radiometric", "/camera/radiometric", 50);
|
||||
|
||||
// Chain 3: Flight Control
|
||||
class GPSNode : public SensorNode { public: GPSNode(std::string const &name, std::string const &topic, int rate_hz) : SensorNode(name, topic, rate_hz) {}};
|
||||
auto gps = std::make_shared<GPSNode>("gps_node", "/gps/fix", 10);
|
||||
class IMUNode : public SensorNode { public: IMUNode(std::string const &name, std::string const &topic, int rate_hz) : SensorNode(name, topic, rate_hz) {}};
|
||||
auto imu = std::make_shared<IMUNode>("imu_node", "/imu/data", 100);
|
||||
class BaroNode : public SensorNode { public: BaroNode(std::string const &name, std::string const &topic, int rate_hz) : SensorNode(name, topic, rate_hz) {}};
|
||||
auto baro = std::make_shared<BaroNode>("baro_node", "/baro/alt", 20);
|
||||
class FusionNode : public MultiInputProcessingNode { public: FusionNode(std::string const &name, std::vector<std::string> const &input_topics, std::string const &output_topic, int delay_ms) : MultiInputProcessingNode(name, input_topics, output_topic, delay_ms) {}};
|
||||
auto fusion = std::make_shared<FusionNode>(
|
||||
"sensor_fusion_node", std::vector<std::string>{"/gps/fix","/imu/data","/baro/alt"}, "/sensors/fused", 10);
|
||||
class LidarNode : public SensorNode { public: LidarNode(std::string const &name, std::string const &topic, int rate_hz) : SensorNode(name, topic, rate_hz) {}};
|
||||
"sensor_fusion_node", std::vector<std::string>{"/gps/fix", "/imu/data", "/baro/alt"}, "/sensors/fused", 10);
|
||||
auto lidar = std::make_shared<LidarNode>("lidar_node", "/lidar/scan", 15);
|
||||
class CommandNode : public SensorNode { public: CommandNode(std::string const &name, std::string const &topic, int rate_hz) : SensorNode(name, topic, rate_hz) {}};
|
||||
auto cmd = std::make_shared<CommandNode>("operator_cmd_node", "/operator/commands", 1);
|
||||
class FlightManagementNode : public MultiInputProcessingNode { public: FlightManagementNode(std::string const &name, std::vector<std::string> const &input_topics, std::string const &output_topic, int delay_ms) : MultiInputProcessingNode(name, input_topics, output_topic, delay_ms) {}};
|
||||
auto mgmt = std::make_shared<FlightManagementNode>(
|
||||
"flight_mgmt_node", std::vector<std::string>{"/sensors/fused","/lidar/scan","/operator/commands"}, "/flight/plan", 20);
|
||||
class ControlNode : public ProcessingNode { public: ControlNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {}};
|
||||
"flight_mgmt_node", std::vector<std::string>{"/sensors/fused", "/lidar/scan", "/operator/commands"}, "/flight/plan", 20);
|
||||
auto control = std::make_shared<ControlNode>("flight_control_node", "/flight/plan", "/flight/cmd", 25);
|
||||
|
||||
// Chain 4: Data Storage/Streaming
|
||||
class TelemetryNode : public MultiInputProcessingNode { public: TelemetryNode(std::string const &name, std::vector<std::string> const &input_topics, std::string const &output_topic, int delay_ms) : MultiInputProcessingNode(name, input_topics, output_topic, delay_ms) {}};
|
||||
auto telemetry = std::make_shared<TelemetryNode>(
|
||||
"telemetry_node", std::vector<std::string>{"/sensors/fused","/camera/mapped"}, "/telemetry/data", 5);
|
||||
class RadioNode : public ProcessingNode { public: RadioNode(std::string const &name, std::string const &input_topic, std::string const &output_topic, int delay_ms) : ProcessingNode(name, input_topic, output_topic, delay_ms) {}};
|
||||
"telemetry_node", std::vector<std::string>{"/sensors/fused", "/camera/mapped"}, "/telemetry/data", 5);
|
||||
auto radio = std::make_shared<RadioNode>("radio_tx_node", "/telemetry/data", "/telemetry/radio", 10);
|
||||
|
||||
// Add all to executor
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
Subproject commit cf17b6348126f52cdb151e427a7904cbe6378f22
|
||||
Subproject commit d99bb4041665d77d6f9502d614af498d31d7b6de
|
||||
Loading…
Add table
Add a link
Reference in a new issue