Skip to content

Commit df20ddd

Browse files
committed
feat(#94): add topic-based component discovery
Add hybrid discovery that creates virtual components from topic namespaces when ROS 2 nodes are not available (e.g., Isaac Sim publishing topics without creating discoverable nodes). Changes: - Add field to Component struct (node or topic) - Add discover_topic_namespaces() to extract namespaces from topics - Add get_topics_for_namespace() to get topics under a namespace - Add is_system_topic() to filter /parameter_events, /rosout, /clock - Add discover_topic_components() to create virtual components - Update discover_areas() to include topic-based namespaces - Update refresh_cache() to merge node-based and topic-based components
1 parent 9e180d4 commit df20ddd

File tree

7 files changed

+224
-6
lines changed

7 files changed

+224
-6
lines changed

.devcontainer/devcontainer.json

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
{
22
"name": "ROS2 ros2_medkit Development",
3+
"runArgs": ["--network=host"],
34
"build": {
45
"dockerfile": "Dockerfile",
56
"context": "..",

src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery_manager.hpp

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#include <memory>
1919
#include <optional>
2020
#include <rclcpp/rclcpp.hpp>
21+
#include <set>
2122
#include <string>
2223
#include <vector>
2324

@@ -34,6 +35,25 @@ class DiscoveryManager {
3435
std::vector<Area> discover_areas();
3536
std::vector<Component> discover_components();
3637

38+
/**
39+
* @brief Discover components from topic namespaces (topic-based discovery)
40+
*
41+
* Creates "virtual" components for topic namespaces that don't have
42+
* corresponding ROS 2 nodes. This is useful for systems like Isaac Sim
43+
* that publish topics without creating proper ROS 2 nodes.
44+
*
45+
* Example: Topics ["/carter1/odom", "/carter1/cmd_vel", "/carter2/odom"]
46+
* Creates components: carter1, carter2 (if no matching nodes exist)
47+
*
48+
* Components are created with:
49+
* - id: namespace name (e.g., "carter1")
50+
* - source: "topic" (to distinguish from node-based components)
51+
* - topics.publishes: all topics under this namespace
52+
*
53+
* @return Vector of topic-based components (excludes namespaces with existing nodes)
54+
*/
55+
std::vector<Component> discover_topic_components();
56+
3757
/// Discover all services in the system with their types
3858
std::vector<ServiceInfo> discover_services();
3959

@@ -87,6 +107,9 @@ class DiscoveryManager {
87107
/// Extract the last segment from a path (e.g., "/a/b/c" -> "c")
88108
std::string extract_name_from_path(const std::string & path);
89109

110+
/// Get set of namespaces that have ROS 2 nodes (for deduplication)
111+
std::set<std::string> get_node_namespaces();
112+
90113
/// Check if a service path belongs to a component namespace
91114
bool path_belongs_to_namespace(const std::string & path, const std::string & ns) const;
92115

src/ros2_medkit_gateway/include/ros2_medkit_gateway/models.hpp

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -147,13 +147,14 @@ struct Component {
147147
std::string fqn;
148148
std::string type = "Component";
149149
std::string area;
150+
std::string source = "node"; ///< Discovery source: "node" or "topic"
150151
std::vector<ServiceInfo> services;
151152
std::vector<ActionInfo> actions;
152153
ComponentTopics topics; ///< Topics this component publishes/subscribes
153154

154155
json to_json() const {
155-
json j = {{"id", id}, {"namespace", namespace_path}, {"fqn", fqn}, {"type", type},
156-
{"area", area}, {"topics", topics.to_json()}};
156+
json j = {{"id", id}, {"namespace", namespace_path}, {"fqn", fqn}, {"type", type}, {"area", area},
157+
{"source", source}, {"topics", topics.to_json()}};
157158

158159
// Add operations array combining services and actions
159160
json operations = json::array();

src/ros2_medkit_gateway/include/ros2_medkit_gateway/native_topic_sampler.hpp

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include <nlohmann/json.hpp>
2121
#include <optional>
2222
#include <rclcpp/rclcpp.hpp>
23+
#include <set>
2324
#include <string>
2425
#include <utility>
2526
#include <vector>
@@ -199,6 +200,49 @@ class NativeTopicSampler {
199200
*/
200201
ComponentTopics get_component_topics(const std::string & component_fqn);
201202

203+
/**
204+
* @brief Discover unique namespace prefixes from all topics
205+
*
206+
* Extracts the first segment of each topic path to identify namespaces.
207+
* Used for topic-based component discovery when nodes are not available
208+
* (e.g., Isaac Sim publishing topics without creating ROS 2 nodes).
209+
*
210+
* Example: Topics ["/carter1/odom", "/carter2/cmd_vel", "/tf"]
211+
* Returns: {"carter1", "carter2"} (root topics like /tf are excluded)
212+
*
213+
* @return Set of unique namespace prefixes (without leading slash)
214+
*/
215+
std::set<std::string> discover_topic_namespaces();
216+
217+
/**
218+
* @brief Get all topics under a specific namespace prefix
219+
*
220+
* Returns ComponentTopics containing all topics that start with the given
221+
* namespace prefix. For topic-based discovery, all matched topics are
222+
* placed in the 'publishes' list since direction cannot be determined
223+
* without node information.
224+
*
225+
* @param ns_prefix Namespace prefix including leading slash (e.g., "/carter1")
226+
* @return ComponentTopics with matching topics in publishes list
227+
*/
228+
ComponentTopics get_topics_for_namespace(const std::string & ns_prefix);
229+
230+
/**
231+
* @brief Check if a topic is a ROS 2 system/infrastructure topic
232+
*
233+
* System topics are filtered out during topic-based discovery to avoid
234+
* creating spurious components. Filtered topics include:
235+
* - /parameter_events
236+
* - /rosout
237+
* - /clock
238+
*
239+
* Note: /tf and /tf_static are NOT filtered (useful for diagnostics).
240+
*
241+
* @param topic_name Full topic path
242+
* @return true if this is a system topic that should be filtered
243+
*/
244+
static bool is_system_topic(const std::string & topic_name);
245+
202246
private:
203247
/**
204248
* @brief Parse YAML-formatted message string to JSON

src/ros2_medkit_gateway/src/discovery_manager.cpp

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,14 @@ std::vector<Area> DiscoveryManager::discover_areas() {
5151
area_set.insert(area);
5252
}
5353

54+
// Also include areas from topic namespaces (for topic-based discovery)
55+
if (topic_sampler_) {
56+
auto topic_namespaces = topic_sampler_->discover_topic_namespaces();
57+
for (const auto & ns : topic_namespaces) {
58+
area_set.insert(ns);
59+
}
60+
}
61+
5462
// Convert set to vector of Area structs
5563
std::vector<Area> areas;
5664
for (const auto & area_name : area_set) {
@@ -360,6 +368,67 @@ std::string DiscoveryManager::extract_name_from_path(const std::string & path) {
360368
return path;
361369
}
362370

371+
std::set<std::string> DiscoveryManager::get_node_namespaces() {
372+
std::set<std::string> namespaces;
373+
374+
auto node_graph = node_->get_node_graph_interface();
375+
auto names_and_namespaces = node_graph->get_node_names_and_namespaces();
376+
377+
for (const auto & name_and_ns : names_and_namespaces) {
378+
std::string ns = name_and_ns.second;
379+
std::string area = extract_area_from_namespace(ns);
380+
if (area != "root") {
381+
namespaces.insert(area);
382+
}
383+
}
384+
385+
return namespaces;
386+
}
387+
388+
std::vector<Component> DiscoveryManager::discover_topic_components() {
389+
std::vector<Component> components;
390+
391+
if (!topic_sampler_) {
392+
RCLCPP_DEBUG(node_->get_logger(), "Topic sampler not set, skipping topic-based discovery");
393+
return components;
394+
}
395+
396+
// Get namespaces from topics
397+
auto topic_namespaces = topic_sampler_->discover_topic_namespaces();
398+
399+
// Get namespaces that already have nodes (to avoid duplicates)
400+
auto node_namespaces = get_node_namespaces();
401+
402+
RCLCPP_DEBUG(node_->get_logger(), "Topic-based discovery: %zu topic namespaces, %zu node namespaces",
403+
topic_namespaces.size(), node_namespaces.size());
404+
405+
for (const auto & ns : topic_namespaces) {
406+
// Skip if there's already a node with this namespace
407+
if (node_namespaces.count(ns) > 0) {
408+
RCLCPP_DEBUG(node_->get_logger(), "Skipping namespace '%s' - already has nodes", ns.c_str());
409+
continue;
410+
}
411+
412+
Component comp;
413+
comp.id = ns;
414+
comp.namespace_path = "/" + ns;
415+
comp.fqn = "/" + ns;
416+
comp.area = ns;
417+
comp.source = "topic";
418+
419+
// Get topics for this namespace
420+
comp.topics = topic_sampler_->get_topics_for_namespace("/" + ns);
421+
422+
RCLCPP_DEBUG(node_->get_logger(), "Created topic-based component '%s' with %zu topics", ns.c_str(),
423+
comp.topics.publishes.size());
424+
425+
components.push_back(comp);
426+
}
427+
428+
RCLCPP_INFO(node_->get_logger(), "Discovered %zu topic-based components", components.size());
429+
return components;
430+
}
431+
363432
bool DiscoveryManager::path_belongs_to_namespace(const std::string & path, const std::string & ns) const {
364433
if (ns.empty() || ns == "/") {
365434
// Root namespace - check if path has only one segment after leading slash

src/ros2_medkit_gateway/src/gateway_node.cpp

Lines changed: 19 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -171,22 +171,37 @@ void GatewayNode::refresh_cache() {
171171

172172
// Discover data outside the lock to minimize lock time
173173
auto areas = discovery_mgr_->discover_areas();
174-
auto components = discovery_mgr_->discover_components();
174+
175+
// Discover node-based components (standard ROS 2 nodes)
176+
auto node_components = discovery_mgr_->discover_components();
177+
178+
// Discover topic-based components (for systems like Isaac Sim that
179+
// publish topics without creating proper ROS 2 nodes)
180+
auto topic_components = discovery_mgr_->discover_topic_components();
181+
182+
// Merge both component lists
183+
std::vector<Component> all_components;
184+
all_components.reserve(node_components.size() + topic_components.size());
185+
all_components.insert(all_components.end(), node_components.begin(), node_components.end());
186+
all_components.insert(all_components.end(), topic_components.begin(), topic_components.end());
187+
175188
auto timestamp = std::chrono::system_clock::now();
176189

177190
// Capture sizes before move for logging
178191
const size_t area_count = areas.size();
179-
const size_t component_count = components.size();
192+
const size_t node_component_count = node_components.size();
193+
const size_t topic_component_count = topic_components.size();
180194

181195
// Lock only for the actual cache update
182196
{
183197
std::lock_guard<std::mutex> lock(cache_mutex_);
184198
entity_cache_.areas = std::move(areas);
185-
entity_cache_.components = std::move(components);
199+
entity_cache_.components = std::move(all_components);
186200
entity_cache_.last_update = timestamp;
187201
}
188202

189-
RCLCPP_DEBUG(get_logger(), "Cache refreshed: %zu areas, %zu components", area_count, component_count);
203+
RCLCPP_DEBUG(get_logger(), "Cache refreshed: %zu areas, %zu components (%zu node-based, %zu topic-based)",
204+
area_count, node_component_count + topic_component_count, node_component_count, topic_component_count);
190205
} catch (const std::exception & e) {
191206
RCLCPP_ERROR(get_logger(), "Failed to refresh cache: %s", e.what());
192207
} catch (...) {

src/ros2_medkit_gateway/src/native_topic_sampler.cpp

Lines changed: 65 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -501,4 +501,69 @@ ComponentTopics NativeTopicSampler::get_component_topics(const std::string & com
501501
return ComponentTopics{};
502502
}
503503

504+
bool NativeTopicSampler::is_system_topic(const std::string & topic_name) {
505+
// System topics to filter out during topic-based discovery
506+
// Note: /tf and /tf_static are NOT filtered (useful for diagnostics)
507+
static const std::vector<std::string> system_topics = {"/parameter_events", "/rosout", "/clock"};
508+
509+
return std::find(system_topics.begin(), system_topics.end(), topic_name) != system_topics.end();
510+
}
511+
512+
std::set<std::string> NativeTopicSampler::discover_topic_namespaces() {
513+
std::set<std::string> namespaces;
514+
515+
auto all_topics = node_->get_topic_names_and_types();
516+
517+
for (const auto & [topic_name, types] : all_topics) {
518+
// Skip system topics
519+
if (is_system_topic(topic_name)) {
520+
continue;
521+
}
522+
523+
// Extract first segment from topic path
524+
// "/carter1/odom" -> "carter1"
525+
// "/tf" -> "" (root topic, skip)
526+
if (topic_name.length() > 1 && topic_name[0] == '/') {
527+
// Find the second slash
528+
size_t second_slash = topic_name.find('/', 1);
529+
if (second_slash != std::string::npos) {
530+
// Has namespace: "/carter1/odom" -> "carter1"
531+
std::string ns = topic_name.substr(1, second_slash - 1);
532+
if (!ns.empty()) {
533+
namespaces.insert(ns);
534+
}
535+
}
536+
// else: root topic like "/tf", skip (no namespace)
537+
}
538+
}
539+
540+
RCLCPP_DEBUG(node_->get_logger(), "Discovered %zu topic namespaces", namespaces.size());
541+
return namespaces;
542+
}
543+
544+
ComponentTopics NativeTopicSampler::get_topics_for_namespace(const std::string & ns_prefix) {
545+
ComponentTopics topics;
546+
547+
auto all_topics = node_->get_topic_names_and_types();
548+
549+
for (const auto & [topic_name, types] : all_topics) {
550+
// Skip system topics
551+
if (is_system_topic(topic_name)) {
552+
continue;
553+
}
554+
555+
// Check if topic starts with namespace prefix followed by '/'
556+
// ns_prefix is like "/carter1", topic is like "/carter1/odom"
557+
if (topic_name.length() > ns_prefix.length() && topic_name.find(ns_prefix) == 0 &&
558+
topic_name[ns_prefix.length()] == '/') {
559+
// For topic-based discovery, we put all topics in 'publishes'
560+
// since we can't determine direction without node info
561+
topics.publishes.push_back(topic_name);
562+
}
563+
}
564+
565+
RCLCPP_DEBUG(node_->get_logger(), "Found %zu topics for namespace '%s'", topics.publishes.size(), ns_prefix.c_str());
566+
return topics;
567+
}
568+
504569
} // namespace ros2_medkit_gateway

0 commit comments

Comments
 (0)