diff --git a/clearpath_generator_common/clearpath_generator_common/bash/generator.py b/clearpath_generator_common/clearpath_generator_common/bash/generator.py index 2edc5de8..6be22d75 100644 --- a/clearpath_generator_common/clearpath_generator_common/bash/generator.py +++ b/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() diff --git a/clearpath_generator_common/clearpath_generator_common/bash/writer.py b/clearpath_generator_common/clearpath_generator_common/bash/writer.py index db51e754..8e9199a3 100644 --- a/clearpath_generator_common/clearpath_generator_common/bash/writer.py +++ b/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() diff --git a/clearpath_generator_common/clearpath_generator_common/common.py b/clearpath_generator_common/clearpath_generator_common/common.py index ec70a53f..b4597fad 100644 --- a/clearpath_generator_common/clearpath_generator_common/common.py +++ b/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( diff --git a/clearpath_generator_common/clearpath_generator_common/description/manipulators.py b/clearpath_generator_common/clearpath_generator_common/description/manipulators.py index a82c8fdf..c8fed8e6 100644 --- a/clearpath_generator_common/clearpath_generator_common/description/manipulators.py +++ b/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): diff --git a/clearpath_generator_common/clearpath_generator_common/param/platform.py b/clearpath_generator_common/clearpath_generator_common/param/platform.py index bd975469..bd3b1120 100644 --- a/clearpath_generator_common/clearpath_generator_common/param/platform.py +++ b/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,7 +370,28 @@ 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': { @@ -377,8 +399,8 @@ def generate_parameters(self, use_sim_time: bool = False) -> None: 'networking': { 'type': 'diagnostic_aggregator/GenericAnalyzer', 'path': 'Networking', - 'contains': ['Wi-Fi'], - 'expected': ['wireless_watcher: Wi-Fi Monitor'] + 'contains': networking_contains, + 'expected': networking_expected, } } } diff --git a/clearpath_generator_common/package.xml b/clearpath_generator_common/package.xml index 8b0ec119..5696ffbe 100644 --- a/clearpath_generator_common/package.xml +++ b/clearpath_generator_common/package.xml @@ -20,6 +20,8 @@ clearpath_diagnostics clearpath_manipulators + python3-apt + ament_lint_auto ament_lint_common ament_cmake_pytest diff --git a/clearpath_generator_common/test/test_generator_bash.py b/clearpath_generator_common/test/test_generator_bash.py index 5dbc994e..e4e6096f 100644 --- a/clearpath_generator_common/test/test_generator_bash.py +++ b/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, diff --git a/clearpath_generator_common/test/test_generator_description.py b/clearpath_generator_common/test/test_generator_description.py index 2802bfb9..dcc7d342 100644 --- a/clearpath_generator_common/test/test_generator_description.py +++ b/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, diff --git a/clearpath_generator_common/test/test_generator_discovery_server.py b/clearpath_generator_common/test/test_generator_discovery_server.py index 2a849e59..f61592ac 100644 --- a/clearpath_generator_common/test/test_generator_discovery_server.py +++ b/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, diff --git a/clearpath_generator_common/test/test_generator_vcan.py b/clearpath_generator_common/test/test_generator_vcan.py index 96df1d16..cf177929 100644 --- a/clearpath_generator_common/test/test_generator_vcan.py +++ b/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,