From 310b6bfccfc7c03defe0eba29f25d494a30b689f Mon Sep 17 00:00:00 2001 From: Niklas Halle Date: Fri, 2 May 2025 15:32:04 +0200 Subject: [PATCH] Fri May 2 03:32:04 PM CEST 2025 --- Dockerfile | 2 +- src/full_topology/CMakeLists.txt | 53 ++++++ .../launch/trace_full_topology.launch.py | 22 +++ src/full_topology/package.xml | 35 ++++ src/full_topology/src/full_topology.cpp | 178 ++++++++++++++++++ src/simple_topology/CMakeLists.txt | 51 +++-- src/simple_topology/package.xml | 5 +- src/simple_topology/src/simple_topology.cpp | 24 ++- src/tools/tracetools_analysis | 2 +- 9 files changed, 347 insertions(+), 25 deletions(-) create mode 100644 src/full_topology/CMakeLists.txt create mode 100644 src/full_topology/launch/trace_full_topology.launch.py create mode 100644 src/full_topology/package.xml create mode 100644 src/full_topology/src/full_topology.cpp diff --git a/Dockerfile b/Dockerfile index e999dac..6af618f 100644 --- a/Dockerfile +++ b/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 diff --git a/src/full_topology/CMakeLists.txt b/src/full_topology/CMakeLists.txt new file mode 100644 index 0000000..1ff30a6 --- /dev/null +++ b/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 + $ + $ +) + +# 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() \ No newline at end of file diff --git a/src/full_topology/launch/trace_full_topology.launch.py b/src/full_topology/launch/trace_full_topology.launch.py new file mode 100644 index 0000000..0c340d9 --- /dev/null +++ b/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', + ), + ]) diff --git a/src/full_topology/package.xml b/src/full_topology/package.xml new file mode 100644 index 0000000..4de6334 --- /dev/null +++ b/src/full_topology/package.xml @@ -0,0 +1,35 @@ + + + + full_topology + 0.1.0 + TODO: Package description + dev + TODO: License declaration + + ament_cmake + + launch + launch_ros + tracetools_trace + tracetools_launch + + + rclcpp + std_msgs + + ament_copyright + ament_flake8 + ament_mypy + ament_pep257 + ament_xmllint + python3-pytest + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/full_topology/src/full_topology.cpp b/src/full_topology/src/full_topology.cpp new file mode 100644 index 0000000..708f47c --- /dev/null +++ b/src/full_topology/src/full_topology.cpp @@ -0,0 +1,178 @@ +// full_topology.cpp + +#include +#include +#include +#include +#include +#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(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::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( + 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(output_topic, 10); + } +private: + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Publisher::SharedPtr publisher_; +}; + +// A processing node that fuses multiple inputs +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) + { + 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()); + } + }); + subscriptions_.push_back(sub); + } + publisher_ = create_publisher(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::SharedPtr> subscriptions_; + rclcpp::Publisher::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("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) {}}; + 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) {}}; + 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) {}}; + auto radio = std::make_shared("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; +} diff --git a/src/simple_topology/CMakeLists.txt b/src/simple_topology/CMakeLists.txt index 548d51b..027a67f 100644 --- a/src/simple_topology/CMakeLists.txt +++ b/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 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() +# 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() @@ -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) -ament_target_dependencies(simple_topology rclcpp std_msgs) -install(TARGETS simple_topology DESTINATION lib/${PROJECT_NAME}) +add_executable(${PROJECT_NAME} src/simple_topology.cpp) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +# 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() -ament_package() +# Export and package configuration +ament_export_include_directories(include) +ament_package() \ No newline at end of file diff --git a/src/simple_topology/package.xml b/src/simple_topology/package.xml index 6fc7362..2ec3890 100644 --- a/src/simple_topology/package.xml +++ b/src/simple_topology/package.xml @@ -2,7 +2,7 @@ simple_topology - 0.0.0 + 0.1.0 TODO: Package description dev TODO: License declaration @@ -14,6 +14,7 @@ tracetools_trace tracetools_launch + rclcpp std_msgs @@ -24,11 +25,11 @@ ament_xmllint python3-pytest + ament_lint_auto ament_lint_common ament_cmake - ament_python diff --git a/src/simple_topology/src/simple_topology.cpp b/src/simple_topology/src/simple_topology.cpp index e8e2489..2165157 100644 --- a/src/simple_topology/src/simple_topology.cpp +++ b/src/simple_topology/src/simple_topology.cpp @@ -1,4 +1,3 @@ -// --- includes --- #include #include #include @@ -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 { diff --git a/src/tools/tracetools_analysis b/src/tools/tracetools_analysis index 00a3e8c..cf17b63 160000 --- a/src/tools/tracetools_analysis +++ b/src/tools/tracetools_analysis @@ -1 +1 @@ -Subproject commit 00a3e8c3a6b67ce0dab01ef69e6abc42bb48bf10 +Subproject commit cf17b6348126f52cdb151e427a7904cbe6378f22