Add type registry file

Signed-off-by: Mohamed Abdelaziz <[email protected]>

Author: 3473f

greenwave_monitor/CMakeLists.txt

@@ -36,6 +36,7 @@ ament_target_dependencies(greenwave_monitor
   diagnostic_msgs
   greenwave_monitor_interfaces
   yaml_cpp_vendor
+  ament_index_cpp
 )
 target_link_libraries(greenwave_monitor message_diagnostics)
 
@@ -61,7 +62,7 @@ install(TARGETS minimal_publisher_node
   DESTINATION lib/${PROJECT_NAME})
 
 install(
-  DIRECTORY launch examples
+  DIRECTORY launch examples config
   DESTINATION share/${PROJECT_NAME}
 )
 

greenwave_monitor/config/type_registry.yml

@@ -1,37 +1,2 @@
 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
+  - stereo_msgs/msg/DisparityImage

greenwave_monitor/include/greenwave_monitor.hpp

@@ -63,15 +63,14 @@ class GreenwaveMonitor : public rclcpp::Node
 
   bool has_header_from_type(const std::string & type_name);
 
-  bool read_type_registry(const std::string path);
+  bool has_header_from_type_registry(const std::string & type_name);
 
   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>ament_index_cpp</depend>
   <depend>yaml_cpp_vendor</depend>
 
   <exec_depend>launch</exec_depend>

greenwave_monitor/src/greenwave_monitor.cpp

@@ -24,6 +24,7 @@
 #include <sys/stat.h>
 #include <yaml-cpp/yaml.h>
 
+#include "ament_index_cpp/get_package_share_directory.hpp"
 #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
 
 using namespace std::chrono_literals;
@@ -49,23 +50,17 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options)
   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");
+  std::string default_type_registry_path = ament_index_cpp::get_package_share_directory("greenwave_monitor") + "/config/type_registry.yml";
+  this->declare_parameter<std::string>("type_registry_path", default_type_registry_path);
   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.",
+      this->get_logger(), "Type registry path '%s' is not a valid file."
+      "Falling back to build-in type registry.",
       type_registry_path_.c_str());
+    type_registry_path_.clear();
   }
 
   message_diagnostics::MessageDiagnosticsConfig diagnostics_config;
@@ -123,45 +118,6 @@ 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(), "====================================================");
@@ -254,22 +210,123 @@ bool GreenwaveMonitor::has_header_from_type(const std::string & type_name)
     return type_has_header_cache[type_name];
   }
 
-  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;
+  // 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;
+
+  // In case the type is not in the known list, attempt to read from type registry
+  if (it == known_header_types.end() && !type_registry_path_.empty()) {
+    has_header = has_header_from_type_registry(type_name);
+  }
 
   // Fallback of no header in case of unknown type, log for reference
-  if (it == known_header_types_.end()) {
+  if (it == known_header_types.end() && !has_header) {
     RCLCPP_WARN_ONCE(
       this->get_logger(),
       "Unknown message type '%s' - assuming no header. Consider adding to registry.",
       type_name.c_str());
   }
 
+  // Cache the result for future lookups
+  type_has_header_cache[type_name] = has_header;
+
   return has_header;
 }
 
+bool GreenwaveMonitor::has_header_from_type_registry(const std::string & type_name) {
+  try {
+    YAML::Node config = YAML::LoadFile(type_registry_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()) {
+            // Check if the type matches
+            if (type_node.as<std::string>() == type_name) {
+              return 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.");
+      }
+    } else {
+      RCLCPP_ERROR(
+        this->get_logger(),
+        "'has_header' key is not found in the YAML file.");
+    }
+  } 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 false;
+}
+
 bool GreenwaveMonitor::add_topic(const std::string & topic, std::string & message)
 {
   // Check if topic already exists