PoC YAML registry

Author: 3473f

greenwave_monitor/CMakeLists.txt

@@ -35,6 +35,7 @@ ament_target_dependencies(greenwave_monitor
   std_msgs
   diagnostic_msgs
   greenwave_monitor_interfaces
+  yaml_cpp_vendor
 )
 target_link_libraries(greenwave_monitor message_diagnostics)
 

greenwave_monitor/config/type_registry.yml

@@ -0,0 +1,37 @@
+has_header:
+  # sensor_msgs
+  - sensor_msgs/msg/Image
+  - sensor_msgs/msg/CompressedImage
+  - sensor_msgs/msg/CameraInfo
+  - sensor_msgs/msg/PointCloud2
+  - sensor_msgs/msg/LaserScan
+  - sensor_msgs/msg/Imu
+  - sensor_msgs/msg/NavSatFix
+  - sensor_msgs/msg/MagneticField
+  - sensor_msgs/msg/FluidPressure
+  - sensor_msgs/msg/Illuminance
+  - sensor_msgs/msg/RelativeHumidity
+  - sensor_msgs/msg/Temperature
+  - sensor_msgs/msg/Range
+  - sensor_msgs/msg/PointCloud
+  
+  # geometry_msgs
+  - geometry_msgs/msg/PoseStamped
+  - geometry_msgs/msg/TwistStamped
+  - geometry_msgs/msg/AccelStamped
+  - geometry_msgs/msg/Vector3Stamped
+  - geometry_msgs/msg/PointStamped
+  - geometry_msgs/msg/QuaternionStamped
+  - geometry_msgs/msg/TransformStamped
+  - geometry_msgs/msg/WrenchStamped
+  
+  # nav_msgs
+  - nav_msgs/msg/OccupancyGrid
+  - nav_msgs/msg/GridCells
+  - nav_msgs/msg/Path
+  - nav_msgs/msg/Odometry
+  
+  # visualization_msgs
+  - visualization_msgs/msg/Marker
+  - visualization_msgs/msg/MarkerArray
+  - visualization_msgs/msg/InteractiveMarker

greenwave_monitor/include/greenwave_monitor.hpp

@@ -63,11 +63,15 @@ class GreenwaveMonitor : public rclcpp::Node
 
   bool has_header_from_type(const std::string & type_name);
 
+  bool read_type_registry(const std::string path);
+
   std::chrono::time_point<std::chrono::system_clock>
   GetTimestampFromSerializedMessage(
     std::shared_ptr<rclcpp::SerializedMessage> serialized_message_ptr,
     const std::string & type);
-
+  
+  std::string type_registry_path_;
+  std::unordered_map<std::string, bool> known_header_types_;
   std::map<std::string,
     std::unique_ptr<message_diagnostics::MessageDiagnostics>> message_diagnostics_;
   std::vector<std::shared_ptr<rclcpp::GenericSubscription>> subscriptions_;

greenwave_monitor/package.xml

@@ -36,6 +36,7 @@
   <depend>sensor_msgs</depend>
   <depend>greenwave_monitor_interfaces</depend>
   <depend>rclpy</depend>
+  <depend>yaml_cpp_vendor</depend>
 
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>

greenwave_monitor/src/greenwave_monitor.cpp

@@ -21,11 +21,24 @@
 #include <cstring>
 #include <mutex>
 #include <unordered_map>
+#include <sys/stat.h>
+#include <yaml-cpp/yaml.h>
 
 #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
 
 using namespace std::chrono_literals;
 
+namespace {
+
+// Check if a file exists and is a regular file
+bool is_valid_file(const std::string& path) {
+  struct stat st;
+  if (stat(path.c_str(), &st) != 0) { return false; }
+  return static_cast<bool>(st.st_mode & S_IFREG);
+}
+
+}  // anonymous namespace
+
 GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options)
 : Node("greenwave_monitor", options)
 {
@@ -35,6 +48,26 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options)
   this->declare_parameter<std::vector<std::string>>("topics", {""});
   auto topics = this->get_parameter("topics").as_string_array();
 
+  // Declare and get the type registry path parameter
+  this->declare_parameter<std::string>("type_registry_path", "/workspace/colcon_ws/src/greenwave_monitor/greenwave_monitor/config/type_registry.yml");
+  type_registry_path_ = this->get_parameter("type_registry_path").as_string();
+
+  // Check if the type registry path is valid
+  if (!type_registry_path_.empty() && !is_valid_file(type_registry_path_)) {
+    RCLCPP_ERROR(
+      this->get_logger(), "Type registry path '%s' is not a valid file",
+      type_registry_path_.c_str());
+    type_registry_path_.clear();
+    return;
+  }
+
+  if(!read_type_registry(type_registry_path_)) {
+    RCLCPP_WARN(
+      this->get_logger(), "Failed to read type registry from path '%s'. "
+      "Assuming no message types have a header. Consider providing a valid type registry.",
+      type_registry_path_.c_str());
+  }
+
   message_diagnostics::MessageDiagnosticsConfig diagnostics_config;
   diagnostics_config.enable_all_topic_diagnostics = true;
 
@@ -90,6 +123,45 @@ void GreenwaveMonitor::topic_callback(
   message_diagnostics_[topic]->updateDiagnostics(msg_timestamp.time_since_epoch().count());
 }
 
+// Read YAML type registry
+bool GreenwaveMonitor::read_type_registry(const std::string path) {
+  try {
+    YAML::Node config = YAML::LoadFile(path);
+    if (config["has_header"]) {
+      // Check if 'has_header' is a sequence (list)
+      if (config["has_header"].IsSequence()) {
+        for (const auto& type_node : config["has_header"]) {
+          if (type_node.IsScalar()) {
+            known_header_types_[type_node.as<std::string>()] = true;
+          } else {
+            RCLCPP_WARN(
+              this->get_logger(),
+              "Found a non-string value in the registry: %s. Skipping.",
+              type_node.as<std::string>().c_str());
+          }
+        }
+      } else {
+        RCLCPP_ERROR(
+          this->get_logger(),
+          "'has_header' key is not a sequence in the YAML file.");
+        return false;
+      }
+    } else {
+      RCLCPP_ERROR(
+        this->get_logger(),
+        "'has_header' key is not found in the YAML file.");
+      return false;
+    }
+  } catch (const YAML::BadFile& e) {
+    RCLCPP_ERROR(this->get_logger(), "Error reading YAML file: %s", e.what());
+  } catch (const YAML::ParserException& e) {
+    RCLCPP_ERROR(this->get_logger(), "Error parsing YAML string: %s", e.what());
+  } catch (const std::exception& e) {
+    RCLCPP_ERROR(this->get_logger(), "An unexpected error occurred: %s", e.what());
+  }
+  return true;
+}
+
 void GreenwaveMonitor::timer_callback()
 {
   RCLCPP_INFO(this->get_logger(), "====================================================");
@@ -182,69 +254,13 @@ bool GreenwaveMonitor::has_header_from_type(const std::string & type_name)
     return type_has_header_cache[type_name];
   }
 
-  // rosidl typesupport API is unstable across ROS distributions, so we use this
-  // map as a more robust way to determine if a message type has a header
-  static const std::unordered_map<std::string, bool> known_header_types = {
-    // sensor_msgs
-    {"sensor_msgs/msg/Image", true},
-    {"sensor_msgs/msg/CompressedImage", true},
-    {"sensor_msgs/msg/CameraInfo", true},
-    {"sensor_msgs/msg/PointCloud2", true},
-    {"sensor_msgs/msg/LaserScan", true},
-    {"sensor_msgs/msg/Imu", true},
-    {"sensor_msgs/msg/NavSatFix", true},
-    {"sensor_msgs/msg/MagneticField", true},
-    {"sensor_msgs/msg/FluidPressure", true},
-    {"sensor_msgs/msg/Illuminance", true},
-    {"sensor_msgs/msg/RelativeHumidity", true},
-    {"sensor_msgs/msg/Temperature", true},
-    {"sensor_msgs/msg/Range", true},
-    {"sensor_msgs/msg/PointCloud", true},
-
-    // geometry_msgs
-    {"geometry_msgs/msg/PoseStamped", true},
-    {"geometry_msgs/msg/TwistStamped", true},
-    {"geometry_msgs/msg/AccelStamped", true},
-    {"geometry_msgs/msg/Vector3Stamped", true},
-    {"geometry_msgs/msg/PointStamped", true},
-    {"geometry_msgs/msg/QuaternionStamped", true},
-    {"geometry_msgs/msg/TransformStamped", true},
-    {"geometry_msgs/msg/WrenchStamped", true},
-
-    // nav_msgs
-    {"nav_msgs/msg/OccupancyGrid", true},
-    {"nav_msgs/msg/GridCells", true},
-    {"nav_msgs/msg/Path", true},
-    {"nav_msgs/msg/Odometry", true},
-
-    // visualization_msgs
-    {"visualization_msgs/msg/Marker", true},
-    {"visualization_msgs/msg/MarkerArray", true},
-    {"visualization_msgs/msg/InteractiveMarker", true},
-
-    // std_msgs (no headers)
-    {"std_msgs/msg/String", false},
-    {"std_msgs/msg/Int32", false},
-    {"std_msgs/msg/Float64", false},
-    {"std_msgs/msg/Bool", false},
-    {"std_msgs/msg/Empty", false},
-    {"std_msgs/msg/Header", false},  // Header itself doesn't have a header
-
-    // Common message types without headers
-    {"geometry_msgs/msg/Twist", false},
-    {"geometry_msgs/msg/Pose", false},
-    {"geometry_msgs/msg/Point", false},
-    {"geometry_msgs/msg/Vector3", false},
-    {"geometry_msgs/msg/Quaternion", false}
-  };
-
-  auto it = known_header_types.find(type_name);
-  bool has_header = (it != known_header_types.end()) ? it->second : false;
+  auto it = known_header_types_.find(type_name);
+  bool has_header = (it != known_header_types_.end()) ? it->second : false;
 
   type_has_header_cache[type_name] = has_header;
 
   // Fallback of no header in case of unknown type, log for reference
-  if (it == known_header_types.end()) {
+  if (it == known_header_types_.end()) {
     RCLCPP_WARN_ONCE(
       this->get_logger(),
       "Unknown message type '%s' - assuming no header. Consider adding to registry.",