Move from other project.

launch/zenoh_router.launch.py

@@ -0,0 +1,64 @@
+import launch
+import launch_ros.actions
+import launch.substitutions
+import launch.substitution
+from launch.some_substitutions_type import SomeSubstitutionsType
+import launch.conditions
+
+import socket
+
+def generate_launch_description():
+    nodes = []
+    nodes.append(make_router())
+    return launch.LaunchDescription(nodes)
+
+def setup_router_env():
+    # In theory you can use this to set up the environment on the router
+    # in a unique way.
+    # I tried something like wat you see below at one point, though
+    # it didn't pan out.
+    # I'm just goint to leave it as an example here.
+    #zenoh_runtime_settings = "( rx: (worker_threads: 2), acc: (worker_threads: 2), tx: (worker_threads: 2), app: (worker_threads: 2) )"
+    #env = {"ZENOH_RUNTIME": zenoh_runtime_settings}
+    env = None
+    return env
+
+def make_router():
+    zenoh_is_rmw = launch.substitutions.EqualsSubstitution(
+                    'rmw_zenoh_cpp',
+                    launch.substitutions.EnvironmentVariable('RMW_IMPLEMENTATION', default_value='')
+    )
+    router_port_is_free = PortIsFree('7447')
+
+    condition = launch.conditions.IfCondition(
+        launch.substitutions.AndSubstitution(zenoh_is_rmw, router_port_is_free)
+    )
+    node = launch_ros.actions.Node(
+        package = "rmw_zenoh_cpp",
+        executable="rmw_zenohd",
+        name="zenoh_router",
+        condition=condition,
+        additional_env=setup_router_env()
+    )
+    return node
+
+######
+# Custom actions/substitutions
+######
+
+class PortIsFree(launch.substitution.Substitution):
+    def __init__(self, port: SomeSubstitutionsType):
+        super().__init__()
+        self._port = port
+
+    def perform(self, context):
+        port_as_string = self._port if isinstance(self._port, str) else self._port.perform(context)
+        port_as_int = int(port_as_string)
+
+        if is_port_in_use_for_tcp(port_as_int):
+            return "False"
+        return "True"
+
+def is_port_in_use_for_tcp(port):
+    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
+        return sock.connect_ex(('localhost', port)) == 0