diff --git a/greenwave_monitor/CMakeLists.txt b/greenwave_monitor/CMakeLists.txt index 80dd5a6..52e8440 100644 --- a/greenwave_monitor/CMakeLists.txt +++ b/greenwave_monitor/CMakeLists.txt @@ -21,6 +21,7 @@ endif() # find dependencies find_package(ament_cmake_auto REQUIRED) +find_package(yaml-cpp REQUIRED) ament_auto_find_build_dependencies() # Add message_diagnostics.hpp as a header-only library @@ -35,8 +36,12 @@ ament_target_dependencies(greenwave_monitor std_msgs diagnostic_msgs greenwave_monitor_interfaces + yaml_cpp_vendor +) +target_link_libraries(greenwave_monitor + message_diagnostics + yaml-cpp ) -target_link_libraries(greenwave_monitor message_diagnostics) target_include_directories(greenwave_monitor PUBLIC $ @@ -60,7 +65,7 @@ install(TARGETS minimal_publisher_node DESTINATION lib/${PROJECT_NAME}) install( - DIRECTORY launch examples + DIRECTORY launch examples config DESTINATION share/${PROJECT_NAME} ) diff --git a/greenwave_monitor/config/type_registry.yml b/greenwave_monitor/config/type_registry.yml new file mode 100644 index 0000000..cf2691c --- /dev/null +++ b/greenwave_monitor/config/type_registry.yml @@ -0,0 +1,2 @@ +has_header: + - stereo_msgs/msg/DisparityImage \ No newline at end of file diff --git a/greenwave_monitor/include/greenwave_monitor.hpp b/greenwave_monitor/include/greenwave_monitor.hpp index f9a39bb..6a874ea 100644 --- a/greenwave_monitor/include/greenwave_monitor.hpp +++ b/greenwave_monitor/include/greenwave_monitor.hpp @@ -63,11 +63,14 @@ class GreenwaveMonitor : public rclcpp::Node bool has_header_from_type(const std::string & type_name); + bool has_header_from_type_registry(const std::string & type_name); + std::chrono::time_point GetTimestampFromSerializedMessage( std::shared_ptr serialized_message_ptr, const std::string & type); + std::string type_registry_path_; std::map> message_diagnostics_; std::vector> subscriptions_; diff --git a/greenwave_monitor/package.xml b/greenwave_monitor/package.xml index 84b0407..16fb73c 100644 --- a/greenwave_monitor/package.xml +++ b/greenwave_monitor/package.xml @@ -37,6 +37,7 @@ sensor_msgs greenwave_monitor_interfaces rclpy + yaml_cpp_vendor launch launch_ros diff --git a/greenwave_monitor/src/greenwave_monitor.cpp b/greenwave_monitor/src/greenwave_monitor.cpp index e73fc65..91d4f2e 100644 --- a/greenwave_monitor/src/greenwave_monitor.cpp +++ b/greenwave_monitor/src/greenwave_monitor.cpp @@ -17,6 +17,8 @@ #include "greenwave_monitor.hpp" +#include +#include #include #include #include @@ -26,6 +28,19 @@ 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(st.st_mode & S_IFREG); +} + +} // anonymous namespace + GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) : Node("greenwave_monitor", options) { @@ -35,6 +50,19 @@ GreenwaveMonitor::GreenwaveMonitor(const rclcpp::NodeOptions & options) this->declare_parameter>("topics", {""}); auto topics = this->get_parameter("topics").as_string_array(); + // Declare and get the type registry path parameter + this->declare_parameter("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_WARN( + 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; diagnostics_config.enable_all_topic_diagnostics = true; @@ -241,19 +269,65 @@ bool GreenwaveMonitor::has_header_from_type(const std::string & 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; + // 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() == type_name) { + return true; + } + } else { + RCLCPP_WARN( + this->get_logger(), + "Found a non-string value in the registry: %s. Skipping.", + type_node.as().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