Merge pull request clearpathrobotics#63 from clearpathrobotics/discovery-server

Discovery server support

Author: luis-camero

clearpath_generator_common/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(ament_cmake_python REQUIRED)
 
 install(PROGRAMS
   ${PROJECT_NAME}/description/generate_description
+  ${PROJECT_NAME}/discovery_server/generate_discovery_server
   ${PROJECT_NAME}/bash/generate_bash
   DESTINATION lib/${PROJECT_NAME}
 )

clearpath_generator_common/clearpath_generator_common/bash/generator.py

@@ -32,36 +32,60 @@
 # modification, is not permitted without the express permission
 # of Clearpath Robotics.
 
+from clearpath_config.common.types.discovery import Discovery
 from clearpath_generator_common.bash.writer import BashWriter
 from clearpath_generator_common.common import BashFile, BaseGenerator
 
 
 class BashGenerator(BaseGenerator):
 
+    ROS_DISTRO_PATH = '/opt/ros/humble/'
+
     def generate(self) -> None:
+        # Generate setup.bash
         self.generate_setup_bash()
 
     def generate_setup_bash(self) -> None:
-        clearpath_setup_bash = BashFile(name='setup', path=self.setup_path)
+        clearpath_setup_bash = BashFile(filename='setup.bash', path=self.setup_path)
         bash_writer = BashWriter(clearpath_setup_bash)
 
         workspaces = self.clearpath_config.system.workspaces
 
         # Source Humble
-        humble_setup_bash = BashFile(name='setup', path='/opt/ros/humble/')
+        humble_setup_bash = BashFile(filename='setup.bash', path=self.ROS_DISTRO_PATH)
         bash_writer.add_source(humble_setup_bash)
 
         # Additional workspaces
         for workspace in workspaces:
             bash_writer.add_source(
-                BashFile(name='setup', path=workspace.strip('setup.bash')))
+                BashFile(filename='setup.bash', path=workspace.strip('setup.bash')))
 
         # ROS_DOMAIN_ID
         domain_id = self.clearpath_config.system.domain_id
         bash_writer.add_export('ROS_DOMAIN_ID', domain_id)
 
         # RMW_IMPLEMENTATION
-        rmw = self.clearpath_config.system.rmw_implementation
+        rmw = self.clearpath_config.system.middleware.rmw_implementation
         bash_writer.add_export('RMW_IMPLEMENTATION', rmw)
 
+        # Custom DDS Profile
+        if self.clearpath_config.system.middleware.profile:
+            bash_writer.add_export(
+                'FASTRTPS_DEFAULT_PROFILES_FILE', self.clearpath_config.system.middleware.profile)
+
+        # Fast DDS Discovery Server
+        if self.clearpath_config.system.middleware.discovery == Discovery.SERVER:
+            server_str = self.clearpath_config.system.middleware.get_servers_string()
+            bash_writer.add_export('ROS_DISCOVERY_SERVER', f'"{server_str}"')
+
+            # If this is not being called by systemd then set as super user (for ROS 2 cli tools)
+            bash_writer.write('if [ -t 0 ]; then')
+            bash_writer.add_export('ROS_SUPER_CLIENT', True, indent_level=1)
+            bash_writer.write('else')
+            bash_writer.add_unset('ROS_SUPER_CLIENT', indent_level=1)
+            bash_writer.write('fi')
+
+        else:
+            bash_writer.add_unset('ROS_DISCOVERY_SERVER')
+
         bash_writer.close()

clearpath_generator_common/clearpath_generator_common/bash/writer.py

@@ -41,16 +41,19 @@ def __init__(self, bash_file: BashFile):
         self.file = open(self.bash_file.full_path, 'w')
 
     def write(self, string, indent_level=0):
-        self.file.write('{0}{1}\n'.format(self.tab * indent_level, string))
+        self.file.write(f'{self.tab * indent_level}{string}\n')
 
-    def add_export(self, envar: str, value):
-        self.write('export {0}={1}'.format(envar, value))
+    def add_export(self, envar: str, value, indent_level=0):
+        self.write(f'{self.tab * indent_level}export {envar}={value}')
 
-    def add_unset(self, envar: str):
-        self.write('unset {0}'.format(envar))
+    def add_unset(self, envar: str, indent_level=0):
+        self.write(f'{self.tab * indent_level}unset {envar}')
 
-    def add_source(self, bash_file: BashFile):
-        self.write('source {0}'.format(bash_file.full_path))
+    def add_source(self, bash_file: BashFile, indent_level=0):
+        self.write(f'{self.tab * indent_level}source {bash_file.full_path}')
+
+    def add_echo(self, msg: str, indent_level=0):
+        self.write(f'{self.tab * indent_level}echo "{msg}"')
 
     def close(self):
         self.file.close()

clearpath_generator_common/clearpath_generator_common/common.py

@@ -194,14 +194,13 @@ def update(self, parameters: dict) -> None:
 
 class BashFile():
     def __init__(self,
-                 name: str,
+                 filename: str,
                  path: str,
                  package: Package = None,
                  ) -> None:
         self.package = package
         self.path = path
-        self.name = name
-        self.file = '{0}.bash'.format(name)
+        self.file = filename
 
     @property
     def full_path(self):

clearpath_generator_common/clearpath_generator_common/discovery_server/generate_discovery_server

@@ -0,0 +1,46 @@
+#!/usr/bin/env python3
+
+# Software License Agreement (BSD)
+#
+# @author    Hilary Luo <[email protected]>
+# @copyright (c) 2024, Clearpath Robotics, Inc., All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+# * Neither the name of Clearpath Robotics nor the names of its contributors
+#   may be used to endorse or promote products derived from this software
+#   without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, is not permitted without the express permission
+# of Clearpath Robotics.
+
+from clearpath_generator_common.common import BaseGenerator
+from clearpath_generator_common.discovery_server.generator import DiscoveryServerGenerator
+
+
+def main():
+    setup_path = BaseGenerator.get_args()
+    dsg = DiscoveryServerGenerator(setup_path)
+    dsg.generate()
+
+
+if __name__ == '__main__':
+    main()

clearpath_generator_common/clearpath_generator_common/discovery_server/generator.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+
+# Software License Agreement (BSD)
+#
+# @author    Hilary Luo <[email protected]>
+# @copyright (c) 2024, Clearpath Robotics, Inc., All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+# * Neither the name of Clearpath Robotics nor the names of its contributors
+#   may be used to endorse or promote products derived from this software
+#   without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, is not permitted without the express permission
+# of Clearpath Robotics.
+
+from clearpath_config.common.types.discovery import Discovery
+from clearpath_config.common.types.rmw_implementation import RMWImplementation
+from clearpath_generator_common.bash.writer import BashWriter
+from clearpath_generator_common.common import BaseGenerator, BashFile
+
+
+class DiscoveryServerGenerator(BaseGenerator):
+
+    ROS_DISTRO_PATH = '/opt/ros/humble/'
+
+    def generate(self) -> None:
+        # Generate the file that launches the FastDDS discovery server
+        self.generate_server_start()
+
+    def generate_server_start(self) -> None:
+        # Generate script that launches the discovery server
+        discovery_server_start = BashFile(filename='discovery-server-start', path=self.setup_path)
+        bash_writer = BashWriter(discovery_server_start)
+
+        # Source Humble
+        humble_setup_bash = BashFile(filename='setup.bash', path=self.ROS_DISTRO_PATH)
+        bash_writer.add_source(humble_setup_bash)
+
+        # If Fast DDS Discovery Server is selected then check if a local server should be run
+        middleware_config = self.clearpath_config.system.middleware
+        if ((middleware_config.rmw_implementation == RMWImplementation.FAST_RTPS) and
+                (middleware_config.discovery == Discovery.SERVER)):
+
+            # For Debug:
+            # for i, s in enumerate(self.clearpath_config.system.middleware.servers.get_all()):
+            #     print(f"Server {i} is {str(s)}")
+            # print(f"Localhost is {self.clearpath_config.system.localhost}")
+
+            ls = middleware_config.get_local_server()
+            if ls and ls.enabled:
+                bash_writer.write(
+                    f'fastdds discovery -i {ls.server_id} -l 127.0.0.1 -p {ls.port}'
+                )
+            else:
+                bash_writer.add_echo(
+                    'No local discovery server enabled. ' +
+                    'If this was launched as a service then the service will now end.'
+                )
+        else:
+            bash_writer.add_echo(
+                'Discovery server not enabled. ' +
+                'If this was launched as a service then the service will now end.'
+            )
+
+        bash_writer.close()