diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 3ca72b9..3f22794 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,5 +1,6 @@ { "name": "ROS2 ros2_medkit Development", + "runArgs": ["--network=host"], "build": { "dockerfile": "Dockerfile", "context": "..", diff --git a/postman/collections/ros2-medkit-gateway.postman_collection.json b/postman/collections/ros2-medkit-gateway.postman_collection.json index a31a18f..3d1e6c0 100644 --- a/postman/collections/ros2-medkit-gateway.postman_collection.json +++ b/postman/collections/ros2-medkit-gateway.postman_collection.json @@ -76,7 +76,7 @@ "components" ] }, - "description": "List all discovered components across all areas. Returns component metadata including id, namespace, fqn, type, parent area, and available operations (services and actions). Each operation includes type_info with schema information: services have request/response schemas, actions have goal/result/feedback schemas." + "description": "List all discovered components across all areas. Returns component metadata including id, namespace, fqn, type, area, source (node or topic), and available operations (services and actions). The 'source' field indicates whether the component was discovered from a ROS 2 node ('node') or from topic namespaces ('topic' - for systems like Isaac Sim)." }, "response": [] }, diff --git a/src/ros2_medkit_fault_manager/src/fault_storage.cpp b/src/ros2_medkit_fault_manager/src/fault_storage.cpp index 8e38af6..5e646a1 100644 --- a/src/ros2_medkit_fault_manager/src/fault_storage.cpp +++ b/src/ros2_medkit_fault_manager/src/fault_storage.cpp @@ -92,7 +92,8 @@ InMemoryFaultStorage::get_faults(bool filter_by_severity, uint8_t severity, // Determine which statuses to include std::set status_filter; if (statuses.empty()) { - // Default: CONFIRMED only + // Default: PENDING and CONFIRMED (exclude CLEARED) + status_filter.insert(ros2_medkit_msgs::msg::Fault::STATUS_PENDING); status_filter.insert(ros2_medkit_msgs::msg::Fault::STATUS_CONFIRMED); } else { for (const auto & s : statuses) { diff --git a/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp b/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp index f20d58f..18d4a5a 100644 --- a/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp +++ b/src/ros2_medkit_fault_manager/test/test_fault_manager.cpp @@ -62,16 +62,17 @@ TEST_F(FaultStorageTest, ReportExistingFaultUpdates) { EXPECT_EQ(fault->reporting_sources.size(), 2u); } -TEST_F(FaultStorageTest, GetFaultsDefaultReturnsConfirmedOnly) { +TEST_F(FaultStorageTest, GetFaultsDefaultReturnsPendingAndConfirmed) { rclcpp::Clock clock; auto timestamp = clock.now(); // Report a fault (starts as PENDING) storage_.report_fault("FAULT_1", Fault::SEVERITY_ERROR, "Test", "/node1", timestamp); - // Default query should return empty (only PENDING exists) + // Default query should return PENDING faults (PENDING + CONFIRMED by default) auto faults = storage_.get_faults(false, 0, {}); - EXPECT_EQ(faults.size(), 0u); + EXPECT_EQ(faults.size(), 1u); + EXPECT_EQ(faults[0].status, Fault::STATUS_PENDING); } TEST_F(FaultStorageTest, GetFaultsWithPendingStatus) { diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index beccf6e..6c424bf 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -127,6 +127,17 @@ if(BUILD_TESTING) ament_add_gtest(test_configuration_manager test/test_configuration_manager.cpp) target_link_libraries(test_configuration_manager gateway_lib) + # Add NativeTopicSampler tests + find_package(std_msgs REQUIRED) + ament_add_gtest(test_native_topic_sampler test/test_native_topic_sampler.cpp) + target_link_libraries(test_native_topic_sampler gateway_lib) + ament_target_dependencies(test_native_topic_sampler std_msgs) + + # Add DiscoveryManager tests + ament_add_gtest(test_discovery_manager test/test_discovery_manager.cpp) + target_link_libraries(test_discovery_manager gateway_lib) + ament_target_dependencies(test_discovery_manager std_msgs) + # Apply coverage flags to test targets if(ENABLE_COVERAGE) target_compile_options(test_gateway_node PRIVATE --coverage -O0 -g) @@ -135,6 +146,10 @@ if(BUILD_TESTING) target_link_options(test_operation_manager PRIVATE --coverage) target_compile_options(test_configuration_manager PRIVATE --coverage -O0 -g) target_link_options(test_configuration_manager PRIVATE --coverage) + target_compile_options(test_native_topic_sampler PRIVATE --coverage -O0 -g) + target_link_options(test_native_topic_sampler PRIVATE --coverage) + target_compile_options(test_discovery_manager PRIVATE --coverage -O0 -g) + target_link_options(test_discovery_manager PRIVATE --coverage) endif() # Integration testing diff --git a/src/ros2_medkit_gateway/README.md b/src/ros2_medkit_gateway/README.md index 2cc9052..be6e8d3 100644 --- a/src/ros2_medkit_gateway/README.md +++ b/src/ros2_medkit_gateway/README.md @@ -90,23 +90,26 @@ curl http://localhost:8080/api/v1/components "namespace": "/powertrain/engine", "fqn": "/powertrain/engine/temp_sensor", "type": "Component", - "area": "powertrain" + "area": "powertrain", + "source": "node" }, { - "id": "rpm_sensor", - "namespace": "/powertrain/engine", - "fqn": "/powertrain/engine/rpm_sensor", + "id": "carter1", + "namespace": "/carter1", + "fqn": "/carter1", "type": "Component", - "area": "powertrain" + "area": "carter1", + "source": "topic" } ] ``` **Response Fields:** -- `id` - Component name (node name) +- `id` - Component name (node name or namespace for topic-based) - `namespace` - ROS 2 namespace where the component is running - `fqn` - Fully qualified name (namespace + node name) - `type` - Always "Component" +- `source` - Discovery source: `"node"` (standard ROS 2 node) or `"topic"` (discovered from topic namespaces) - `area` - Parent area this component belongs to #### GET /api/v1/areas/{area_id}/components @@ -126,14 +129,16 @@ curl http://localhost:8080/api/v1/areas/powertrain/components "namespace": "/powertrain/engine", "fqn": "/powertrain/engine/temp_sensor", "type": "Component", - "area": "powertrain" + "area": "powertrain", + "source": "node" }, { "id": "rpm_sensor", "namespace": "/powertrain/engine", "fqn": "/powertrain/engine/rpm_sensor", "type": "Component", - "area": "powertrain" + "area": "powertrain", + "source": "node" } ] ``` @@ -659,6 +664,24 @@ cors: - **REST Server**: HTTP server using cpp-httplib - **Entity Cache**: In-memory cache of discovered areas and components, updated periodically +### Topic-Based Discovery + +In addition to standard ROS 2 node discovery, the gateway supports **topic-based discovery** for systems that publish topics without creating discoverable nodes (e.g., NVIDIA Isaac Sim, hardware bridges). + +**How it works:** +1. Gateway scans all topics in the ROS 2 graph +2. Extracts unique namespace prefixes (e.g., `/carter1/odom` → `carter1`) +3. Creates virtual "components" for namespaces that have topics but no nodes +4. These components have `"source": "topic"` to distinguish them from node-based components + +**Example:** Isaac Sim publishes topics like `/carter1/odom`, `/carter1/cmd_vel`, `/carter2/imu` without creating ROS 2 nodes. The gateway discovers: +- Component `carter1` with topics: `/carter1/odom`, `/carter1/cmd_vel` +- Component `carter2` with topics: `/carter2/imu` + +**System topic filtering:** The following topics are filtered out during discovery: +- `/parameter_events`, `/rosout`, `/clock` +- Note: `/tf` and `/tf_static` are NOT filtered (useful for diagnostics) + ### Area Organization The gateway organizes nodes into "areas" based on their namespace: diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery_manager.hpp index 81afb09..6f242f6 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery_manager.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery_manager.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -34,6 +35,25 @@ class DiscoveryManager { std::vector discover_areas(); std::vector discover_components(); + /** + * @brief Discover components from topic namespaces (topic-based discovery) + * + * Creates "virtual" components for topic namespaces that don't have + * corresponding ROS 2 nodes. This is useful for systems like Isaac Sim + * that publish topics without creating proper ROS 2 nodes. + * + * Example: Topics ["/carter1/odom", "/carter1/cmd_vel", "/carter2/odom"] + * Creates components: carter1, carter2 (if no matching nodes exist) + * + * Components are created with: + * - id: namespace name (e.g., "carter1") + * - source: "topic" (to distinguish from node-based components) + * - topics.publishes: all topics under this namespace + * + * @return Vector of topic-based components (excludes namespaces with existing nodes) + */ + std::vector discover_topic_components(); + /// Discover all services in the system with their types std::vector discover_services(); @@ -87,6 +107,9 @@ class DiscoveryManager { /// Extract the last segment from a path (e.g., "/a/b/c" -> "c") std::string extract_name_from_path(const std::string & path); + /// Get set of namespaces that have ROS 2 nodes (for deduplication) + std::set get_node_namespaces(); + /// Check if a service path belongs to a component namespace bool path_belongs_to_namespace(const std::string & path, const std::string & ns) const; diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/models.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/models.hpp index 553db10..f7b8f4c 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/models.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/models.hpp @@ -147,13 +147,14 @@ struct Component { std::string fqn; std::string type = "Component"; std::string area; + std::string source = "node"; ///< Discovery source: "node" or "topic" std::vector services; std::vector actions; ComponentTopics topics; ///< Topics this component publishes/subscribes json to_json() const { - json j = {{"id", id}, {"namespace", namespace_path}, {"fqn", fqn}, {"type", type}, - {"area", area}, {"topics", topics.to_json()}}; + json j = {{"id", id}, {"namespace", namespace_path}, {"fqn", fqn}, {"type", type}, {"area", area}, + {"source", source}, {"topics", topics.to_json()}}; // Add operations array combining services and actions json operations = json::array(); diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/native_topic_sampler.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/native_topic_sampler.hpp index 9b4c7cb..7660123 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/native_topic_sampler.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/native_topic_sampler.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -199,6 +200,85 @@ class NativeTopicSampler { */ ComponentTopics get_component_topics(const std::string & component_fqn); + /** + * @brief Result of topic-based discovery containing namespaces and their topics + * + * This struct aggregates all topic-based discovery results to avoid + * multiple ROS 2 graph queries (N+1 query problem). + */ + struct TopicDiscoveryResult { + std::set namespaces; ///< Unique namespace prefixes + std::map topics_by_ns; ///< Topics grouped by namespace + }; + + /** + * @brief Discover namespaces and their topics in a single graph query + * + * This method performs a single call to get_topic_names_and_types() and + * processes all topics to extract namespaces and group topics by namespace. + * This avoids the N+1 query problem of calling discover_topic_namespaces() + * followed by get_topics_for_namespace() for each namespace. + * + * Example: Topics ["/carter1/odom", "/carter1/cmd_vel", "/carter2/imu"] + * Returns: { + * namespaces: {"carter1", "carter2"}, + * topics_by_ns: { + * "/carter1": {publishes: ["/carter1/odom", "/carter1/cmd_vel"]}, + * "/carter2": {publishes: ["/carter2/imu"]} + * } + * } + * + * @return TopicDiscoveryResult with namespaces and topics grouped by namespace + */ + TopicDiscoveryResult discover_topics_by_namespace(); + + /** + * @brief Discover unique namespace prefixes from all topics + * + * Extracts the first segment of each topic path to identify namespaces. + * Used for topic-based component discovery when nodes are not available + * (e.g., Isaac Sim publishing topics without creating ROS 2 nodes). + * + * Example: Topics ["/carter1/odom", "/carter2/cmd_vel", "/tf"] + * Returns: {"carter1", "carter2"} (root topics like /tf are excluded) + * + * @note Consider using discover_topics_by_namespace() instead to avoid N+1 queries + * + * @return Set of unique namespace prefixes (without leading slash) + */ + std::set discover_topic_namespaces(); + + /** + * @brief Get all topics under a specific namespace prefix + * + * Returns ComponentTopics containing all topics that start with the given + * namespace prefix. For topic-based discovery, all matched topics are + * placed in the 'publishes' list since direction cannot be determined + * without node information. + * + * @note Consider using discover_topics_by_namespace() instead to avoid N+1 queries + * + * @param ns_prefix Namespace prefix including leading slash (e.g., "/carter1") + * @return ComponentTopics with matching topics in publishes list + */ + ComponentTopics get_topics_for_namespace(const std::string & ns_prefix); + + /** + * @brief Check if a topic is a ROS 2 system/infrastructure topic + * + * System topics are filtered out during topic-based discovery to avoid + * creating spurious components. Filtered topics include: + * - /parameter_events + * - /rosout + * - /clock + * + * Note: /tf and /tf_static are NOT filtered (useful for diagnostics). + * + * @param topic_name Full topic path + * @return true if this is a system topic that should be filtered + */ + static bool is_system_topic(const std::string & topic_name); + private: /** * @brief Parse YAML-formatted message string to JSON diff --git a/src/ros2_medkit_gateway/src/discovery_manager.cpp b/src/ros2_medkit_gateway/src/discovery_manager.cpp index f731f32..afb95eb 100644 --- a/src/ros2_medkit_gateway/src/discovery_manager.cpp +++ b/src/ros2_medkit_gateway/src/discovery_manager.cpp @@ -51,6 +51,14 @@ std::vector DiscoveryManager::discover_areas() { area_set.insert(area); } + // Also include areas from topic namespaces (for topic-based discovery) + if (topic_sampler_) { + auto topic_namespaces = topic_sampler_->discover_topic_namespaces(); + for (const auto & ns : topic_namespaces) { + area_set.insert(ns); + } + } + // Convert set to vector of Area structs std::vector areas; for (const auto & area_name : area_set) { @@ -360,6 +368,71 @@ std::string DiscoveryManager::extract_name_from_path(const std::string & path) { return path; } +std::set DiscoveryManager::get_node_namespaces() { + std::set namespaces; + + auto node_graph = node_->get_node_graph_interface(); + auto names_and_namespaces = node_graph->get_node_names_and_namespaces(); + + for (const auto & name_and_ns : names_and_namespaces) { + std::string ns = name_and_ns.second; + std::string area = extract_area_from_namespace(ns); + if (area != "root") { + namespaces.insert(area); + } + } + + return namespaces; +} + +std::vector DiscoveryManager::discover_topic_components() { + std::vector components; + + if (!topic_sampler_) { + RCLCPP_DEBUG(node_->get_logger(), "Topic sampler not set, skipping topic-based discovery"); + return components; + } + + // Single graph query - get all namespaces and their topics at once (avoids N+1 queries) + auto discovery_result = topic_sampler_->discover_topics_by_namespace(); + + // Get namespaces that already have nodes (to avoid duplicates) + auto node_namespaces = get_node_namespaces(); + + RCLCPP_DEBUG(node_->get_logger(), "Topic-based discovery: %zu topic namespaces, %zu node namespaces", + discovery_result.namespaces.size(), node_namespaces.size()); + + for (const auto & ns : discovery_result.namespaces) { + // Skip if there's already a node with this namespace + if (node_namespaces.count(ns) > 0) { + RCLCPP_DEBUG(node_->get_logger(), "Skipping namespace '%s' - already has nodes", ns.c_str()); + continue; + } + + Component comp; + comp.id = ns; + comp.namespace_path = "/" + ns; + comp.fqn = "/" + ns; + comp.area = ns; + comp.source = "topic"; + + // Get topics from cached result (no additional graph query) + std::string ns_prefix = "/" + ns; + auto it = discovery_result.topics_by_ns.find(ns_prefix); + if (it != discovery_result.topics_by_ns.end()) { + comp.topics = it->second; + } + + RCLCPP_DEBUG(node_->get_logger(), "Created topic-based component '%s' with %zu topics", ns.c_str(), + comp.topics.publishes.size()); + + components.push_back(comp); + } + + RCLCPP_INFO(node_->get_logger(), "Discovered %zu topic-based components", components.size()); + return components; +} + bool DiscoveryManager::path_belongs_to_namespace(const std::string & path, const std::string & ns) const { if (ns.empty() || ns == "/") { // Root namespace - check if path has only one segment after leading slash diff --git a/src/ros2_medkit_gateway/src/gateway_node.cpp b/src/ros2_medkit_gateway/src/gateway_node.cpp index a94b4f6..8b172b5 100644 --- a/src/ros2_medkit_gateway/src/gateway_node.cpp +++ b/src/ros2_medkit_gateway/src/gateway_node.cpp @@ -171,22 +171,37 @@ void GatewayNode::refresh_cache() { // Discover data outside the lock to minimize lock time auto areas = discovery_mgr_->discover_areas(); - auto components = discovery_mgr_->discover_components(); + + // Discover node-based components (standard ROS 2 nodes) + auto node_components = discovery_mgr_->discover_components(); + + // Discover topic-based components (for systems like Isaac Sim that + // publish topics without creating proper ROS 2 nodes) + auto topic_components = discovery_mgr_->discover_topic_components(); + + // Merge both component lists + std::vector all_components; + all_components.reserve(node_components.size() + topic_components.size()); + all_components.insert(all_components.end(), node_components.begin(), node_components.end()); + all_components.insert(all_components.end(), topic_components.begin(), topic_components.end()); + auto timestamp = std::chrono::system_clock::now(); // Capture sizes before move for logging const size_t area_count = areas.size(); - const size_t component_count = components.size(); + const size_t node_component_count = node_components.size(); + const size_t topic_component_count = topic_components.size(); // Lock only for the actual cache update { std::lock_guard lock(cache_mutex_); entity_cache_.areas = std::move(areas); - entity_cache_.components = std::move(components); + entity_cache_.components = std::move(all_components); entity_cache_.last_update = timestamp; } - RCLCPP_DEBUG(get_logger(), "Cache refreshed: %zu areas, %zu components", area_count, component_count); + RCLCPP_DEBUG(get_logger(), "Cache refreshed: %zu areas, %zu components (%zu node-based, %zu topic-based)", + area_count, node_component_count + topic_component_count, node_component_count, topic_component_count); } catch (const std::exception & e) { RCLCPP_ERROR(get_logger(), "Failed to refresh cache: %s", e.what()); } catch (...) { diff --git a/src/ros2_medkit_gateway/src/native_topic_sampler.cpp b/src/ros2_medkit_gateway/src/native_topic_sampler.cpp index 1d9115e..ff02011 100644 --- a/src/ros2_medkit_gateway/src/native_topic_sampler.cpp +++ b/src/ros2_medkit_gateway/src/native_topic_sampler.cpp @@ -501,4 +501,78 @@ ComponentTopics NativeTopicSampler::get_component_topics(const std::string & com return ComponentTopics{}; } +bool NativeTopicSampler::is_system_topic(const std::string & topic_name) { + // System topics to filter out during topic-based discovery + // Note: /tf and /tf_static are NOT filtered (useful for diagnostics) + static const std::vector system_topics = {"/parameter_events", "/rosout", "/clock"}; + + return std::find(system_topics.begin(), system_topics.end(), topic_name) != system_topics.end(); +} + +NativeTopicSampler::TopicDiscoveryResult NativeTopicSampler::discover_topics_by_namespace() { + TopicDiscoveryResult result; + + // Single graph query - avoids N+1 problem + auto all_topics = node_->get_topic_names_and_types(); + + for (const auto & [topic_name, types] : all_topics) { + // Skip system topics + if (is_system_topic(topic_name)) { + continue; + } + + // Extract first segment from topic path + // "/carter1/odom" -> namespace "carter1", ns_prefix "/carter1" + if (topic_name.length() > 1 && topic_name[0] == '/') { + size_t second_slash = topic_name.find('/', 1); + if (second_slash != std::string::npos) { + std::string ns = topic_name.substr(1, second_slash - 1); + if (!ns.empty()) { + // Add namespace to set + result.namespaces.insert(ns); + + // Add topic to the namespace's topic list + std::string ns_prefix = "/" + ns; + result.topics_by_ns[ns_prefix].publishes.push_back(topic_name); + } + } + // else: root topic like "/tf", skip (no namespace) + } + } + + RCLCPP_DEBUG(node_->get_logger(), "Discovered %zu topic namespaces with topics in single query", + result.namespaces.size()); + return result; +} + +std::set NativeTopicSampler::discover_topic_namespaces() { + // Delegate to optimized method (for backward compatibility) + return discover_topics_by_namespace().namespaces; +} + +ComponentTopics NativeTopicSampler::get_topics_for_namespace(const std::string & ns_prefix) { + ComponentTopics topics; + + auto all_topics = node_->get_topic_names_and_types(); + + for (const auto & [topic_name, types] : all_topics) { + // Skip system topics + if (is_system_topic(topic_name)) { + continue; + } + + // Check if topic starts with namespace prefix followed by '/' + // ns_prefix is like "/carter1", topic is like "/carter1/odom" + if (topic_name.length() > ns_prefix.length() && topic_name.find(ns_prefix) == 0 && + topic_name[ns_prefix.length()] == '/') { + // For topic-based discovery, we put all topics in 'publishes' + // since we can't determine direction without node info + topics.publishes.push_back(topic_name); + } + } + + RCLCPP_DEBUG(node_->get_logger(), "Found %zu topics for namespace '%s'", topics.publishes.size(), ns_prefix.c_str()); + return topics; +} + } // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/test/test_discovery_manager.cpp b/src/ros2_medkit_gateway/test/test_discovery_manager.cpp new file mode 100644 index 0000000..38e3275 --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_discovery_manager.cpp @@ -0,0 +1,235 @@ +// Copyright 2025 mfaferek93 +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include "ros2_medkit_gateway/discovery_manager.hpp" +#include "ros2_medkit_gateway/native_topic_sampler.hpp" + +using ros2_medkit_gateway::DiscoveryManager; +using ros2_medkit_gateway::NativeTopicSampler; + +// ============================================================================= +// DiscoveryManager tests +// ============================================================================= + +class DiscoveryManagerTest : public ::testing::Test { + protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + void SetUp() override { + node_ = std::make_shared("test_discovery_node"); + topic_sampler_ = std::make_shared(node_.get()); + discovery_manager_ = std::make_unique(node_.get()); + discovery_manager_->set_topic_sampler(topic_sampler_.get()); + } + + void TearDown() override { + discovery_manager_.reset(); + topic_sampler_.reset(); + node_.reset(); + } + + std::shared_ptr node_; + std::shared_ptr topic_sampler_; + std::unique_ptr discovery_manager_; +}; + +TEST_F(DiscoveryManagerTest, DiscoverTopicComponents_ReturnsEmptyWhenNoTopics) { + // In a clean environment with only system topics, should return empty + auto components = discovery_manager_->discover_topic_components(); + + // System topics are filtered, so we may get empty list + // Each component should have source="topic" if present + for (const auto & comp : components) { + EXPECT_EQ(comp.source, "topic") << "Topic-based component should have source='topic'"; + } +} + +TEST_F(DiscoveryManagerTest, DiscoverTopicComponents_SetsSourceField) { + auto components = discovery_manager_->discover_topic_components(); + + // All topic-based components should have source="topic" + for (const auto & comp : components) { + EXPECT_EQ(comp.source, "topic"); + EXPECT_FALSE(comp.id.empty()); + EXPECT_FALSE(comp.namespace_path.empty()); + EXPECT_FALSE(comp.fqn.empty()); + } +} + +TEST_F(DiscoveryManagerTest, DiscoverComponents_NodeBasedHaveSourceNode) { + auto components = discovery_manager_->discover_components(); + + // Node-based components should have source="node" (default) + for (const auto & comp : components) { + EXPECT_EQ(comp.source, "node") << "Node-based component should have source='node'"; + } +} + +// ============================================================================= +// Integration test with publishers (topic-based discovery) +// ============================================================================= + +class DiscoveryManagerWithPublishersTest : public ::testing::Test { + protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + void SetUp() override { + // Create main node for discovery + node_ = std::make_shared("test_discovery_node"); + topic_sampler_ = std::make_shared(node_.get()); + discovery_manager_ = std::make_unique(node_.get()); + discovery_manager_->set_topic_sampler(topic_sampler_.get()); + + // Create publishers on namespaced topics (simulating Isaac Sim) + // These topics have no associated nodes in those namespaces + pub1_ = node_->create_publisher("/robot_alpha/status", 10); + pub2_ = node_->create_publisher("/robot_alpha/odom", 10); + pub3_ = node_->create_publisher("/robot_beta/status", 10); + + // Allow time for graph discovery + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + rclcpp::spin_some(node_); + } + + void TearDown() override { + pub1_.reset(); + pub2_.reset(); + pub3_.reset(); + discovery_manager_.reset(); + topic_sampler_.reset(); + node_.reset(); + } + + std::shared_ptr node_; + std::shared_ptr topic_sampler_; + std::unique_ptr discovery_manager_; + rclcpp::Publisher::SharedPtr pub1_; + rclcpp::Publisher::SharedPtr pub2_; + rclcpp::Publisher::SharedPtr pub3_; +}; + +TEST_F(DiscoveryManagerWithPublishersTest, DiscoverTopicComponents_FindsNamespacedTopics) { + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + rclcpp::spin_some(node_); + + auto components = discovery_manager_->discover_topic_components(); + + // Should discover robot_alpha and robot_beta namespaces + bool found_alpha = false; + bool found_beta = false; + + for (const auto & comp : components) { + if (comp.id == "robot_alpha") { + found_alpha = true; + EXPECT_EQ(comp.source, "topic"); + EXPECT_EQ(comp.namespace_path, "/robot_alpha"); + EXPECT_EQ(comp.area, "robot_alpha"); + // Should have at least 2 topics + EXPECT_GE(comp.topics.publishes.size(), 2u); + } + if (comp.id == "robot_beta") { + found_beta = true; + EXPECT_EQ(comp.source, "topic"); + EXPECT_EQ(comp.namespace_path, "/robot_beta"); + EXPECT_GE(comp.topics.publishes.size(), 1u); + } + } + + EXPECT_TRUE(found_alpha) << "Should discover robot_alpha from topics"; + EXPECT_TRUE(found_beta) << "Should discover robot_beta from topics"; +} + +TEST_F(DiscoveryManagerWithPublishersTest, DiscoverTopicComponents_ComponentHasCorrectTopics) { + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + rclcpp::spin_some(node_); + + auto components = discovery_manager_->discover_topic_components(); + + for (const auto & comp : components) { + if (comp.id == "robot_alpha") { + bool has_status = false; + bool has_odom = false; + + for (const auto & topic : comp.topics.publishes) { + if (topic == "/robot_alpha/status") { + has_status = true; + } + if (topic == "/robot_alpha/odom") { + has_odom = true; + } + } + + EXPECT_TRUE(has_status) << "robot_alpha should have /robot_alpha/status topic"; + EXPECT_TRUE(has_odom) << "robot_alpha should have /robot_alpha/odom topic"; + } + } +} + +TEST_F(DiscoveryManagerWithPublishersTest, DiscoverAreas_IncludesTopicBasedAreas) { + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + rclcpp::spin_some(node_); + + auto areas = discovery_manager_->discover_areas(); + + bool found_alpha = false; + bool found_beta = false; + + for (const auto & area : areas) { + if (area.id == "robot_alpha") { + found_alpha = true; + } + if (area.id == "robot_beta") { + found_beta = true; + } + } + + EXPECT_TRUE(found_alpha) << "Areas should include robot_alpha from topics"; + EXPECT_TRUE(found_beta) << "Areas should include robot_beta from topics"; +} + +TEST_F(DiscoveryManagerWithPublishersTest, DiscoverTopicComponents_DoesNotDuplicateNodeNamespaces) { + // The discovery manager's own node is in root namespace + // It should not create a topic-based component for root namespace + + auto topic_components = discovery_manager_->discover_topic_components(); + + for (const auto & comp : topic_components) { + // Topic-based components should not be in root namespace + EXPECT_NE(comp.namespace_path, "/") << "Topic-based component should not be in root namespace"; + // Also check it's not duplicating test_discovery_node's namespace + // (which is root "/") + EXPECT_FALSE(comp.id.empty()); + } +} diff --git a/src/ros2_medkit_gateway/test/test_native_topic_sampler.cpp b/src/ros2_medkit_gateway/test/test_native_topic_sampler.cpp new file mode 100644 index 0000000..574f5cf --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_native_topic_sampler.cpp @@ -0,0 +1,272 @@ +// Copyright 2025 mfaferek93 +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "ros2_medkit_gateway/native_topic_sampler.hpp" + +using ros2_medkit_gateway::NativeTopicSampler; + +// ============================================================================= +// is_system_topic tests (static method - no node required) +// ============================================================================= + +class IsSystemTopicTest : public ::testing::Test {}; + +TEST_F(IsSystemTopicTest, FiltersParameterEvents) { + EXPECT_TRUE(NativeTopicSampler::is_system_topic("/parameter_events")); +} + +TEST_F(IsSystemTopicTest, FiltersRosout) { + EXPECT_TRUE(NativeTopicSampler::is_system_topic("/rosout")); +} + +TEST_F(IsSystemTopicTest, FiltersClock) { + EXPECT_TRUE(NativeTopicSampler::is_system_topic("/clock")); +} + +TEST_F(IsSystemTopicTest, DoesNotFilterTf) { + // /tf should NOT be filtered - useful for diagnostics + EXPECT_FALSE(NativeTopicSampler::is_system_topic("/tf")); +} + +TEST_F(IsSystemTopicTest, DoesNotFilterTfStatic) { + // /tf_static should NOT be filtered - useful for diagnostics + EXPECT_FALSE(NativeTopicSampler::is_system_topic("/tf_static")); +} + +TEST_F(IsSystemTopicTest, DoesNotFilterUserTopics) { + EXPECT_FALSE(NativeTopicSampler::is_system_topic("/carter1/odom")); + EXPECT_FALSE(NativeTopicSampler::is_system_topic("/cmd_vel")); + EXPECT_FALSE(NativeTopicSampler::is_system_topic("/chassis/imu")); +} + +TEST_F(IsSystemTopicTest, DoesNotFilterNamespacedSystemTopics) { + // Namespaced versions are NOT system topics (belong to specific component) + EXPECT_FALSE(NativeTopicSampler::is_system_topic("/robot1/parameter_events")); + EXPECT_FALSE(NativeTopicSampler::is_system_topic("/robot1/rosout")); +} + +// ============================================================================= +// NativeTopicSampler tests requiring a ROS 2 node +// ============================================================================= + +class NativeTopicSamplerTest : public ::testing::Test { + protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + void SetUp() override { + node_ = std::make_shared("test_sampler_node"); + sampler_ = std::make_unique(node_.get()); + } + + void TearDown() override { + sampler_.reset(); + node_.reset(); + } + + std::shared_ptr node_; + std::unique_ptr sampler_; +}; + +TEST_F(NativeTopicSamplerTest, DiscoverTopicsByNamespace_ReturnsEmptyForNoTopics) { + // In a clean test environment, we may have system topics but no user topics + // This test verifies the method doesn't crash and returns valid results + auto result = sampler_->discover_topics_by_namespace(); + + // Should return a valid struct (may have system topics filtered out) + // The important thing is that namespaces and topics_by_ns are consistent + for (const auto & ns : result.namespaces) { + std::string ns_prefix = "/" + ns; + // If namespace is in the set, it should have topics in the map + // (unless topics were filtered by other logic) + EXPECT_FALSE(ns.empty()) << "Namespace should not be empty"; + } +} + +TEST_F(NativeTopicSamplerTest, DiscoverTopicsByNamespace_ConsistentNamespacesAndTopics) { + auto result = sampler_->discover_topics_by_namespace(); + + // Every namespace in the set should have a corresponding entry in topics_by_ns + for (const auto & ns : result.namespaces) { + std::string ns_prefix = "/" + ns; + auto it = result.topics_by_ns.find(ns_prefix); + EXPECT_NE(it, result.topics_by_ns.end()) << "Namespace '" << ns << "' should have topics in topics_by_ns"; + + if (it != result.topics_by_ns.end()) { + EXPECT_FALSE(it->second.publishes.empty()) << "Namespace '" << ns << "' should have at least one topic"; + } + } + + // Every entry in topics_by_ns should have its namespace in the namespaces set + for (const auto & [ns_prefix, topics] : result.topics_by_ns) { + // ns_prefix is like "/carter1", we need "carter1" + std::string ns = ns_prefix.substr(1); // Remove leading slash + EXPECT_TRUE(result.namespaces.count(ns) > 0) + << "topics_by_ns key '" << ns_prefix << "' should have its namespace in namespaces set"; + } +} + +TEST_F(NativeTopicSamplerTest, DiscoverTopicNamespaces_BackwardCompatibility) { + // Test that the legacy method still works (delegates to new method) + auto namespaces = sampler_->discover_topic_namespaces(); + + // Compare with new method + auto result = sampler_->discover_topics_by_namespace(); + + EXPECT_EQ(namespaces, result.namespaces) + << "Legacy discover_topic_namespaces should return same result as new method"; +} + +TEST_F(NativeTopicSamplerTest, GetTopicsForNamespace_ReturnsEmptyForNonexistentNamespace) { + auto topics = sampler_->get_topics_for_namespace("/nonexistent_namespace_xyz123"); + + EXPECT_TRUE(topics.publishes.empty()); + EXPECT_TRUE(topics.subscribes.empty()); +} + +TEST_F(NativeTopicSamplerTest, GetTopicsForNamespace_MatchesPrefix) { + // This test verifies the matching logic by checking it doesn't match partial prefixes + auto topics = sampler_->get_topics_for_namespace("/test"); + + // All returned topics should start with "/test/" (with trailing slash) + for (const auto & topic : topics.publishes) { + EXPECT_TRUE(topic.find("/test/") == 0) << "Topic '" << topic << "' should start with '/test/'"; + } +} + +// ============================================================================= +// Integration test with publishers +// ============================================================================= + +class NativeTopicSamplerWithPublishersTest : public ::testing::Test { + protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + void SetUp() override { + // Create main node for sampler + node_ = std::make_shared("test_sampler_node"); + sampler_ = std::make_unique(node_.get()); + + // Create a publisher node in a specific namespace to test discovery + rclcpp::NodeOptions options; + options.arguments({"--ros-args", "-r", "__ns:=/test_robot"}); + publisher_node_ = std::make_shared("publisher_node", options); + + // Create publishers on topics + pub1_ = publisher_node_->create_publisher("/test_robot/status", 10); + pub2_ = publisher_node_->create_publisher("/test_robot/sensor/data", 10); + + // Give ROS 2 graph time to discover the topics + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node_); + } + + void TearDown() override { + pub1_.reset(); + pub2_.reset(); + sampler_.reset(); + publisher_node_.reset(); + node_.reset(); + } + + std::shared_ptr node_; + std::shared_ptr publisher_node_; + std::unique_ptr sampler_; + rclcpp::Publisher::SharedPtr pub1_; + rclcpp::Publisher::SharedPtr pub2_; +}; + +TEST_F(NativeTopicSamplerWithPublishersTest, DiscoverTopicsByNamespace_FindsTestRobotNamespace) { + // Allow more time for graph discovery + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + rclcpp::spin_some(node_); + + auto result = sampler_->discover_topics_by_namespace(); + + // Should discover "test_robot" namespace + EXPECT_TRUE(result.namespaces.count("test_robot") > 0) + << "Should discover 'test_robot' namespace from published topics"; +} + +TEST_F(NativeTopicSamplerWithPublishersTest, DiscoverTopicsByNamespace_FindsTestRobotTopics) { + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + rclcpp::spin_some(node_); + + auto result = sampler_->discover_topics_by_namespace(); + + auto it = result.topics_by_ns.find("/test_robot"); + ASSERT_NE(it, result.topics_by_ns.end()) << "Should have /test_robot in topics_by_ns"; + + // Should find our published topics + const auto & topics = it->second.publishes; + bool found_status = false; + bool found_sensor = false; + + for (const auto & topic : topics) { + if (topic == "/test_robot/status") { + found_status = true; + } + if (topic == "/test_robot/sensor/data") { + found_sensor = true; + } + } + + EXPECT_TRUE(found_status) << "Should find /test_robot/status topic"; + EXPECT_TRUE(found_sensor) << "Should find /test_robot/sensor/data topic"; +} + +TEST_F(NativeTopicSamplerWithPublishersTest, GetTopicsForNamespace_ReturnsCorrectTopics) { + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + rclcpp::spin_some(node_); + + auto topics = sampler_->get_topics_for_namespace("/test_robot"); + + EXPECT_GE(topics.publishes.size(), 2u) << "Should find at least 2 topics for /test_robot"; + + bool found_status = false; + bool found_sensor = false; + + for (const auto & topic : topics.publishes) { + if (topic == "/test_robot/status") { + found_status = true; + } + if (topic == "/test_robot/sensor/data") { + found_sensor = true; + } + } + + EXPECT_TRUE(found_status) << "Should find /test_robot/status topic"; + EXPECT_TRUE(found_sensor) << "Should find /test_robot/sensor/data topic"; +}