diff --git a/src/Bringup/launch/core.launch.py b/src/Bringup/launch/core.launch.py index 12c64e56..8c89c1b5 100644 --- a/src/Bringup/launch/core.launch.py +++ b/src/Bringup/launch/core.launch.py @@ -38,9 +38,12 @@ def generate_launch_description(): output="screen", ) + status_node = Node( + package="system-telemetry-cpp", + executable="node_status_publisher", + name="node_status_publisher", + ) + return LaunchDescription( - get_included_launch_descriptions(launch_files) - + [ - snmp_node, - ] + get_included_launch_descriptions(launch_files) + [snmp_node, status_node] ) diff --git a/src/Teleop-Control/system-telemetry-cpp/CMakeLists.txt b/src/Teleop-Control/system-telemetry-cpp/CMakeLists.txt index eec9cc89..9dfca1e1 100644 --- a/src/Teleop-Control/system-telemetry-cpp/CMakeLists.txt +++ b/src/Teleop-Control/system-telemetry-cpp/CMakeLists.txt @@ -36,11 +36,20 @@ add_executable(system_telemetry_publisher src/gpu_collector.cpp ) +add_executable(node_status_publisher + src/node_status_publisher.cpp +) + ament_target_dependencies(system_telemetry_publisher rclcpp interfaces ) +ament_target_dependencies(node_status_publisher + rclcpp + interfaces +) + add_executable(snmp_network_stats src/snmp_network_stats.cpp ) @@ -62,6 +71,7 @@ target_link_libraries(snmp_network_stats install(TARGETS system_telemetry_publisher snmp_network_stats + node_status_publisher DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/node_status_publisher.hpp b/src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/node_status_publisher.hpp new file mode 100644 index 00000000..8c5cd904 --- /dev/null +++ b/src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/node_status_publisher.hpp @@ -0,0 +1,25 @@ +#ifndef NODE_STATUS_PUBLISHER_HPP +#define NODE_STATUS_PUBLISHER_HPP + +#include +#include + +#include +#include + +#include "interfaces/msg/node_list.hpp" + +class NodeStatusPublisher : public rclcpp::Node { +public: + NodeStatusPublisher(); + +private: + void publish_nodes(); + + double frequency_; + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +#endif // NODE_STATUS_PUBLISHER_HPP \ No newline at end of file diff --git a/src/Teleop-Control/system-telemetry-cpp/src/node_status_publisher.cpp b/src/Teleop-Control/system-telemetry-cpp/src/node_status_publisher.cpp new file mode 100644 index 00000000..21117ee9 --- /dev/null +++ b/src/Teleop-Control/system-telemetry-cpp/src/node_status_publisher.cpp @@ -0,0 +1,37 @@ +#include "../include/system-telemetry-cpp/node_status_publisher.hpp" + +#include +#include +#include + +NodeStatusPublisher::NodeStatusPublisher() + : Node("node_status_publisher"), + frequency_(this->declare_parameter("frequency", 1.0)) { + + rclcpp::QoS qos(10); + qos.transient_local(); + + publisher_ = + this->create_publisher("/system/nodes", qos); + + const auto period = std::chrono::duration(1.0 / frequency_); + timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&NodeStatusPublisher::publish_nodes, this)); + + RCLCPP_INFO(this->get_logger(), "NodeStatusPublisher started (ROS-native)"); +} + +void NodeStatusPublisher::publish_nodes() { + interfaces::msg::NodeList msg; + msg.nodes = this->get_node_names(); + + publisher_->publish(msg); +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/interfaces/CMakeLists.txt b/src/interfaces/CMakeLists.txt index aece4b06..a21dce11 100644 --- a/src/interfaces/CMakeLists.txt +++ b/src/interfaces/CMakeLists.txt @@ -24,6 +24,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/EspSensorReadings.msg" "msg/GasSensorReading.msg" "msg/MoveGroupStatus.msg" + "msg/NodeList.msg" "msg/ObjectDetected.msg" "msg/PolarimeterSweep.msg" "msg/PwmCommand.msg" diff --git a/src/interfaces/msg/NodeList.msg b/src/interfaces/msg/NodeList.msg new file mode 100644 index 00000000..566e2d01 --- /dev/null +++ b/src/interfaces/msg/NodeList.msg @@ -0,0 +1 @@ +string[] nodes \ No newline at end of file