Fri May 2 03:32:04 PM CEST 2025

Dockerfile

@@ -83,7 +83,7 @@ RUN python3 -m pip install --upgrade jupyterlab
 # Install dependencies for ros2_tracing
 RUN apt-get update -q && \
     apt-get install -y lttng-tools lttng-modules-dkms liblttng-ust-dev
-RUN apt-get install -y python3-babeltrace python3-lttng python3-pytest python3-pandas
+RUN apt-get install -y python3-babeltrace python3-lttng python3-pytest python3-pytest-cov python3-pandas
 
 # Python bokeh is not available in the default apt repository
 # So we install it using pip

src/full_topology/CMakeLists.txt

@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 3.5)
+project(full_topology VERSION 0.1.0)
+
+# Set C++ standards
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+# Compiler options
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+add_executable(${PROJECT_NAME} src/full_topology.cpp)
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+# Add dependencies with ament
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  std_msgs
+)
+
+# Installation
+install(
+  TARGETS ${PROJECT_NAME}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+# Testing
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+# Export and package configuration
+ament_export_include_directories(include)
+ament_package()

src/full_topology/launch/trace_full_topology.launch.py

@@ -0,0 +1,22 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from tracetools_launch.action import Trace
+
+def generate_launch_description():
+    return LaunchDescription([
+        Trace(
+            session_name='full_topology_tracing',
+            events_ust=[
+                'rclcpp:*', 'rcl:*', 'rmw:*', 'ros2:*', 'rclcpp:*callback*', 'rclcpp:*executor*'
+            ],
+            events_kernel=[
+                'sched_switch', 'sched_wakeup'
+            ],
+        ),
+        
+        Node(
+            package='full_topology',
+            executable='full_topology',
+            output='screen',
+        ),
+    ])

src/full_topology/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>full_topology</name>
+  <version>0.1.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="niklas@niklashalle.net">dev</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>launch</depend>
+  <depend>launch_ros</depend>
+  <depend>tracetools_trace</depend>
+  <depend>tracetools_launch</depend>
+  
+  <!-- Build Dependencies -->
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_mypy</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>ament_xmllint</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <!-- Test Dependencies -->
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/full_topology/src/full_topology.cpp

@@ -0,0 +1,178 @@
+// full_topology.cpp
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+#include <thread>
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+using namespace std::chrono_literals;
+
+constexpr int RUN_DURATION_SECONDS = 100;
+constexpr bool DEBUG = true;
+
+// A generic sensor node that publishes at a given rate
+class SensorNode : public rclcpp::Node {
+public:
+  SensorNode(std::string const &name,
+             std::string const &topic,
+             int rate_hz)
+  : Node(name)
+  {
+    publisher_ = create_publisher<std_msgs::msg::String>(topic, 10);
+    auto period = std::chrono::milliseconds(1000 / rate_hz);
+    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);
+    });
+  }
+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 {
+public:
+  ProcessingNode(std::string const &name,
+                 std::string const &input_topic,
+                 std::string const &output_topic,
+                 int delay_ms)
+  : Node(name)
+  {
+    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());
+        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<std_msgs::msg::String>(output_topic, 10);
+  }
+private:
+  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 {
+public:
+  MultiInputProcessingNode(std::string const &name,
+                           std::vector<std::string> const &input_topics,
+                           std::string const &output_topic,
+                           int delay_ms)
+  : Node(name)
+  {
+    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) {
+          // 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<std_msgs::msg::String>(output_topic, 10);
+    // Also publish fused result periodically
+    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);
+    });
+  }
+private:
+  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) {
+  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
+  executor.add_node(camera);
+  executor.add_node(debayer);
+  executor.add_node(radiometric);
+  executor.add_node(geometric);
+  executor.add_node(mapping);
+  executor.add_node(smoke);
+  executor.add_node(gps);
+  executor.add_node(imu);
+  executor.add_node(baro);
+  executor.add_node(fusion);
+  executor.add_node(lidar);
+  executor.add_node(cmd);
+  executor.add_node(mgmt);
+  executor.add_node(control);
+  executor.add_node(telemetry);
+  executor.add_node(radio);
+
+  // ToDo: find a better way to handle time limited executions
+  auto start_time = std::chrono::steady_clock::now();
+  while (rclcpp::ok() && (std::chrono::steady_clock::now() - start_time) < std::chrono::seconds(RUN_DURATION_SECONDS))
+  {
+    executor.spin_some();
+    std::this_thread::sleep_for(1ms);
+  }
+
+  rclcpp::shutdown();
+  return 0;
+}

src/simple_topology/CMakeLists.txt

@@ -1,16 +1,12 @@
 cmake_minimum_required(VERSION 3.5)
-project(simple_topology)
+project(simple_topology VERSION 0.1.0)
 
-# Default to C99
+# Set C++ standards
-if(NOT CMAKE_C_STANDARD)
+set(CMAKE_CXX_STANDARD 17)
-  set(CMAKE_C_STANDARD 99)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
-endif()
+set(CMAKE_CXX_EXTENSIONS OFF)
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
 
+# Compiler options
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
@@ -20,23 +16,38 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 
-add_executable(simple_topology src/simple_topology.cpp)
+add_executable(${PROJECT_NAME} src/simple_topology.cpp)
-ament_target_dependencies(simple_topology rclcpp std_msgs)
+target_include_directories(${PROJECT_NAME} PUBLIC
-install(TARGETS simple_topology DESTINATION lib/${PROJECT_NAME})
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+# Add dependencies with ament
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  std_msgs
+)
+
+# Installation
+install(
+  TARGETS ${PROJECT_NAME}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
 install(
   DIRECTORY launch
   DESTINATION share/${PROJECT_NAME}
 )
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
 
+# Testing
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+# Export and package configuration
+ament_export_include_directories(include)
 ament_package()

src/simple_topology/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>simple_topology</name>
-  <version>0.0.0</version>
+  <version>0.1.0</version>
   <description>TODO: Package description</description>
   <maintainer email="niklas@niklashalle.net">dev</maintainer>
   <license>TODO: License declaration</license>
@@ -14,6 +14,7 @@
   <depend>tracetools_trace</depend>
   <depend>tracetools_launch</depend>
   
+  <!-- Build Dependencies -->
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
 
@@ -24,11 +25,11 @@
   <test_depend>ament_xmllint</test_depend>
   <test_depend>python3-pytest</test_depend>
 
+  <!-- Test Dependencies -->
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
-    <build_type>ament_python</build_type>
   </export>
 </package>

src/simple_topology/src/simple_topology.cpp

@@ -1,4 +1,3 @@
-// --- includes ---
 #include <chrono>
 #include <memory>
 #include <string>
@@ -11,6 +10,29 @@ using namespace std::chrono_literals;
 
 constexpr int RUN_DURATION_SECONDS = 5;
 
+// Nodes in the topology:
+// - Chain 1: Image2Ground Mapping
+//   - Node 1: Camera Node (Sensor Node): Simulates a camera that publishes messages.
+//   - Node 2: De-Bayering Node (Processing Node): Processes the camera messages. Listens to Node 1.
+//   - Node 3: Radiometric Correction Node (Processing Node): Further processes the messages. Listens to Node 2.
+//   - Node 4: Geometric Correction Node (Processing Node): Applies geometric corrections. Listens to Node 3.
+//   - Node 5: Image to Ground Mapping Node (Processing Node): Maps the image to ground coordinates. Listens to Node 4.
+// - Chain 2: Semantic Scheduler Adaptation
+//   - Node 6: Smoke/Fire Classification Node (Decision Node): Listens to Node 3 and makes scheduluer decisions based on the processed data.
+// - Chain 3: Flight Control
+//   - Node 7: GPS Node (Sensor Node): Simulates a GPS sensor that publishes messages.
+//   - Node 8: IMU Node (Sensor Node): Simulates an IMU sensor that publishes messages.
+//   - Node 9: Barometric Altimeter Node (Sensor Node): Simulates a barometric altimeter that publishes messages.
+//   - Node 10: Sensor Fusion Node (Processing Node): Fuses data from the GPS, IMU, and barometric altimeter. Listens to Nodes 7, 8, and 9.
+//   - Node 11: LIDAR Node (Sensor Node): Simulates a LiDAR sensor that publishes messages.
+//   - Node 12: Operator Command Node (Sensor Node): Remote control or flight plan issuing node
+//   - Node 13: Flight Management Node (Processing Node): Manages flight operations. Listens to Node 10, 11, and 12.
+//   - Node 14: Flight Control Node (Processing Node): Controls the flight based on data from Node 13. Listens to Node 13.
+// - Chain 4: Data Storage/Streaming
+//   - Node 15: Telemetry Node (Processing Node): Collects telemetry data. Listens to Node 10 and 5.
+//   - Node 16: Radio Transmitter Node (Processing Node): Transmits data. Listens to Node 15.
+
+
 // --- Sensor Node ---
 class SensorNode : public rclcpp::Node
 {

src/tools/tracetools_analysis

@@ -1 +1 @@
-Subproject commit 00a3e8c3a6b67ce0dab01ef69e6abc42bb48bf10
+Subproject commit cf17b6348126f52cdb151e427a7904cbe6378f22