Node status executable

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]
     )

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}
 )
 

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 <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include <string>
+#include <vector>
+
+#include "interfaces/msg/node_list.hpp"
+
+class NodeStatusPublisher : public rclcpp::Node {
+public:
+  NodeStatusPublisher();
+
+private:
+  void publish_nodes();
+
+  double frequency_;
+
+  rclcpp::Publisher<interfaces::msg::NodeList>::SharedPtr publisher_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+#endif // NODE_STATUS_PUBLISHER_HPP

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 <rclcpp/qos.hpp>
+#include <sstream>
+#include <vector>
+
+NodeStatusPublisher::NodeStatusPublisher()
+    : Node("node_status_publisher"),
+      frequency_(this->declare_parameter<double>("frequency", 1.0)) {
+
+  rclcpp::QoS qos(10);
+  qos.transient_local();
+
+  publisher_ =
+      this->create_publisher<interfaces::msg::NodeList>("/system/nodes", qos);
+
+  const auto period = std::chrono::duration<double>(1.0 / frequency_);
+  timer_ = this->create_wall_timer(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(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<NodeStatusPublisher>());
+  rclcpp::shutdown();
+  return 0;
+}

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"

src/interfaces/msg/NodeList.msg

@@ -0,0 +1 @@
+string[] nodes