65 lines
2.0 KiB
Python
65 lines
2.0 KiB
Python
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
|