Add zenoh service files & generators

clearpath_diagnostics/clearpath_diagnostics/diagnostics_updater

@@ -45,6 +45,7 @@ from rclpy.qos import qos_profile_sensor_data
 from sensor_msgs.msg import BatteryState, Image, Imu, LaserScan, MagneticField, NavSatFix, PointCloud2
 
 from clearpath_generator_common.common import BaseGenerator
+from clearpath_generator_common.ros import ROS_DISTRO
 from clearpath_config.common.utils.yaml import read_yaml
 from clearpath_config.clearpath_config import ClearpathConfig
 from clearpath_config.sensors.types.sensor import BaseSensor
@@ -100,12 +101,12 @@ class ClearpathDiagnosticUpdater(Node):
         # Check current firmware version
         cache = Cache()
         try:
-            pkg = cache['ros-humble-clearpath-firmware']
+            pkg = cache[f'ros-{ROS_DISTRO}-clearpath-firmware']
             versions = pkg.versions
             newest: Version = versions[0]
             self.newest_firmware_version = newest.version.split('-')[0]
         except KeyError:
-            self.get_logger().warn('ros-humble-clearpath-firmware package not found')
+            self.get_logger().warn(f'ros-{ROS_DISTRO}-clearpath-firmware package not found')
             self.newest_firmware_version = 'Not found'
 
         self.firmware_version = '0.0.0'
@@ -124,13 +125,13 @@ class ClearpathDiagnosticUpdater(Node):
 
     def check_firmware_version(self, stat: DiagnosticStatusWrapper):
         if self.newest_firmware_version == 'Not found':
-            stat.summary(DiagnosticStatus.ERROR, 'ros-humble-clearpath-firmware package not found')
+            stat.summary(DiagnosticStatus.ERROR, f'ros-{ROS_DISTRO}-clearpath-firmware package not found')
         elif self.firmware_version == self.newest_firmware_version:
             stat.summary(DiagnosticStatus.OK, f'Firmware is up to date (v{self.firmware_version})')
         elif self.firmware_version < self.newest_firmware_version:
             stat.summary(DiagnosticStatus.WARN, f'New firmware available. (v{self.firmware_version}) -> (v{self.newest_firmware_version})')
         else:
-            stat.summary(DiagnosticStatus.WARN, 'ros-humble-clearpath-firmware package is outdated.')
+            stat.summary(DiagnosticStatus.WARN, f'ros-{ROS_DISTRO}-clearpath-firmware package is outdated.')
         return stat
 
     def get_headerless_topic_diagnostic(self,

clearpath_robot/scripts/generate

@@ -37,6 +37,9 @@ source /etc/clearpath/setup.bash
 # Generate discovery server start file
 ros2 run clearpath_generator_common generate_discovery_server
 
+# Generate Zenoh router start file
+ros2 run clearpath_generator_common generate_zenoh_router
+
 # Generate vcan bridge start file
 ros2 run clearpath_generator_common generate_vcan
 

clearpath_robot/scripts/install

@@ -38,6 +38,8 @@ from ament_index_python.packages import get_package_share_directory
 
 import os
 
+import robot_upstart.providers
+
 
 class VirtualCANProvider(robot_upstart.providers.Generic):
     def post_install(self):
@@ -55,16 +57,16 @@ class VirtualCANProvider(robot_upstart.providers.Generic):
         with open(vcan_bridge_path) as f:
             vcan_bridge_contents = f.read()
         return {
-            "/lib/systemd/system/clearpath-vcan.service": {
-                "content": vcan_service_contents,
-                "mode": 0o644
+            '/lib/systemd/system/clearpath-vcan.service': {
+                'content': vcan_service_contents,
+                'mode': 0o644
             },
-            "/usr/sbin/clearpath-vcan-bridge": {
-                "content": vcan_bridge_contents,
-                "mode": 0o755
+            '/usr/sbin/clearpath-vcan-bridge': {
+                'content': vcan_bridge_contents,
+                'mode': 0o755
             },
-            "/etc/systemd/system/clearpath-robot.service.wants/clearpath-vcan.service": {
-                "symlink": "/lib/systemd/system/clearpath-vcan.service"
+            '/etc/systemd/system/clearpath-robot.service.wants/clearpath-vcan.service': {
+                'symlink': '/lib/systemd/system/clearpath-vcan.service'
             },
         }
 
@@ -86,12 +88,12 @@ class PlatformProvider(robot_upstart.providers.Generic):
                     line = 'User={0}\n'.format(clearpath_config.system.username)
                 platform_service_contents += line
         return {
-            "/lib/systemd/system/clearpath-platform.service": {
-                "content": platform_service_contents,
-                "mode": 0o644
+            '/lib/systemd/system/clearpath-platform.service': {
+                'content': platform_service_contents,
+                'mode': 0o644
             },
-            "/etc/systemd/system/clearpath-robot.service.wants/clearpath-platform.service": {
-                "symlink": "/lib/systemd/system/clearpath-platform.service"
+            '/etc/systemd/system/clearpath-robot.service.wants/clearpath-platform.service': {
+                'symlink': '/lib/systemd/system/clearpath-platform.service'
             }
         }
 
@@ -113,12 +115,12 @@ class SensorsProvider(robot_upstart.providers.Generic):
                     line = 'User={0}\n'.format(clearpath_config.system.username)
                 sensors_service_contents += line
         return {
-            "/lib/systemd/system/clearpath-sensors.service": {
-                "content": sensors_service_contents,
-                "mode": 0o644
+            '/lib/systemd/system/clearpath-sensors.service': {
+                'content': sensors_service_contents,
+                'mode': 0o644
             },
-            "/etc/systemd/system/clearpath-robot.service.wants/clearpath-sensors.service": {
-                "symlink": "/lib/systemd/system/clearpath-sensors.service"
+            '/etc/systemd/system/clearpath-robot.service.wants/clearpath-sensors.service': {
+                'symlink': '/lib/systemd/system/clearpath-sensors.service'
             }
         }
 
@@ -140,12 +142,12 @@ class ManipulatorsProvider(robot_upstart.providers.Generic):
                     line = 'User={0}\n'.format(clearpath_config.system.username)
                 manipulators_service_contents += line
         return {
-            "/lib/systemd/system/clearpath-manipulators.service": {
-                "content": manipulators_service_contents,
-                "mode": 0o644
+            '/lib/systemd/system/clearpath-manipulators.service': {
+                'content': manipulators_service_contents,
+                'mode': 0o644
             },
-            "/etc/systemd/system/clearpath-robot.service.wants/clearpath-manipulators.service": {
-                "symlink": "/lib/systemd/system/clearpath-manipulators.service"
+            '/etc/systemd/system/clearpath-robot.service.wants/clearpath-manipulators.service': {
+                'symlink': '/lib/systemd/system/clearpath-manipulators.service'
             }
         }
 
@@ -168,16 +170,44 @@ class DiscoveryServerProvider(robot_upstart.providers.Generic):
                 discovery_service_contents += line
 
         return {
-            "/lib/systemd/system/clearpath-discovery.service": {
-                "content": discovery_service_contents,
-                "mode": 0o644
+            '/lib/systemd/system/clearpath-discovery.service': {
+                'content': discovery_service_contents,
+                'mode': 0o644
             },
-            "/etc/systemd/system/clearpath-robot.service.wants/clearpath-discovery.service": {
-                "symlink": "/lib/systemd/system/clearpath-discovery.service"
+            '/etc/systemd/system/clearpath-robot.service.wants/clearpath-discovery.service': {
+                'symlink': '/lib/systemd/system/clearpath-discovery.service'
             }
         }
 
 
+class ZenohRouterServiceProvider(robot_upstart.providers.Generic):
+    def post_install(self):
+        pass
+
+    def generate_install(self):
+        zenoh_router_service_path = os.path.join(
+            get_package_share_directory('clearpath_robot'),
+            'services/clearpath-zenoh-router.service')
+        with open(zenoh_router_service_path) as f:
+            zenoh_router_service_contents: str = ''
+            zenoh_router_lines = f.readlines()
+            for line in zenoh_router_lines:
+                # Replace User with username from config
+                if 'User=' in line:
+                    line = f'User={clearpath_config.system.username}\n'
+                zenoh_router_service_contents += line
+
+        return {
+                '/lib/systemd/system/clearpath-zenoh-router.service': {
+                    'content': zenoh_router_service_contents,
+                    'mode': 0o644
+                },
+                '/etc/systemd/system/clearpath-robot.service.wants/clearpath-zenoh-router.service': {
+                    'symlink': '/lib/systemd/system/clearpath-zenoh-router.service'
+                }
+            }
+
+
 class RobotProvider(robot_upstart.providers.Generic):
     def post_install(self):
         pass
@@ -206,17 +236,17 @@ class RobotProvider(robot_upstart.providers.Generic):
         with open(robot_service_exec_contents_path) as f:
             robot_service_exec_contents = f.read()
         return {
-            "/lib/systemd/system/clearpath-robot.service": {
-                "content": robot_service_contents,
-                "mode": 0o644
+            '/lib/systemd/system/clearpath-robot.service': {
+                'content': robot_service_contents,
+                'mode': 0o644
             },
-            "/usr/sbin/clearpath-robot-generate": {
-                "content": robot_service_execpre_contents,
-                "mode": 0o755
+            '/usr/sbin/clearpath-robot-generate': {
+                'content': robot_service_execpre_contents,
+                'mode': 0o755
             },
-            "/usr/sbin/clearpath-robot-check": {
-                "content": robot_service_exec_contents,
-                "mode": 0o755
+            '/usr/sbin/clearpath-robot-check': {
+                'content': robot_service_exec_contents,
+                'mode': 0o755
             }
         }
 
@@ -297,6 +327,10 @@ manipulators_extras.install(Provider=ManipulatorsProvider)
 discovery_server = robot_upstart.Job(workspace_setup=workspace_setup)
 discovery_server.install(Provider=DiscoveryServerProvider)
 
+# Zenoh Router
+zenoh_router = robot_upstart.Job(workspace_setup=workspace_setup)
+zenoh_router.install(Provider=ZenohRouterServiceProvider)
+
 # Virtual CAN Bridge
 vcan_bridge = robot_upstart.Job(workspace_setup=workspace_setup)
 vcan_bridge.install(Provider=VirtualCANProvider)

clearpath_robot/services/clearpath-zenoh-router.service

@@ -0,0 +1,14 @@
+[Unit]
+Description="Clearpath Zenoh router service"
+PartOf=clearpath-robot.service
+After=clearpath-robot.service
+
+[Service]
+User=administrator
+Type=simple
+Restart=on-failure
+RestartSec=1
+ExecStart=/bin/bash -e /etc/clearpath/zenoh-router-start
+
+[Install]
+WantedBy=clearpath-robot.service