Merge pull request #269 from clearpathrobotics/rc/jazzy/2.8

Jazzy 2.8 Release

Author: tonybaltovski

clearpath_generator_common/clearpath_generator_common/bash/generator.py

@@ -32,6 +32,8 @@
 # modification, is not permitted without the express permission
 # of Clearpath Robotics.
 
+from datetime import datetime, UTC
+
 from clearpath_config.common.types.discovery import Discovery
 from clearpath_generator_common.bash.writer import BashWriter
 from clearpath_generator_common.common import BaseGenerator, BashFile
@@ -48,18 +50,34 @@ def generate_setup_bash(self) -> None:
         clearpath_setup_bash = BashFile(filename='setup.bash', path=self.setup_path)
         bash_writer = BashWriter(clearpath_setup_bash)
 
-        workspaces = self.clearpath_config.system.workspaces
+        bash_writer.add_comment(f'Bash setup generated at {datetime.now(UTC)}')
+
+        # Additional ROS sources
+        sources = self.clearpath_config.system.bash.additional_sources
+        envs = self.clearpath_config.system.bash.additional_envars
+        if len(sources) > 0 or len(envs) > 0:
+            bash_writer.add_comment('Additional bash configuration from robot.yaml')
+            for s in sources:
+                bash_writer.add_source(BashFile(filename=s))
+
+            for e in envs:
+                bash_writer.add_export(e, envs[e])
 
         # Source core ROS
+        bash_writer.add_comment('Core ROS setup')
         ros_setup_bash = BashFile(filename='setup.bash', path=ROS_DISTRO_PATH)
         bash_writer.add_source(ros_setup_bash)
 
         # Additional workspaces
-        for workspace in workspaces:
-            bash_writer.add_source(
-                BashFile(filename='setup.bash', path=workspace.strip('setup.bash')))
+        workspaces = self.clearpath_config.system.workspaces
+        if len(workspaces) > 0:
+            bash_writer.add_comment('Additional workspaces')
+            for workspace in workspaces:
+                bash_writer.add_source(
+                    BashFile(filename='setup.bash', path=workspace.strip('setup.bash')))
 
         # ROS_DOMAIN_ID
+        bash_writer.add_comment('ROS configuration')
         domain_id = self.clearpath_config.system.domain_id
         bash_writer.add_export('ROS_DOMAIN_ID', domain_id)
 
@@ -87,4 +105,17 @@ def generate_setup_bash(self) -> None:
         else:
             bash_writer.add_unset('ROS_DISCOVERY_SERVER')
 
+        # ROS automatic discovery range
+        bash_writer.add_export(
+            'ROS_AUTOMATIC_DISCOVERY_RANGE',
+            self.clearpath_config.system.middleware.automatic_discovery_range.upper(),
+        )
+        if len(self.clearpath_config.system.middleware.static_peers) > 0:
+            bash_writer.add_export(
+                'ROS_STATIC_PEERS',
+                f'"{";".join(self.clearpath_config.system.middleware.static_peers)}"',
+            )
+        else:
+            bash_writer.add_unset('ROS_STATIC_PEERS')
+
         bash_writer.close()

clearpath_generator_common/clearpath_generator_common/bash/writer.py

@@ -44,7 +44,15 @@ def write(self, string, indent_level=0):
         self.file.write(f'{self.tab * indent_level}{string}\n')
 
     def add_export(self, envar: str, value, indent_level=0):
-        self.write(f'{self.tab * indent_level}export {envar}={value}')
+        value = str(value)
+        if (
+            value.startswith('"') and value.endswith('"')
+        ) or (
+            value.startswith("'") and value.endswith("'")
+        ):
+            self.write(f'{self.tab * indent_level}export {envar}={value}')
+        else:
+            self.write(f'{self.tab * indent_level}export {envar}="{value}"')
 
     def add_unset(self, envar: str, indent_level=0):
         self.write(f'{self.tab * indent_level}unset {envar}')
@@ -55,5 +63,8 @@ def add_source(self, bash_file: BashFile, indent_level=0):
     def add_echo(self, msg: str, indent_level=0):
         self.write(f'{self.tab * indent_level}echo "{msg}"')
 
+    def add_comment(self, msg: str, indent_level=0):
+        self.write(f'{self.tab * indent_level}# {msg}')
+
     def close(self):
         self.file.close()

clearpath_generator_common/clearpath_generator_common/common.py

@@ -295,10 +295,18 @@ def __add__(self, other):
 
 
 class BashFile():
+    """
+    A bash file we can source.
+
+    :param filename: The name of the file or the absolute path to the file
+    :param path: The absolute or relative directory containing the file if filename
+        is not a complete path. package must be specified if path is not absolute
+    :param package: The ROS package that contains path if it is not absolute
+    """
 
     def __init__(self,
                  filename: str,
-                 path: str,
+                 path: str = None,
                  package: Package = None,
                  ) -> None:
         self.package = package
@@ -310,8 +318,10 @@ def full_path(self):
         if self.package:
             return os.path.join(
                 get_package_share_directory(self.package.name), self.path, self.file)
-        else:
+        elif self.path:
             return os.path.join(self.path, self.file)
+        else:
+            return self.file
 
 
 class BaseGenerator():
@@ -326,7 +336,8 @@ def __init__(self,
                  setup_path: str = '/etc/clearpath/') -> None:
         # Define paths
         self.config_path = os.path.join(setup_path, 'robot.yaml')
-        assert os.path.exists(self.config_path)
+        if not os.path.exists(self.config_path):
+            raise FileNotFoundError(f'Config path {self.config_path} does not exist')
 
         self.setup_path = setup_path
         self.sensors_params_path = os.path.join(

clearpath_generator_common/clearpath_generator_common/description/manipulators.py

@@ -31,6 +31,9 @@
 # of Clearpath Robotics.
 from typing import List
 
+from clearpath_config.common.types.exception import (
+    UnsupportedAccessoryException,
+)
 from clearpath_config.manipulators.types.arms import (
     BaseArm,
     Franka,
@@ -145,7 +148,10 @@ def __init__(self, gripper: FrankaGripper):
     class BaseRobotiqGripperDescription(BaseDescription):
 
         def __init__(self, gripper: BaseGripper):
-            assert isinstance(gripper, Robotiq2F85) or isinstance(gripper, Robotiq2F140)
+            if not (isinstance(gripper, Robotiq2F85) or isinstance(gripper, Robotiq2F140)):
+                raise UnsupportedAccessoryException(
+                    f'Gripper must be of type "Robotiq2F85" or "Robotiq2F140", not "{type(gripper)}"'  # noqa: E501
+                )
             super().__init__(gripper)
             if (gripper.arm.MANIPULATOR_MODEL == KinovaGen3Dof6.MANIPULATOR_MODEL or
                     gripper.arm.MANIPULATOR_MODEL == KinovaGen3Dof7.MANIPULATOR_MODEL):

clearpath_generator_common/clearpath_generator_common/param/platform.py

@@ -39,6 +39,7 @@
 from clearpath_config.manipulators.types.arms import Franka
 from clearpath_config.manipulators.types.grippers import FrankaGripper
 from clearpath_config.platform.battery import BatteryConfig
+from clearpath_config.platform.wireless import PeplinkRouter
 from clearpath_config.sensors.types.cameras import BaseCamera, IntelRealsense
 from clearpath_config.sensors.types.gps import BaseGPS, NMEA
 from clearpath_config.sensors.types.imu import BaseIMU, PhidgetsSpatial
@@ -369,16 +370,37 @@ def generate_parameters(self, use_sim_time: bool = False) -> None:
                     }
                 })
 
-            if self.clearpath_config.platform.enable_wireless_watcher:
+            # We have a few optional nodes that go into the Networking section
+            # collect them all and then create the aggregator node
+            networking_contains = []
+            networking_expected = []
+
+            if self.clearpath_config.platform.wireless.enable_wireless_watcher:
+                networking_contains.append('Wi-Fi')
+                networking_expected.append('wireless_watcher: Wi-Fi Monitor')
+
+            if self.clearpath_config.platform.wireless.router:
+                if self.clearpath_config.platform.wireless.router == PeplinkRouter.MODEL:
+                    networking_contains.append('Router')
+                    networking_expected.append('router_node: Router')
+                # Put additional supported router hardware here...
+
+            if self.clearpath_config.platform.wireless.base_station:
+                if self.clearpath_config.platform.wireless.base_station == PeplinkRouter.MODEL:
+                    networking_contains.append('Base Station')
+                    networking_expected.append('base_station_node: Base Station')
+                # Put additional supported base station hardware here...
+
+            if len(networking_contains) > 0:
                 self.param_file.update({
                     self.DIAGNOSTIC_AGGREGATOR_NODE: {
                         'platform': {
                             'analyzers': {
                                 'networking': {
                                     'type': 'diagnostic_aggregator/GenericAnalyzer',
                                     'path': 'Networking',
-                                    'contains': ['Wi-Fi'],
-                                    'expected': ['wireless_watcher: Wi-Fi Monitor']
+                                    'contains': networking_contains,
+                                    'expected': networking_expected,
                                 }
                             }
                         }

clearpath_generator_common/package.xml

@@ -20,6 +20,8 @@
   <exec_depend version_gte="2.5.0">clearpath_diagnostics</exec_depend>
   <exec_depend version_gte="2.5.0">clearpath_manipulators</exec_depend>
 
+  <exec_depend>python3-apt</exec_depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_cmake_pytest</test_depend>

clearpath_generator_common/test/test_generator_bash.py

@@ -57,6 +57,8 @@ def test_samples(self):
                 print(f'Unsupported accessory: {e}. Skipping')
             except UnsupportedPlatformException as e:
                 print(f'Unsupported platform: {e}. Skipping')
+            except FileNotFoundError as e:
+                print(f'File not found: {e}. Skipping')
             except Exception as e:
                 errors.append("Sample '%s' failed to load: '%s'" % (
                     sample,

clearpath_generator_common/test/test_generator_description.py

@@ -62,6 +62,9 @@ def test_samples(self):
             except UnsupportedPlatformException as e:
                 print(f'Unsupported platform. {e}. Skipping')
                 continue
+            except FileNotFoundError as e:
+                print(f'File not found: {e}. Skipping')
+                continue
             except Exception as e:
                 errors.append("Sample '%s' failed to load: '%s'" % (
                     sample,

clearpath_generator_common/test/test_generator_discovery_server.py

@@ -57,6 +57,8 @@ def test_samples(self):
                 print(f'Unsupported accessory. {e}')
             except UnsupportedPlatformException as e:
                 print(f'Unsupported platform. {e}')
+            except FileNotFoundError as e:
+                print(f'File not found: {e}. Skipping')
             except Exception as e:
                 errors.append("Sample '%s' failed to load: '%s'" % (
                     sample,

clearpath_generator_common/test/test_generator_vcan.py

@@ -55,6 +55,8 @@ def test_samples(self):
                 print(f'Unsupported accessory: {e}. Skipping')
             except UnsupportedPlatformException as e:
                 print(f'Unsupported platform: {e}. Skipping')
+            except FileNotFoundError as e:
+                print(f'File not found: {e}. Skipping')
             except Exception as e:
                 errors.append("Sample '%s' failed to load: '%s'" % (
                     sample,