Fri May 2 03:32:04 PM CEST 2025
This commit is contained in:
parent
1531c9adc1
commit
310b6bfccf
9 changed files with 347 additions and 25 deletions
53
src/full_topology/CMakeLists.txt
Normal file
53
src/full_topology/CMakeLists.txt
Normal file
|
|
@ -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()
|
||||
22
src/full_topology/launch/trace_full_topology.launch.py
Normal file
22
src/full_topology/launch/trace_full_topology.launch.py
Normal file
|
|
@ -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',
|
||||
),
|
||||
])
|
||||
35
src/full_topology/package.xml
Normal file
35
src/full_topology/package.xml
Normal file
|
|
@ -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>
|
||||
178
src/full_topology/src/full_topology.cpp
Normal file
178
src/full_topology/src/full_topology.cpp
Normal file
|
|
@ -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;
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue