diff --git a/src/full_topology/src/full_topology.cpp b/src/full_topology/src/full_topology.cpp index 708f47c..84b373a 100644 --- a/src/full_topology/src/full_topology.cpp +++ b/src/full_topology/src/full_topology.cpp @@ -10,141 +10,259 @@ 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, int rate_hz) - : Node(name) + : Node(name) { publisher_ = create_publisher(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::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, std::string const &output_topic, int delay_ms) - : Node(name) + : Node(name) { subscription_ = create_subscription( - 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()); - std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); - auto out = std_msgs::msg::String(); - out.data = msg->data + " -> " + name; - publisher_->publish(out); - }); + input_topic, 10, + [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; + publisher_->publish(out); + }); publisher_ = create_publisher(output_topic, 10); } -private: + +protected: rclcpp::Subscription::SharedPtr subscription_; rclcpp::Publisher::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 const &input_topics, std::string const &output_topic, int delay_ms) - : Node(name) + : Node(name) { - for (auto & topic : input_topics) { + for (auto &topic : input_topics) + { auto sub = create_subscription( - topic, 10, - [this, name, delay_ms](std_msgs::msg::String::SharedPtr msg) { - // simple fusion: log each arrival - if (DEBUG) { - RCLCPP_INFO(get_logger(), "%s fusing input: %s", name.c_str(), msg->data.c_str()); - } - }); + topic, 10, + [this, name, delay_ms](std_msgs::msg::String::SharedPtr msg) + { + // simple fusion: log each arrival + if (DEBUG) + { + RCLCPP_INFO(get_logger(), "%s fusing input: %s", name.c_str(), msg->data.c_str()); + } + }); subscriptions_.push_back(sub); } publisher_ = create_publisher(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::SharedPtr> subscriptions_; rclcpp::Publisher::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( + 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 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 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 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("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("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("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("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("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("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("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("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("baro_node", "/baro/alt", 20); - class FusionNode : public MultiInputProcessingNode { public: FusionNode(std::string const &name, std::vector const &input_topics, std::string const &output_topic, int delay_ms) : MultiInputProcessingNode(name, input_topics, output_topic, delay_ms) {}}; auto fusion = std::make_shared( - "sensor_fusion_node", std::vector{"/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{"/gps/fix", "/imu/data", "/baro/alt"}, "/sensors/fused", 10); auto lidar = std::make_shared("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("operator_cmd_node", "/operator/commands", 1); - class FlightManagementNode : public MultiInputProcessingNode { public: FlightManagementNode(std::string const &name, std::vector const &input_topics, std::string const &output_topic, int delay_ms) : MultiInputProcessingNode(name, input_topics, output_topic, delay_ms) {}}; auto mgmt = std::make_shared( - "flight_mgmt_node", std::vector{"/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{"/sensors/fused", "/lidar/scan", "/operator/commands"}, "/flight/plan", 20); auto control = std::make_shared("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 const &input_topics, std::string const &output_topic, int delay_ms) : MultiInputProcessingNode(name, input_topics, output_topic, delay_ms) {}}; auto telemetry = std::make_shared( - "telemetry_node", std::vector{"/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{"/sensors/fused", "/camera/mapped"}, "/telemetry/data", 5); auto radio = std::make_shared("radio_tx_node", "/telemetry/data", "/telemetry/radio", 10); // Add all to executor diff --git a/src/tools/tracetools_analysis b/src/tools/tracetools_analysis index cf17b63..d99bb40 160000 --- a/src/tools/tracetools_analysis +++ b/src/tools/tracetools_analysis @@ -1 +1 @@ -Subproject commit cf17b6348126f52cdb151e427a7904cbe6378f22 +Subproject commit d99bb4041665d77d6f9502d614af498d31d7b6de