Fri May 2 04:14:19 PM CEST 2025

src/full_topology/src/full_topology.cpp

@@ -10,11 +10,12 @@
 
 using namespace std::chrono_literals;
 
-constexpr int RUN_DURATION_SECONDS = 100;
+constexpr int RUN_DURATION_SECONDS = 20;
-constexpr bool DEBUG = true;
+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) {
+        [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());
+        {
+          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) {}};
   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) {}};
   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) {}};
   auto radio = std::make_shared<RadioNode>("radio_tx_node", "/telemetry/data", "/telemetry/radio", 10);
 
   // Add all to executor

src/tools/tracetools_analysis

@@ -1 +1 @@
-Subproject commit cf17b6348126f52cdb151e427a7904cbe6378f22
+Subproject commit d99bb4041665d77d6f9502d614af498d31d7b6de