Merge pull request #6 from uulm-mrm/feat_node_namespaces

Allow nodes to be in namespaces

ros2/orchestrator/orchestrator/orchestrator_lib/model_loader.py

@@ -85,7 +85,9 @@ def load_launch_config(package: str, name: str, schema: JsonSchema) -> LaunchCon
 
 def load_models(launch_config: dict[str, Any], node_config_schema: JsonSchema) -> List[NodeModel]:
     models: list[NodeModel] = []
-    for name, node in launch_config["nodes"].items():
+    nodes_with_priorities = [(node.get('priority', 0), k, node) for k, node in launch_config["nodes"].items()]
+    nodes_with_priorities = sorted(nodes_with_priorities, key=lambda x: x[:2], reverse=True)
+    for _, name, node in nodes_with_priorities:
         remappings: Dict[str, str] = node.get("remappings", {})
         if isinstance(node["config_file"], str):
             path = node["config_file"]

ros2/orchestrator/orchestrator/orchestrator_lib/name_utils.py

@@ -22,6 +22,8 @@ def remove_prefix(text: str, prefix: str) -> str:
 
 
 def intercepted_name(node_name: str, topic_name: str) -> str:
+    if node_name.startswith("/"):
+        node_name = node_name[1:]
     if topic_name.startswith("/"):
         topic_name = topic_name[1:]
     return f"/intercepted/{node_name}/sub/{topic_name}"

ros2/orchestrator/orchestrator/orchestrator_lib/ros_utils/pubsub.py

@@ -37,7 +37,7 @@ def wait_for_node_sub(topic_name: str, node_name: str, logger: RcutilsLogger, no
 
     def try_get_type() -> Optional[Type[Any]]:
         for info in node.get_subscriptions_info_by_topic(topic_name):
-            if info.node_name == node_name:
+            if f"{info.node_namespace}/{info.node_name}" == node_name:
                 return type_from_string(info.topic_type)
         return None
 
@@ -59,9 +59,11 @@ def try_get_type() -> Optional[Type[Any]]:
 
 def wait_for_node_pub(topic_name: str, node_name: str, logger: RcutilsLogger, node: Node, executor: Executor) -> None:
     topic_name = node.resolve_topic_name(topic_name, only_expand=True)
+    splt = node_name.split('/')
+    node_name, node_namespace = splt[-1], '/'.join(splt[:-1])
 
     def node_has_pub():
-        by_node = node.get_publisher_names_and_types_by_node(node_name, node.get_namespace())
+        by_node = node.get_publisher_names_and_types_by_node(node_name, node_namespace)
         for topic, _types in by_node:
             if topic == topic_name:
                 return True

ros2/orchestrator/orchestrator/util/node_wrapper.py

@@ -19,7 +19,7 @@ def __init__(self, node: Node, topics: Union[List[str], None] = None):
 
     def publish_status(self) -> None:
         status_msg = Status()
-        status_msg.node_name = self.node.get_name()
+        status_msg.node_name = self.node.get_fully_qualified_name()
         self.orchestrator_status_pub.publish(status_msg)
 
     def create_subscription(self, topic_type: type, topic: str, callback: Callable[[Any], None], *args: Any,

ros2/orchestrator/schemas/node_config_schema.json

@@ -8,6 +8,9 @@
         "name": {
             "type": "string"
         },
+        "priority": {
+            "type": "number"
+        },
         "callbacks": {
             "title": "List of callback specifications",
             "type": "array",
@@ -150,4 +153,4 @@
             "additionalProperties": false
         }
     }
-}
+}

ros2/orchestrator_helper/include/orchestrator_helper/orchestrator_helper.hpp

@@ -116,7 +116,7 @@ class OrchestratorHelper
   {
     static int debug_id_ = 0;
     StatusMsg status_msg;
-    status_msg.node_name = node_->get_name();
+    status_msg.node_name = std::string(node_->get_namespace()) + "/" + node_->get_name();
     status_msg.debug_id = debug_id >= 0 ? debug_id : debug_id_++;
     status_msg.omitted_outputs = omitted_outputs;
     status_pub_->publish(std::move(status_msg));