diff --git a/.github/workflows/unittests.yml b/.github/workflows/unittests.yml index 194560b..265e4d5 100644 --- a/.github/workflows/unittests.yml +++ b/.github/workflows/unittests.yml @@ -19,7 +19,7 @@ jobs: - name: Set up Python uses: actions/setup-python@v5 with: - python-version: '3.x' + python-version: '3.12' - name: Install system dependencies run: | @@ -40,4 +40,4 @@ jobs: - name: Run unit tests run: | - python -m unittest discover + python -u -m unittest discover -v diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 8a5da70..19f8e17 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,7 +1,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v5.0.0 + rev: v6.0.0 hooks: - id: check-added-large-files - id: check-merge-conflict @@ -16,12 +16,18 @@ repos: # Python hooks - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.12.7 + rev: v0.12.8 hooks: - id: ruff-check args: [ --fix ] - id: ruff-format + - repo: https://github.com/asottile/pyupgrade + rev: v3.20.0 + hooks: + - id: pyupgrade + args: [--py39-plus] + # Spellcheck - repo: https://github.com/crate-ci/typos rev: v1.34.0 diff --git a/package_xml_validation/helpers/cmake_parsers.py b/package_xml_validation/helpers/cmake_parsers.py index 4f74aed..31ddef5 100644 --- a/package_xml_validation/helpers/cmake_parsers.py +++ b/package_xml_validation/helpers/cmake_parsers.py @@ -1,14 +1,13 @@ from pathlib import Path import re -from typing import List -def remove_comments(lines: List[str]) -> list[str]: +def remove_comments(lines: list[str]) -> list[str]: """Removes comments from a list of lines.""" return [line.split("#", 1)[0].strip() for line in lines] -def read_cmake_lines_with_parens_joined(raw_lines: List[str]) -> list[str]: +def read_cmake_lines_with_parens_joined(raw_lines: list[str]) -> list[str]: """ Reads a CMake file and joins lines that have an opening '(' without a matching ')' until the closing ')' is found. Returns a list of logically complete lines. @@ -41,7 +40,7 @@ def has_balanced_parens(s: str) -> bool: return lines -def resolve_for_each(raw_lines: List[str]) -> List[str]: +def resolve_for_each(raw_lines: list[str]) -> list[str]: """Expands CMake's foreach() loops in a list of lines.""" foreach_stack = [] @@ -97,11 +96,11 @@ def resolve_for_each(raw_lines: List[str]) -> List[str]: return lines -def retrieve_cmake_dependencies(lines: List[str]) -> List[str]: +def retrieve_cmake_dependencies(lines: list[str]) -> tuple[list[str], list[str]]: if isinstance(lines, Path): lines = read_cmake_file(lines) - main_deps = [] - test_deps = [] + main_deps: list[str] = [] + test_deps: list[str] = [] # We'll track blocks of 'if(BUILD_TESTING)' with a small stack if_stack = [] @@ -176,14 +175,14 @@ def add_deps(dep_list: list[str], is_test: bool): return main_deps, test_deps -def read_cmake_file(file_path: Path) -> List[str]: +def read_cmake_file(file_path: Path) -> list[str]: """Reads a CMake file and returns a list of lines.""" if isinstance(file_path, str): file_path = Path(file_path) if not file_path.exists(): print(f"File not found: {file_path}") return [] - with open(file_path, "r") as f: + with open(file_path) as f: raw_lines = f.readlines() lines = remove_comments(raw_lines) lines = read_cmake_lines_with_parens_joined(lines) @@ -193,8 +192,10 @@ def read_cmake_file(file_path: Path) -> List[str]: return lines -def read_deps_from_cmake_file(file_path: Path) -> List[str]: +def read_deps_from_cmake_file(file_path: Path | str) -> tuple[list[str], list[str]]: """Reads a CMake file and returns a list of dependencies.""" + if isinstance(file_path, str): + file_path = Path(file_path) lines = read_cmake_file(file_path) try: main_deps, test_deps = retrieve_cmake_dependencies(lines) diff --git a/package_xml_validation/helpers/logger.py b/package_xml_validation/helpers/logger.py index 1dabeaf..f867247 100644 --- a/package_xml_validation/helpers/logger.py +++ b/package_xml_validation/helpers/logger.py @@ -1,4 +1,5 @@ import logging +import sys class ColoredFormatter(logging.Formatter): @@ -30,28 +31,24 @@ def format(self, record): def get_logger(name: str = __name__, level: str = "normal") -> logging.Logger: - """ - Returns a configured logger. If level=='verbose', DEBUG logs will be shown. - Otherwise, we default to INFO (as 'normal'). - """ logger = logging.getLogger(f"{name}_{level}") - logger.setLevel(logging.DEBUG) # We'll filter later using the handler level + logger.setLevel(logging.DEBUG) + + ch = logging.StreamHandler(sys.stdout) # stdout is better for CI + ch.flush = sys.stdout.flush # force flush after each log - ch = logging.StreamHandler() - # Map "normal" to INFO and "verbose" to DEBUG if level == "verbose": ch.setLevel(logging.DEBUG) else: ch.setLevel(logging.INFO) - # Use our custom colored formatter formatter = ColoredFormatter("%(message)s") ch.setFormatter(formatter) - # Prevent adding multiple handlers if the logger already exists if not logger.handlers: logger.addHandler(ch) + logger.propagate = False # don't let root logger duplicate lines return logger diff --git a/package_xml_validation/helpers/pkg_xml_formatter.py b/package_xml_validation/helpers/pkg_xml_formatter.py index f035ece..a4e89e8 100644 --- a/package_xml_validation/helpers/pkg_xml_formatter.py +++ b/package_xml_validation/helpers/pkg_xml_formatter.py @@ -1,11 +1,15 @@ -from lxml import etree as ET +import lxml.etree as ET from copy import deepcopy + try: from .logger import get_logger except ImportError: from helpers.logger import get_logger +# tuple of (element_name, min_occurrences, max_occurrences) +# min_occurrences is 1 if the element is required, 0 if it can be missing +# max_occurrences is None if there is no limit, otherwise it is a positive integer ELEMENTS = [ ("name", 1, 1), ("version", 1, 1), @@ -27,6 +31,20 @@ ("export", 0, 1), ] +NEW_LINE_BEFORE = [ + "buildtool_depend", + "build_depend", + "depend", + "exec_depend", + "doc_depend", + "test_depend", + "group_depend", + "member_of_group", + "export", +] + +NEW_LINE = "\n" + class PackageXmlFormatter: def __init__( @@ -45,10 +63,14 @@ def __init__( ) self.encountered_unresolvable_error = False + def prettyprint(self, element, **kwargs): + xml = ET.tostring(element, pretty_print=True, **kwargs) + print(xml.decode(), end="") + def check_dependency_order(self, root, xml_file): """Check and optionally correct the order of dependencies in the package.xml file (with comment preservation using lxml).""" - dependency_order = [elm[0] for elm in ELEMENTS if "depend" in elm[0]] + dependency_order = [elm[0] for elm in ELEMENTS] dependencies_with_comments = {dep: [] for dep in dependency_order} current_order = [] @@ -96,42 +118,37 @@ def check_dependency_order(self, root, xml_file): if not order_mismatch: return True + indentation = root[0].tail.replace(NEW_LINE, "") # Remove old dependency elements from root for dep_type in dependency_order: for elem in dependencies_with_comments[dep_type]: for comment in elem[1]: root.remove(comment) root.remove(elem[0]) - - # Find index of or append at end - export_index = next( - (i for i, elem in enumerate(root) if elem.tag == "export"), len(root) - ) - member_of_group_index = next( - (i for i, elem in enumerate(root) if elem.tag == "member_of_group"), - len(root), - ) - group_dep_index = next( - (i for i, elem in enumerate(root) if elem.tag == "group_depend"), len(root) - ) - - indendantion = root[0].tail.replace("\n", "") - # Reinsert sorted dependencies before , , or - insert_index = min(export_index, member_of_group_index, group_dep_index) - for dep_type in dependency_order: + # filter out empty lists from dependency order + dependency_order = [ + dep for dep in dependency_order if dependencies_with_comments[dep] + ] + insert_index = 0 + for index, dep_type in enumerate(dependency_order): sorted_elems = sorted( dependencies_with_comments[dep_type], key=lambda x: x[0].text ) for i, elem_with_comment in enumerate(sorted_elems): - if i != len(sorted_elems) - 1: - elem_with_comment[0].tail = "\n" + indendantion + if ( + i == len(sorted_elems) - 1 + and index + 1 < len(dependency_order) + and dependency_order[index + 1] in NEW_LINE_BEFORE + ): + elem_with_comment[0].tail = "\n\n" + indentation else: - elem_with_comment[0].tail = "\n\n" + indendantion + elem_with_comment[0].tail = NEW_LINE + indentation for comment in elem_with_comment[1]: root.insert(insert_index, comment) insert_index += 1 root.insert(insert_index, elem_with_comment[0]) insert_index += 1 + root[-1].tail = NEW_LINE self.logger.info(f"Corrected dependency order in {xml_file}.") return False @@ -251,13 +268,26 @@ def sort_key(elem): elements_with_comments = [] current_comments = [] + last_tail = "" + indentation = root[0].tail.replace(NEW_LINE, "") for elem in root: if elem.tag is ET.Comment: - current_comments.append(deepcopy(elem)) self.logger.error(f"Found comment: {elem.text}") + if last_tail and last_tail[-1] == NEW_LINE: + # inline comment -> append to previous element + elements_with_comments[-1][0].tail = elem.tail + elem.tail = NEW_LINE + indentation + elements_with_comments[-1][1].append(deepcopy(elem)) + else: + # Ensure only one NEW_LINE in elem.tail + elem.tail = NEW_LINE + ( + elem.tail.replace(NEW_LINE, "") if elem.tail else "" + ) + current_comments.append(deepcopy(elem)) else: elements_with_comments.append((deepcopy(elem), current_comments)) current_comments = [] + last_tail = elem.tail if elem.tail else "" # Sort the elements based on the expected order elements_with_comments.sort(key=lambda x: sort_key(x[0])) @@ -281,27 +311,27 @@ def check_for_empty_lines(self, root, xml_file): """ def remove_inner_newlines(s): - first_newline_pos = s.find("\n") - last_newline_pos = s.rfind("\n") + first_newline_pos = s.find(NEW_LINE) + last_newline_pos = s.rfind(NEW_LINE) if first_newline_pos == -1 or first_newline_pos == last_newline_pos: return s start = s[: first_newline_pos + 1] - middle = s[first_newline_pos + 1 : last_newline_pos].replace("\n", "") + middle = s[first_newline_pos + 1 : last_newline_pos].replace(NEW_LINE, "") end = s[last_newline_pos:] return start + middle + end found_empty_lines = False for elm in root: - if elm.tail and elm.tail.count("\n") > 2: + if elm.tail and elm.tail.count(NEW_LINE) > 2: self.logger.info( f"Error: More than one empty line found in {xml_file}." ) found_empty_lines = True if self.check_only: return False - if elm.tail is None or elm.tail.count("\n") == 0: + if elm.tail is None or elm.tail.count(NEW_LINE) == 0: found_empty_lines = True self.logger.info(f"Error: Two Elements in the sane line in {xml_file}.") if self.check_only: @@ -312,17 +342,86 @@ def remove_inner_newlines(s): if not found_empty_lines: return True # elements after last \n - indendantion = root[0].tail[root[0].tail.rfind("\n") + 1 :] + indendantion = root[0].tail[root[0].tail.rfind(NEW_LINE) + 1 :] # correct the empty lines & missing newlines for elm in root: - if elm.tail and elm.tail.count("\n") > 2: + if elm.tail and elm.tail.count(NEW_LINE) > 2: elm.tail = remove_inner_newlines(elm.tail) - elif elm.tail and elm.tail.count("\n") == 0: - elm.tail += "\n" + elif elm.tail and elm.tail.count(NEW_LINE) == 0: + elm.tail += NEW_LINE elif elm.tail is None: - elm.tail = "\n" + indendantion + elm.tail = NEW_LINE + indendantion return False + def check_indentation(self, root, level=1, indentation=" "): + """ + Check if the indentation of the XML file is correct. + recursively checks the indentation of each element. + """ + is_correct = True + + def check_indentation_string(string, expected_indent) -> bool: + """The string should be indented with the expected_indent and contain a newline.""" + if not string or not isinstance(string, str): + return False + parsed_indentation = string.replace(NEW_LINE, "") + return parsed_indentation == expected_indent and NEW_LINE in string + + def fix_indentation(string, expected_indent) -> str: + """Fix the indentation of the string to match the expected_indent.""" + indent = ( + string.replace(" ", "") if string and NEW_LINE in string else NEW_LINE + ) + return indent + expected_indent + + def check_and_correct(string, expected_indent, name) -> tuple[str, bool]: + """Check and correct the indentation of the string.""" + if not check_indentation_string(string, expected_indent): + # self.logger.error( + # f"Incorrect indentation for element '{name}'. Expected: '{expected_indent}', Found: '{string.replace(NEW_LINE, '') if string else 'None'}'" + # ) + if not self.check_only: + string = fix_indentation(string, expected_indent) + return string, True + return string, False + + root.text, corrected = check_and_correct( + root.text, indentation * level, f"{root.tag}-text" + ) + is_correct &= not corrected + + for index, elem in enumerate(root): + is_last = index == len(root) - 1 + expected_indent = ( + indentation * (level - 1) if is_last else indentation * level + ) + elem.tail, corrected = check_and_correct( + elem.tail, + expected_indent, + f"{elem.tag}-{elem.text[:15] if elem.text else 'None'}", + ) + is_correct &= not corrected + if len(elem) > 0: # has children + # check children recursively + if not self.check_indentation(elem, level + 1, indentation): + is_correct = False + else: + # make sure there are no new lines in texts + if elem.text and NEW_LINE in elem.text: + self.logger.error( + f"Element '{elem.tag}' has new lines in its text: '{elem.text}'" + ) + is_correct = False + if not self.check_only: + elem.text = elem.text.replace(NEW_LINE, " ").strip() + if not is_correct and self.check_only: + self.logger.error( + "Incorrect indentation found in package.xml. Please fix the indentations." + ) + elif not is_correct: + self.logger.warning("Auto-corrected indentation in package.xml.") + return is_correct + def check_for_non_existing_tags(self, root, xml_file): """Check for non-existing tags in the XML file.""" non_existing_tags = [] @@ -391,11 +490,12 @@ def add_dependencies(self, root, dependencies, dep_type): elements = [dep[0] for dep in ELEMENTS] if dep_type not in dep_types: raise ValueError(f"Invalid dependency type: {dep_type}") - indendantion = root[0].tail.replace("\n", "") + indendantion = root[0].tail.replace(NEW_LINE, "") + insert_position, first_of_group = 0, 0 for dep in dependencies: new_elem = ET.Element(dep_type) new_elem.text = dep - new_elem.tail = "\n" + indendantion + new_elem.tail = NEW_LINE + indendantion # add element to root at correct position -> correct dep group and alphabetical order # case 1: dependency group is empty if not root.findall(dep_type): @@ -423,11 +523,15 @@ def add_dependencies(self, root, dependencies, dep_type): insert_position = i + 1 root.insert(insert_position, new_elem) # adapt empty lines -> in case element prior ends with empty line move it to the new element - if insert_position > 0 and insert_position > first_of_group: + if ( + insert_position > 0 + and first_of_group is not None + and insert_position > first_of_group + ): previous_element = root[insert_position - 1] - if previous_element.tail and previous_element.tail.count("\n") > 1: + if previous_element.tail and previous_element.tail.count(NEW_LINE) > 1: new_elem.tail = previous_element.tail - previous_element.tail = "\n" + indendantion + previous_element.tail = NEW_LINE + indendantion if insert_position < len(root) - 1: # if next tag is different than the new element, add empty line next_element = root[insert_position + 1] @@ -441,7 +545,7 @@ def add_build_type_export(self, root, build_type: str): If it does not exist, it will be created. Other exports will not be changed(besides the build_type export). """ - indendantion = root[0].tail.replace("\n", "") + indendantion = root[0].tail.replace(NEW_LINE, "") export = root.find("export") if export is None: export = ET.Element("export") @@ -450,15 +554,58 @@ def add_build_type_export(self, root, build_type: str): last_element = root[-1] if last_element.tail: last_element.tail = "\n\n" + indendantion - export.tail = "\n" + export.tail = NEW_LINE root.append(export) build_type_elem = export.find("build_type") if build_type_elem is None: build_type_elem = ET.Element("build_type") - build_type_elem.tail = "\n" + indendantion + build_type_elem.tail = NEW_LINE + indendantion export.append(build_type_elem) build_type_elem.text = build_type - export.text = "\n" + 2 * indendantion + export.text = NEW_LINE + 2 * indendantion + + def add_buildtool_depends(self, root, buildtool: list[str]): + """ + Add the buildtool_depend to the XML file. + If the buildtool_depend already exists, it will be updated. + If it does not exist, it will be created. + """ + indendantion = root[0].tail.replace(NEW_LINE, "") + # 1. clear existing buildtool_depend elements + for elem in root.findall("buildtool_depend"): + root.remove(elem) + # 2. insertposition -> after license, url, or author element + insert_position = 0 + for i, elem in enumerate(root): + if isinstance(elem.tag, str) and elem.tag == "license": + insert_position = i + 1 + elif isinstance(elem.tag, str) and elem.tag == "url": + insert_position = i + 1 + elif isinstance(elem.tag, str) and elem.tag == "author": + insert_position = i + 1 + # 3. add buildtool_depend elements + for i, tool in enumerate(buildtool): + is_last = i == len(buildtool) - 1 + new_elem = ET.Element("buildtool_depend") + new_elem.text = tool + new_elem.tail = "\n\n" if is_last else NEW_LINE + new_elem.tail += indendantion + root.insert(insert_position, new_elem) + insert_position += 1 + + def add_member_of_group(self, root, group_name: str): + """Add member_of_group element to the XML file.""" + indendantion = root[0].tail.replace(NEW_LINE, "") + member_of_group = ET.Element("member_of_group") + member_of_group.text = group_name + member_of_group.tail = "\n\n" + indendantion + # insert position -> right before export or at the end + insert_position = len(root) + for i, elem in enumerate(root): + if isinstance(elem.tag, str) and elem.tag == "export": + insert_position = i + break + root.insert(insert_position, member_of_group) if __name__ == "__main__": diff --git a/package_xml_validation/helpers/workspace.py b/package_xml_validation/helpers/workspace.py index 30a46a5..79c3f33 100644 --- a/package_xml_validation/helpers/workspace.py +++ b/package_xml_validation/helpers/workspace.py @@ -15,7 +15,6 @@ import sys import xml.etree.ElementTree as ET from pathlib import Path -from typing import Dict, List import os @@ -95,9 +94,9 @@ def parse_pkg_name(package_xml: Path) -> str: return package_xml.parent.name # fallback -def pkg_iterator(src_dir: Path) -> Dict[str, Path]: +def pkg_iterator(src_dir: Path) -> dict[str, Path]: """Yield ``{pkg_name: pkg_path}`` for all packages under *src_dir*.""" - pkgs: Dict[str, Path] = {} + pkgs: dict[str, Path] = {} for xml in src_dir.rglob("package.xml"): # Respect COLCON_IGNORE: ignore a path if any ancestor contains the file if any((parent / "COLCON_IGNORE").exists() for parent in xml.parents): @@ -106,12 +105,13 @@ def pkg_iterator(src_dir: Path) -> Dict[str, Path]: return pkgs -def get_pkgs_in_wrs(path: Path) -> List[str]: +def get_pkgs_in_wrs(path: Path) -> list[str]: """Return all package names in the workspace that contains *path*.""" if isinstance(path, str): path = Path(path).resolve(strict=True) if not path.exists(): raise ValueError(f"Path does not exist: {path}") + pkg_dir = None try: pkg_dir = find_package_dir(path) ws_root = find_workspace_root(pkg_dir) diff --git a/package_xml_validation/package_xml_validator.py b/package_xml_validation/package_xml_validator.py index 78266c8..0e1b6e0 100644 --- a/package_xml_validation/package_xml_validator.py +++ b/package_xml_validation/package_xml_validator.py @@ -1,7 +1,7 @@ import argparse import os -from typing import List -from lxml import etree as ET +import lxml.etree as ET +from enum import Enum try: from .helpers.logger import get_logger @@ -16,6 +16,13 @@ from helpers.cmake_parsers import read_deps_from_cmake_file from helpers.find_launch_dependencies import scan_files import subprocess +import re + + +class PackageType(Enum): + CMAKE_PKG = "ament_cmake" + PYTHON_PKG = "ament_python" + MSG_PKG = "rosidl_default_generators" class PackageXmlValidator: @@ -46,7 +53,7 @@ def __init__( self.encountered_unresolvable_error = False # calculate num checks - self.num_checks = 8 + self.num_checks = 11 self.check_count = 1 if self.check_rosdeps: self.num_checks += 1 @@ -73,7 +80,26 @@ def find_package_xml_files(self, paths): package_xml_files = list(set(package_xml_files)) return package_xml_files - def check_for_rosdeps(self, rosdeps: List[str], xml_file: str): + def get_package_type(self, xml_file: str) -> tuple[PackageType, bool]: + """Determine the package type based on the presence of CMakeLists.txt or setup.py. + Returns where bool indicates if the package is a message package.""" + cmake_file = os.path.join(os.path.dirname(xml_file), "CMakeLists.txt") + setup_file = os.path.join(os.path.dirname(xml_file), "setup.py") + is_msg_pkg = False + pkg_type = PackageType.CMAKE_PKG + if os.path.exists(cmake_file): + # check if rosidl_generate_interfaces is in CMakeLists.txt using regex + regex = r"rosidl_generate_interfaces\s*\(\s*.*?\)" + with open(cmake_file) as f: + content = f.read() + if re.search(regex, content, re.DOTALL): + is_msg_pkg = True + pkg_type = PackageType.CMAKE_PKG + elif os.path.exists(setup_file): + pkg_type = PackageType.PYTHON_PKG + return pkg_type, is_msg_pkg + + def check_for_rosdeps(self, rosdeps: list[str], xml_file: str): """extract list of rosdeps and check if they are valid""" if not rosdeps: self.logger.info(f"No ROS dependencies found in {xml_file}.") @@ -88,7 +114,7 @@ def check_for_rosdeps(self, rosdeps: List[str], xml_file: str): return True def check_for_cmake( - self, build_deps: List[str], test_deps: List[str], xml_file: str, root + self, build_deps: list[str], test_deps: list[str], xml_file: str, root ): cmake_file = os.path.join(os.path.dirname(xml_file), "CMakeLists.txt") if not os.path.exists(cmake_file): @@ -167,12 +193,12 @@ def validate_launch_dependencies( root, package_xml_file: str, package_name: str, - exec_deps: List[str], - test_deps: List[str] = [], + exec_deps: list[str], + test_deps: list[str] = [], ): """Validate launch dependencies in the package.xml file.""" - def extract_launch_deps(folder_names: List[str]) -> List[str]: + def extract_launch_deps(folder_names: list[str]) -> list[str]: """Extract launch dependencies from the folder names.""" launch_deps = [] for folder in folder_names: @@ -182,13 +208,10 @@ def extract_launch_deps(folder_names: List[str]) -> List[str]: return launch_deps def validate_launch_folders( - launch_folder_names: List[str], xml_deps: List[str], depend_tag: str + launch_folder_names: list[str], xml_deps: list[str], depend_tag: str ) -> bool: launch_deps = extract_launch_deps(launch_folder_names) if not launch_deps: - self.logger.debug( - f"No launch dependencies found in {package_name}/package.xml." - ) return True missing_deps = [ @@ -196,6 +219,16 @@ def validate_launch_folders( for dep in launch_deps if dep not in xml_deps and dep != package_name ] + if missing_deps: + sep = "\n\t - " + self.logger.warning( + f"Missing <{depend_tag}> dependencies in {package_name}/package.xml: {sep}{sep.join(missing_deps)}" + ) + missing_deps = [ + dep + for dep in launch_deps + if dep not in xml_deps and dep != package_name + ] if missing_deps: sep = "\n\t - " self.logger.warning( @@ -204,6 +237,12 @@ def validate_launch_folders( if self.check_only: return False + elif not self.auto_fill_missing_deps: + self.encountered_unresolvable_error = True + self.logger.error( + f"Cannot auto-fill missing <{depend_tag}> dependencies: {missing_deps} in {package_name}/package.xml. Please add them manually." + ) + return False else: self.logger.info( f"Auto-filling {len(missing_deps)} missing <{depend_tag}> dependencies in {package_name}/package.xml." @@ -233,41 +272,96 @@ def validate_launch_folders( test_deps_valid = validate_launch_folders(["test"], test_deps, "test_depend") return launch_deps_valid and test_deps_valid - def validate_ament_exports(self, root, xml_file: str): - """Validate ament_export tags in the package.xml file. - if a CMakeLists.txt file exists: ament_cmake must be present - if a setup.py file exists: ament_python must be present + def validate_buildtool_depend(self, root, xml_file: str): + """Validate build_tool depend tags in the package.xml file. + Note: for interface packages 2 buildtool_depend tags are required: ament_cmake and rosidl_default_generators """ - cmake_file = os.path.join(os.path.dirname(xml_file), "CMakeLists.txt") - setup_file = os.path.join(os.path.dirname(xml_file), "setup.py") - cmake_present = os.path.exists(cmake_file) - setup_present = os.path.exists(setup_file) + pkg_type, is_msg_pkg = self.get_package_type(xml_file) + buildtool = root.findall("buildtool_depend") + buildtool = [tool.text for tool in buildtool] + is_buildtool_correct = ( + len(buildtool) > 0 + and ( + ( + pkg_type == PackageType.CMAKE_PKG + and PackageType.CMAKE_PKG.value in buildtool + ) + or ( + pkg_type == PackageType.PYTHON_PKG + and PackageType.PYTHON_PKG.value in buildtool + ) + ) + and (not is_msg_pkg or PackageType.MSG_PKG.value in buildtool) + ) + + if is_buildtool_correct: + return True + else: + corrected_buildtool_str = f"{'ament_cmake' if pkg_type == PackageType.CMAKE_PKG else 'ament_python'}" + if is_msg_pkg: + corrected_buildtool_str += ( + f" {PackageType.MSG_PKG.value}" + ) + if len(buildtool) == 0: + self.logger.error( + f"Missing tag in package.xml. Please include {corrected_buildtool_str}." + ) + else: + self.logger.error( + f"Incorrect in package.xml. Expected {corrected_buildtool_str}, found {buildtool if buildtool is not None else 'None'}." + ) + if self.check_only: + return False + if not self.auto_fill_missing_deps: + self.logger.error( + f"Cannot auto-fill missing in {os.path.basename(os.path.dirname(xml_file))}/package.xml. Please add it manually." + ) + self.encountered_unresolvable_error = True + return False + else: + self.formatter.add_buildtool_depends( + root, + [pkg_type.value, PackageType.MSG_PKG.value] + if is_msg_pkg + else [pkg_type.value], + ) + self.logger.warning( + f"Auto-filling {corrected_buildtool_str} in {os.path.basename(os.path.dirname(xml_file))}/package.xml." + ) + return False # Indicate that changes were made + + def validate_ament_exports(self, root, xml_file: str): + """Validate ament_export tags in the package.xml file.""" + pkg_type, _ = self.get_package_type(xml_file) export = root.find("export") export_exists = export is not None build_type = export.find("build_type") if export_exists else None build_type_correct = ( ( - cmake_present + pkg_type == PackageType.CMAKE_PKG and build_type is not None and build_type.text == "ament_cmake" ) or ( - setup_present + pkg_type == PackageType.PYTHON_PKG and build_type is not None and build_type.text == "ament_python" ) - or (not cmake_present and not setup_present) + or ( + not pkg_type == PackageType.CMAKE_PKG + and not pkg_type == PackageType.PYTHON_PKG + ) ) if build_type_correct: return True else: if not export_exists: self.logger.error( - f"Missing tag in package.xml. Please include {'ament_cmake' if cmake_present else 'ament_python'}." + f"Missing tag in package.xml. Please include {'ament_cmake' if pkg_type == PackageType.CMAKE_PKG else 'ament_python'}." ) else: self.logger.error( - f"Incorrect in tag in package.xml. Expected {'ament_cmake' if cmake_present else 'ament_python'}, found {build_type.text if build_type is not None else 'None'}." + f"Incorrect in tag in package.xml. Expected {'ament_cmake' if pkg_type == PackageType.CMAKE_PKG else 'ament_python'}, found {build_type.text if build_type is not None else 'None'}." ) if self.check_only: return False @@ -276,13 +370,40 @@ def validate_ament_exports(self, root, xml_file: str): return False else: self.formatter.add_build_type_export( - root, "ament_cmake" if cmake_present else "ament_python" + root, + "ament_cmake" if pkg_type == PackageType.CMAKE_PKG else "ament_python", ) self.logger.warning( - f"Auto-filling {'ament_cmake' if cmake_present else 'ament_python'} in {os.path.basename(os.path.dirname(xml_file))}/package.xml." + f"Auto-filling {'ament_cmake' if pkg_type == PackageType.CMAKE_PKG else 'ament_python'} in {os.path.basename(os.path.dirname(xml_file))}/package.xml." ) return False # Indicate that changes were made + def validate_member_of_group(self, root, xml_file: str): + """Validate member_of_group tag in the package.xml file. + -> interface packages must include rosidl_interface_packages + """ + member_of_group = root.find("member_of_group") + _, is_msg_pkg = self.get_package_type(xml_file) + if is_msg_pkg and ( + member_of_group is None + or member_of_group.text != "rosidl_interface_packages" + ): + self.logger.error( + f"Missing or incorrect in package.xml. Expected rosidl_interface_packages, found {member_of_group.text if member_of_group is not None else 'None'}." + ) + if self.check_only: + return False + if not self.auto_fill_missing_deps: + self.encountered_unresolvable_error = True + return False + else: + self.formatter.add_member_of_group(root, "rosidl_interface_packages") + self.logger.warning( + f"Auto-filling rosidl_interface_packages in {os.path.basename(os.path.dirname(xml_file))}/package.xml." + ) + return False # Indicate that changes were made + return True + def log_check_result(self, check_name, result): """Log the result of a check.""" if result: @@ -317,6 +438,7 @@ def check_and_format_files(self, package_xml_files): self.xml_valid = True pkg_name = os.path.basename(os.path.dirname(xml_file)) self.logger.info(f"Processing {pkg_name}...") + self.logger.debug(f"Checking {xml_file}...") if not os.path.exists(xml_file): raise FileNotFoundError(f"{xml_file} does not exist.") @@ -369,6 +491,11 @@ def check_and_format_files(self, package_xml_files): root, xml_file, ) + self.perform_check( + "Check indentation", + self.formatter.check_indentation, + root, + ) self.perform_check( "Check launch dependencies", @@ -380,6 +507,20 @@ def check_and_format_files(self, package_xml_files): self.formatter.retrieve_test_dependencies(root), ) + self.perform_check( + "Check build tool depend", + self.validate_buildtool_depend, + root, + xml_file, + ) + + self.perform_check( + "Check member of group", + self.validate_member_of_group, + root, + xml_file, + ) + self.perform_check( "Check build type export", self.validate_ament_exports, @@ -512,11 +653,6 @@ def main(): print("Cannot use --compare-with-cmake with --skip-rosdep-key-validation.") args.compare_with_cmake = False - # --auto-fill-missing-deps is only possible with --compare-with-cmake - if not args.compare_with_cmake and args.auto_fill_missing_deps: - print("Cannot use --auto-fill-missing-deps without --compare-with-cmake.") - args.auto_fill_missing_deps = False - formatter = PackageXmlValidator( check_only=args.check_only, verbose=args.verbose, diff --git a/package_xml_validation/ros2_pkg_validator.py b/package_xml_validation/ros2_pkg_validator.py deleted file mode 100644 index 0b6a4a0..0000000 --- a/package_xml_validation/ros2_pkg_validator.py +++ /dev/null @@ -1,228 +0,0 @@ -#!/usr/bin/env python3 -""" -ros2-dependency-validator.py - -A simple script that: -1. Finds package.xml and/or CMakeLists.txt files in user-specified locations. -2. Parses dependencies (main + test) from each. -3. Optionally (via --check) only reports findings without any attempt to fix them. -""" - -import argparse -import sys -import os -from pathlib import Path -from typing import List -import xml.etree.ElementTree as ET -from helpers.cmake_parsers import retrieve_cmake_dependencies -from helpers.rosdep_validator import check_rosdeps - - -def parse_package_xml(xml_path: Path): - """ - Parse for normal dependencies and test dependencies. - - Returns: - (main_deps, test_deps) - main_deps: list of strings for normal (build, etc.) dependencies - test_deps: list of strings for test_depend - """ - main_deps = [] - test_deps = [] - - if not xml_path.exists(): - return main_deps, test_deps - - tree = ET.parse(xml_path) - root = tree.getroot() - - main_tags = {"depend", "build_depend", "build_export_depend", "buildtool_depend"} - test_tags = {"test_depend"} - - for child in root: - tag_name = child.tag.lower() - text_value = (child.text or "").strip() - if tag_name in main_tags: - main_deps.append(text_value) - elif tag_name in test_tags: - test_deps.append(text_value) - - return main_deps, test_deps - - -def find_files_in_dir(base_dir: Path): - """ - Recursively find all package.xml or CMakeLists.txt files under base_dir. - Returns two sets: (xml_files, cmake_files). - """ - xml_files = set() - cmake_files = set() - for root, dirs, files in os.walk(base_dir): - for file in files: - if file == "package.xml": - xml_files.add(Path(root) / file) - elif file == "CMakeLists.txt": - cmake_files.add(Path(root) / file) - return xml_files, cmake_files - - -def collect_cmake_package_pairs(cmake_files: List[Path], package_xmls: List[Path]): - # corresponding package.xml and cmake files are in the same directory - pairs = [] - visited_cmake_files = set() - for package_xml in package_xmls: - pairs.append((package_xml, package_xml.parent / "CMakeLists.txt")) - visited_cmake_files.add(package_xml.parent / "CMakeLists.txt") - for cmake_file in cmake_files: - if cmake_file not in visited_cmake_files: - pairs.append((cmake_file.parent / "package.xml", cmake_file)) - # make sure all files e - existing_pairs = [] - for package_xml, cmake_file in pairs: - if package_xml.exists() and cmake_file.exists(): - existing_pairs.append((package_xml, cmake_file)) - return existing_pairs - - -def print_missing(source, target, missing_in_target_deps, is_test=False): - dep_name = "test " if is_test else "" - print(f"\t {source} -> {target} missing {dep_name}dependencies:") - for dep in missing_in_target_deps: - print(f"\t\t{dep}") - - -def compare_if_missing(source, target): - # in priniciple missing_in_target = set(source) - set(target), but robust to - # lib vs and -dev vs - missing_in_target = set() - for dep in source: - exists = dep in target - if exists: - continue - if dep.startswith("lib"): - exists = dep[3:] in target - else: - exists = f"lib{dep}" in target - if exists: - continue - if dep.endswith("-dev"): - exists = dep[:-4] in target - else: - exists = f"{dep}-dev" in target - if not exists: - missing_in_target.add(dep) - return missing_in_target - - -def validate(paths, check_rosdep=False, verbose=False): - # If SRC is empty or is just ['.'], treat it as "scan current directory". - if not paths or paths == ["."]: - src_paths = [Path(".").resolve()] - else: - src_paths = [Path(s).resolve() for s in paths] - - # Collect unique package.xml and CMakeLists.txt from all provided paths - all_package_xmls = set() - all_cmake_lists = set() - - for path in src_paths: - if path.is_dir(): - # Recursively scan - xmls, cmakes = find_files_in_dir(path) - all_package_xmls.update(xmls) - all_cmake_lists.update(cmakes) - elif path.is_file(): - if path.name == "package.xml": - all_package_xmls.add(path) - elif path.name == "CMakeLists.txt": - all_cmake_lists.add(path) - # else: non-existent path is ignored in this example - - # If no relevant files found, exit 0 (no error) - if not all_package_xmls and not all_cmake_lists: - print("No package.xml or CMakeLists.txt found. Exiting with code 0.") - sys.exit(0) - - # collect pairs of package.xml and CMakeLists.txt files - pairs = collect_cmake_package_pairs(list(all_cmake_lists), list(all_package_xmls)) - - for package_xml, cmake_list in pairs: - package_name = package_xml.parent.name - main_deps_xml, test_deps_xml = parse_package_xml(package_xml) - main_deps_cmake, test_deps_cmake = retrieve_cmake_dependencies(cmake_list) - if verbose: - print(f"Package.xml: {package_xml}") - print(f"CMakeLists.txt: {cmake_list}") - print(f"Main deps XML: {main_deps_xml}") - print(f"Test deps XML: {test_deps_xml}") - print(f"Main deps CMake: {main_deps_cmake}") - print(f"Test deps CMake: {test_deps_cmake}") - # Check for discrepancies between package.xml and CMakeLists.txt - deps_missing_in_cmake = compare_if_missing(main_deps_xml, main_deps_cmake) - deps_missing_in_xml = compare_if_missing(main_deps_cmake, main_deps_xml) - test_deps_missing_in_cmake = compare_if_missing(test_deps_xml, test_deps_cmake) - test_deps_missing_in_xml = compare_if_missing(test_deps_cmake, test_deps_xml) - - if ( - deps_missing_in_cmake - or deps_missing_in_xml - or test_deps_missing_in_cmake - or test_deps_missing_in_xml - ): - print(f"{package_name}") - - if deps_missing_in_cmake: - print_missing("package.xml", "CMakeLists.txt", deps_missing_in_cmake) - if deps_missing_in_xml: - print_missing("CMakeLists.txt", "package.xml", deps_missing_in_xml) - if test_deps_missing_in_cmake: - print_missing( - "package.xml", "CMakeLists.txt", test_deps_missing_in_cmake, True - ) - if test_deps_missing_in_xml: - print_missing( - "CMakeLists.txt", "package.xml", test_deps_missing_in_xml, True - ) - if ( - deps_missing_in_cmake - or deps_missing_in_xml - or test_deps_missing_in_cmake - or test_deps_missing_in_xml - ): - print("\n") - - # make sure the listed keys in the package.xml are resolvable - if check_rosdep: - unresolvable_deps = check_rosdeps(main_deps_xml + test_deps_xml) - if unresolvable_deps: - print(f"Could not resolve dependencies in {package_name}:") - for dep in unresolvable_deps: - print(f"\t{dep}") - print("\n") - - sys.exit(0) - - -def main(): - parser = argparse.ArgumentParser(description="ROS2 dependency validator.") - parser.add_argument( - "--check_rosdeps", - action="store_true", - default=False, - help="If set tests whether rosdep is able to resolve the listed dependencies.", - ) - parser.add_argument( - "SRC", - nargs="*", - help="List of files or directories to check. If empty or '.', we scan the current directory.", - ) - - args = parser.parse_args() - validate(args.SRC, args.check_rosdeps) - - -if __name__ == "__main__": - # main() - - paths = ["/home/aljoscha-schmidt/hector/src/"] - validate(paths, True, False) diff --git a/pyproject.toml b/pyproject.toml index c091ad0..307736c 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -7,7 +7,7 @@ name = "package_xml_validation" version = "0.1.0" description = "Validates package xml of ros2 pkgs" readme = "README.md" -requires-python = ">=3.7" +requires-python = ">=3.9" dependencies = [ "lxml", "rosdep", diff --git a/tests/__init__.py b/tests/__init__.py index e69de29..1d1547b 100644 --- a/tests/__init__.py +++ b/tests/__init__.py @@ -0,0 +1,9 @@ +import logging +import sys + +logging.basicConfig( + level=logging.DEBUG, # show all levels + format="%(message)s", + stream=sys.stdout, # important for CI + force=True, # override any previous logging config +) diff --git a/tests/examples/export_tag_examples/ament_cmake/pkg_false_buildtool_depend/CMakeLists.txt b/tests/examples/export_tag_examples/ament_cmake/pkg_false_buildtool_depend/CMakeLists.txt new file mode 100644 index 0000000..8b10ece --- /dev/null +++ b/tests/examples/export_tag_examples/ament_cmake/pkg_false_buildtool_depend/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.8) +project(hector_gamepad_manager) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +set(DEPENDENCIES pluginlib rclcpp sensor_msgs yaml-cpp + hector_gamepad_plugin_interface) + +foreach(dependency IN LISTS DEPENDENCIES) + find_package(${dependency} REQUIRED) +endforeach() + +set(HEADERS include/hector_gamepad_manager/hector_gamepad_manager.hpp) + +set(SOURCES src/hector_gamepad_manager.cpp) + +add_library(${PROJECT_NAME} SHARED ${SOURCES} ${HEADERS}) +target_include_directories( + ${PROJECT_NAME} PUBLIC $ + $) +target_link_libraries(${PROJECT_NAME} PUBLIC yaml-cpp) +ament_target_dependencies(${PROJECT_NAME} PUBLIC ${DEPENDENCIES}) + +add_executable(hector_gamepad_manager_node src/hector_gamepad_manager_node.cpp) +target_link_libraries(hector_gamepad_manager_node PUBLIC ${PROJECT_NAME}) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-targets + LIBRARY DESTINATION lib) +install(DIRECTORY include/ DESTINATION include) +install(TARGETS hector_gamepad_manager_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ros_testing REQUIRED) + ament_add_gtest_executable(${PROJECT_NAME}_test + test/test_hector_gamepad_manager.cpp) + target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) + ament_target_dependencies(${PROJECT_NAME}_test + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(test/test_hector_gamepad_manager.launch.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") +endif() + +ament_export_targets(${PROJECT_NAME}-targets) +ament_export_include_directories(include) +ament_export_dependencies(${DEPENDENCIES}) +ament_package() diff --git a/tests/examples/export_tag_examples/ament_cmake/pkg_false_buildtool_depend/package.xml b/tests/examples/export_tag_examples/ament_cmake/pkg_false_buildtool_depend/package.xml new file mode 100644 index 0000000..f44d893 --- /dev/null +++ b/tests/examples/export_tag_examples/ament_cmake/pkg_false_buildtool_depend/package.xml @@ -0,0 +1,28 @@ + + + + hector_gamepad_manager + 0.0.0 + Package for managing gamepad inputs + Simon Giegerich + TODO: License declaration + + ament_python + + hector_gamepad_plugin_interface + pluginlib + rclcpp + sensor_msgs + yaml-cpp + + hector_gamepad_manager_plugins + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + ros_testing + + + ament_cmake + + diff --git a/tests/examples/export_tag_examples/ament_cmake/pkg_no_buildtool_depend/CMakeLists.txt b/tests/examples/export_tag_examples/ament_cmake/pkg_no_buildtool_depend/CMakeLists.txt new file mode 100644 index 0000000..8b10ece --- /dev/null +++ b/tests/examples/export_tag_examples/ament_cmake/pkg_no_buildtool_depend/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.8) +project(hector_gamepad_manager) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +set(DEPENDENCIES pluginlib rclcpp sensor_msgs yaml-cpp + hector_gamepad_plugin_interface) + +foreach(dependency IN LISTS DEPENDENCIES) + find_package(${dependency} REQUIRED) +endforeach() + +set(HEADERS include/hector_gamepad_manager/hector_gamepad_manager.hpp) + +set(SOURCES src/hector_gamepad_manager.cpp) + +add_library(${PROJECT_NAME} SHARED ${SOURCES} ${HEADERS}) +target_include_directories( + ${PROJECT_NAME} PUBLIC $ + $) +target_link_libraries(${PROJECT_NAME} PUBLIC yaml-cpp) +ament_target_dependencies(${PROJECT_NAME} PUBLIC ${DEPENDENCIES}) + +add_executable(hector_gamepad_manager_node src/hector_gamepad_manager_node.cpp) +target_link_libraries(hector_gamepad_manager_node PUBLIC ${PROJECT_NAME}) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-targets + LIBRARY DESTINATION lib) +install(DIRECTORY include/ DESTINATION include) +install(TARGETS hector_gamepad_manager_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ros_testing REQUIRED) + ament_add_gtest_executable(${PROJECT_NAME}_test + test/test_hector_gamepad_manager.cpp) + target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) + ament_target_dependencies(${PROJECT_NAME}_test + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(test/test_hector_gamepad_manager.launch.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") +endif() + +ament_export_targets(${PROJECT_NAME}-targets) +ament_export_include_directories(include) +ament_export_dependencies(${DEPENDENCIES}) +ament_package() diff --git a/tests/examples/export_tag_examples/ament_cmake/pkg_no_buildtool_depend/package.xml b/tests/examples/export_tag_examples/ament_cmake/pkg_no_buildtool_depend/package.xml new file mode 100644 index 0000000..76da736 --- /dev/null +++ b/tests/examples/export_tag_examples/ament_cmake/pkg_no_buildtool_depend/package.xml @@ -0,0 +1,26 @@ + + + + hector_gamepad_manager + 0.0.0 + Package for managing gamepad inputs + Simon Giegerich + TODO: License declaration + + hector_gamepad_plugin_interface + pluginlib + rclcpp + sensor_msgs + yaml-cpp + + hector_gamepad_manager_plugins + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + ros_testing + + + ament_cmake + + diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_correct/CMakeLists.txt b/tests/examples/export_tag_examples/msg_pkg/pkg_correct/CMakeLists.txt new file mode 100644 index 0000000..4e454e4 --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_correct/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.8) +project(hector_transmission_interface_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} srv/AdjustTransmissionOffsets.srv + DEPENDENCIES sensor_msgs) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_correct/package.xml b/tests/examples/export_tag_examples/msg_pkg/pkg_correct/package.xml new file mode 100644 index 0000000..4ad2b87 --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_correct/package.xml @@ -0,0 +1,24 @@ + + + + hector_transmission_interface_msgs + 0.0.0 + TODO + + Aljoscha Schmidt + TODO + Aljoscha Schmidt + + ament_cmake + rosidl_default_generators + + rosidl_default_generators + sensor_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + ament_cmake + + diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_false_export/CMakeLists.txt b/tests/examples/export_tag_examples/msg_pkg/pkg_false_export/CMakeLists.txt new file mode 100644 index 0000000..4e454e4 --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_false_export/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.8) +project(hector_transmission_interface_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} srv/AdjustTransmissionOffsets.srv + DEPENDENCIES sensor_msgs) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_false_export/package.xml b/tests/examples/export_tag_examples/msg_pkg/pkg_false_export/package.xml new file mode 100644 index 0000000..246d1a9 --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_false_export/package.xml @@ -0,0 +1,24 @@ + + + + hector_transmission_interface_msgs + 0.0.0 + TODO + + Aljoscha Schmidt + TODO + Aljoscha Schmidt + + ament_cmake + rosidl_default_generators + + rosidl_default_generators + sensor_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + rosidl_default_generators + + diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_missing_ament_buildtool/CMakeLists.txt b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_ament_buildtool/CMakeLists.txt new file mode 100644 index 0000000..4e454e4 --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_ament_buildtool/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.8) +project(hector_transmission_interface_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} srv/AdjustTransmissionOffsets.srv + DEPENDENCIES sensor_msgs) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_missing_ament_buildtool/package.xml b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_ament_buildtool/package.xml new file mode 100644 index 0000000..18b8762 --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_ament_buildtool/package.xml @@ -0,0 +1,23 @@ + + + + hector_transmission_interface_msgs + 0.0.0 + TODO + + Aljoscha Schmidt + TODO + Aljoscha Schmidt + + rosidl_default_generators + + rosidl_default_generators + sensor_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + ament_cmake + + diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_missing_export/CMakeLists.txt b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_export/CMakeLists.txt new file mode 100644 index 0000000..4e454e4 --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_export/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.8) +project(hector_transmission_interface_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} srv/AdjustTransmissionOffsets.srv + DEPENDENCIES sensor_msgs) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_missing_export/package.xml b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_export/package.xml new file mode 100644 index 0000000..8f4d68b --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_export/package.xml @@ -0,0 +1,21 @@ + + + + hector_transmission_interface_msgs + 0.0.0 + TODO + + Aljoscha Schmidt + TODO + Aljoscha Schmidt + + ament_cmake + rosidl_default_generators + + rosidl_default_generators + sensor_msgs + + rosidl_default_runtime + + rosidl_interface_packages + diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_missing_member_group/CMakeLists.txt b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_member_group/CMakeLists.txt new file mode 100644 index 0000000..4e454e4 --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_member_group/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.8) +project(hector_transmission_interface_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} srv/AdjustTransmissionOffsets.srv + DEPENDENCIES sensor_msgs) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_missing_member_group/package.xml b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_member_group/package.xml new file mode 100644 index 0000000..9fac631 --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_member_group/package.xml @@ -0,0 +1,23 @@ + + + + hector_transmission_interface_msgs + 0.0.0 + TODO + + Aljoscha Schmidt + TODO + Aljoscha Schmidt + + ament_cmake + rosidl_default_generators + + rosidl_default_generators + sensor_msgs + + rosidl_default_runtime + + + ament_cmake + + diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_missing_rosidl_buildtool/CMakeLists.txt b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_rosidl_buildtool/CMakeLists.txt new file mode 100644 index 0000000..4e454e4 --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_rosidl_buildtool/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.8) +project(hector_transmission_interface_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} srv/AdjustTransmissionOffsets.srv + DEPENDENCIES sensor_msgs) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_missing_rosidl_buildtool/package.xml b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_rosidl_buildtool/package.xml new file mode 100644 index 0000000..c37bd14 --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_missing_rosidl_buildtool/package.xml @@ -0,0 +1,23 @@ + + + + hector_transmission_interface_msgs + 0.0.0 + TODO + + Aljoscha Schmidt + TODO + Aljoscha Schmidt + + ament_cmake + + rosidl_default_generators + sensor_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + ament_cmake + + diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_no_buildtool_depend/CMakeLists.txt b/tests/examples/export_tag_examples/msg_pkg/pkg_no_buildtool_depend/CMakeLists.txt new file mode 100644 index 0000000..4e454e4 --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_no_buildtool_depend/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.8) +project(hector_transmission_interface_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} srv/AdjustTransmissionOffsets.srv + DEPENDENCIES sensor_msgs) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/tests/examples/export_tag_examples/msg_pkg/pkg_no_buildtool_depend/package.xml b/tests/examples/export_tag_examples/msg_pkg/pkg_no_buildtool_depend/package.xml new file mode 100644 index 0000000..cc8141f --- /dev/null +++ b/tests/examples/export_tag_examples/msg_pkg/pkg_no_buildtool_depend/package.xml @@ -0,0 +1,21 @@ + + + + hector_transmission_interface_msgs + 0.0.0 + TODO + + Aljoscha Schmidt + TODO + Aljoscha Schmidt + + rosidl_default_generators + sensor_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + ament_cmake + + diff --git a/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_correct/CMakeLists.txt b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_correct/CMakeLists.txt new file mode 100644 index 0000000..fc11ae2 --- /dev/null +++ b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_correct/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.8) +project(dynamixel_ros_control) + +# Default to C++17 +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif () + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(dynamixel_sdk REQUIRED) +find_package(yaml-cpp REQUIRED) +#find_package(dynamixel_ros_control_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(transmission_interface REQUIRED) +find_package(hector_transmission_interface REQUIRED) +find_package(hector_transmission_interface_msgs REQUIRED) +find_package(controller_orchestrator REQUIRED) + + +set(HEADERS + include/${PROJECT_NAME}/common.hpp + include/${PROJECT_NAME}/joint.hpp + include/${PROJECT_NAME}/dynamixel.hpp + include/${PROJECT_NAME}/control_table.hpp + include/${PROJECT_NAME}/control_table_item.hpp + include/${PROJECT_NAME}/dynamixel_driver.hpp + include/${PROJECT_NAME}/dynamixel_hardware_interface.hpp + include/${PROJECT_NAME}/sync_read_manager.hpp + include/${PROJECT_NAME}/sync_write_manager.hpp + include/${PROJECT_NAME}/log.hpp +) + +set(SOURCES + src/common.cpp + src/joint.cpp + src/dynamixel.cpp + src/control_table.cpp + src/control_table_item.cpp + src/dynamixel_driver.cpp + src/dynamixel_hardware_interface.cpp + src/sync_read_manager.cpp + src/sync_write_manager.cpp +) +include_directories(include) + +add_library(${PROJECT_NAME} SHARED ${SOURCES} ${HEADERS}) +target_link_libraries(${PROJECT_NAME} PUBLIC yaml-cpp) +ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp rclcpp_lifecycle lifecycle_msgs hardware_interface pluginlib + dynamixel_sdk yaml-cpp std_msgs std_srvs ament_index_cpp transmission_interface hector_transmission_interface hector_transmission_interface_msgs controller_orchestrator) + +pluginlib_export_plugin_description_file(hardware_interface dynamixel_ros_control.xml) + +# Install library and include folder +install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-targets LIBRARY DESTINATION lib) +install(DIRECTORY include/ DESTINATION include) + +# Install directories +install(DIRECTORY launch config devices + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_correct/launch/controller_manager.launch.yaml b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_correct/launch/controller_manager.launch.yaml new file mode 100644 index 0000000..12541f3 --- /dev/null +++ b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_correct/launch/controller_manager.launch.yaml @@ -0,0 +1,43 @@ +launch: +# Robot description +- include: + file: "$(find-pkg-share dynamixel_ros_control)/launch/load_description.launch.py" + +# Controller manager +- node: + pkg: "controller_manager" + exec: "ros2_control_node" + name: "controller_manager" + param: + - from: "$(find-pkg-share dynamixel_ros_control)/config/test_controllers.yaml" + allow_substs: true + - name: "hardware_components_initial_state.unconfigured" + value: ['hardware_interface'] +# Controller manager lifecycle management +- node: + pkg: "controller_manager" + exec: "hardware_spawner" + name: "hardware_interface_spawner" + output: "screen" + args: "hardware_interface --activate" + respawn: "true" + respawn_delay: 5.0 + +# Controllers +- node: + pkg: "controller_manager" + exec: "spawner" + name: "joint_state_broadcaster_spawner" + output: "screen" + args: "joint_state_broadcaster" + respawn: "true" + respawn_delay: 5.0 + +- node: + pkg: "controller_manager" + exec: "spawner" + name: "position_controller_spawner" + output: "screen" + args: "position_controller" + param: + - from: "$(find-pkg-share dynamixel_ros_control)/config/test_controllers.yaml" diff --git a/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_correct/launch/load_description.launch.py b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_correct/launch/load_description.launch.py new file mode 100644 index 0000000..3d4ae5c --- /dev/null +++ b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_correct/launch/load_description.launch.py @@ -0,0 +1,72 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, PathJoinSubstitution, LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + ld = LaunchDescription() + + # Parameters + port_name_arg = DeclareLaunchArgument( + name="port_name", + default_value="/dev/ttyUSB0", + description="Path to USB serial converter device", + ) + ld.add_action(port_name_arg) + port_name = LaunchConfiguration("port_name") + + baud_rate_arg = DeclareLaunchArgument( + name="baud_rate", default_value="57600", description="Baud rate" + ) + ld.add_action(baud_rate_arg) + baud_rate = LaunchConfiguration("baud_rate") + + id_arg = DeclareLaunchArgument( + name="id", default_value="1", description="ID of the dynamixel to control" + ) + ld.add_action(id_arg) + id_arg = LaunchConfiguration("id") + + # Get the package directory + urdf_file = PathJoinSubstitution( + [ + get_package_share_directory("dynamixel_ros_control"), + "config", + "test_robot.urdf.xacro", + ] + ) + + robot_description = ParameterValue( + Command( + [ + "xacro ", + urdf_file, + " port_name:=", + port_name, + " baud_rate:=", + baud_rate, + " id:=", + id_arg, + ] + ), + value_type=str, + ) + + # robot state publisher + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[ + { + "robot_description": robot_description, + } + ], + ) + ld.add_action(robot_state_publisher) + + return ld diff --git a/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_correct/package.xml b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_correct/package.xml new file mode 100644 index 0000000..958de5c --- /dev/null +++ b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_correct/package.xml @@ -0,0 +1,37 @@ + + + + dynamixel_ros_control + 0.0.0 + Provides a hardware interface for dynamixel motors. + Martin Oehler + MIT + Martin Oehler + + ament_cmake + + ament_index_cpp + controller_orchestrator + dynamixel_sdk + hardware_interface + hector_transmission_interface + hector_transmission_interface_msgs + lifecycle_msgs + pluginlib + rclcpp + rclcpp_lifecycle + std_msgs + std_srvs + transmission_interface + yaml_cpp_vendor + + controller_manager + robot_state_publisher + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_missing_launch_deps/CMakeLists.txt b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_missing_launch_deps/CMakeLists.txt new file mode 100644 index 0000000..fc11ae2 --- /dev/null +++ b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_missing_launch_deps/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.8) +project(dynamixel_ros_control) + +# Default to C++17 +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif () + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(dynamixel_sdk REQUIRED) +find_package(yaml-cpp REQUIRED) +#find_package(dynamixel_ros_control_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(transmission_interface REQUIRED) +find_package(hector_transmission_interface REQUIRED) +find_package(hector_transmission_interface_msgs REQUIRED) +find_package(controller_orchestrator REQUIRED) + + +set(HEADERS + include/${PROJECT_NAME}/common.hpp + include/${PROJECT_NAME}/joint.hpp + include/${PROJECT_NAME}/dynamixel.hpp + include/${PROJECT_NAME}/control_table.hpp + include/${PROJECT_NAME}/control_table_item.hpp + include/${PROJECT_NAME}/dynamixel_driver.hpp + include/${PROJECT_NAME}/dynamixel_hardware_interface.hpp + include/${PROJECT_NAME}/sync_read_manager.hpp + include/${PROJECT_NAME}/sync_write_manager.hpp + include/${PROJECT_NAME}/log.hpp +) + +set(SOURCES + src/common.cpp + src/joint.cpp + src/dynamixel.cpp + src/control_table.cpp + src/control_table_item.cpp + src/dynamixel_driver.cpp + src/dynamixel_hardware_interface.cpp + src/sync_read_manager.cpp + src/sync_write_manager.cpp +) +include_directories(include) + +add_library(${PROJECT_NAME} SHARED ${SOURCES} ${HEADERS}) +target_link_libraries(${PROJECT_NAME} PUBLIC yaml-cpp) +ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp rclcpp_lifecycle lifecycle_msgs hardware_interface pluginlib + dynamixel_sdk yaml-cpp std_msgs std_srvs ament_index_cpp transmission_interface hector_transmission_interface hector_transmission_interface_msgs controller_orchestrator) + +pluginlib_export_plugin_description_file(hardware_interface dynamixel_ros_control.xml) + +# Install library and include folder +install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-targets LIBRARY DESTINATION lib) +install(DIRECTORY include/ DESTINATION include) + +# Install directories +install(DIRECTORY launch config devices + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_missing_launch_deps/launch/controller_manager.launch.yaml b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_missing_launch_deps/launch/controller_manager.launch.yaml new file mode 100644 index 0000000..12541f3 --- /dev/null +++ b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_missing_launch_deps/launch/controller_manager.launch.yaml @@ -0,0 +1,43 @@ +launch: +# Robot description +- include: + file: "$(find-pkg-share dynamixel_ros_control)/launch/load_description.launch.py" + +# Controller manager +- node: + pkg: "controller_manager" + exec: "ros2_control_node" + name: "controller_manager" + param: + - from: "$(find-pkg-share dynamixel_ros_control)/config/test_controllers.yaml" + allow_substs: true + - name: "hardware_components_initial_state.unconfigured" + value: ['hardware_interface'] +# Controller manager lifecycle management +- node: + pkg: "controller_manager" + exec: "hardware_spawner" + name: "hardware_interface_spawner" + output: "screen" + args: "hardware_interface --activate" + respawn: "true" + respawn_delay: 5.0 + +# Controllers +- node: + pkg: "controller_manager" + exec: "spawner" + name: "joint_state_broadcaster_spawner" + output: "screen" + args: "joint_state_broadcaster" + respawn: "true" + respawn_delay: 5.0 + +- node: + pkg: "controller_manager" + exec: "spawner" + name: "position_controller_spawner" + output: "screen" + args: "position_controller" + param: + - from: "$(find-pkg-share dynamixel_ros_control)/config/test_controllers.yaml" diff --git a/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_missing_launch_deps/launch/load_description.launch.py b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_missing_launch_deps/launch/load_description.launch.py new file mode 100644 index 0000000..3d4ae5c --- /dev/null +++ b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_missing_launch_deps/launch/load_description.launch.py @@ -0,0 +1,72 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, PathJoinSubstitution, LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + ld = LaunchDescription() + + # Parameters + port_name_arg = DeclareLaunchArgument( + name="port_name", + default_value="/dev/ttyUSB0", + description="Path to USB serial converter device", + ) + ld.add_action(port_name_arg) + port_name = LaunchConfiguration("port_name") + + baud_rate_arg = DeclareLaunchArgument( + name="baud_rate", default_value="57600", description="Baud rate" + ) + ld.add_action(baud_rate_arg) + baud_rate = LaunchConfiguration("baud_rate") + + id_arg = DeclareLaunchArgument( + name="id", default_value="1", description="ID of the dynamixel to control" + ) + ld.add_action(id_arg) + id_arg = LaunchConfiguration("id") + + # Get the package directory + urdf_file = PathJoinSubstitution( + [ + get_package_share_directory("dynamixel_ros_control"), + "config", + "test_robot.urdf.xacro", + ] + ) + + robot_description = ParameterValue( + Command( + [ + "xacro ", + urdf_file, + " port_name:=", + port_name, + " baud_rate:=", + baud_rate, + " id:=", + id_arg, + ] + ), + value_type=str, + ) + + # robot state publisher + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[ + { + "robot_description": robot_description, + } + ], + ) + ld.add_action(robot_state_publisher) + + return ld diff --git a/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_missing_launch_deps/package.xml b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_missing_launch_deps/package.xml new file mode 100644 index 0000000..af85b85 --- /dev/null +++ b/tests/examples/launch_pkg_examples/dynamixel_ros_control/pkg_missing_launch_deps/package.xml @@ -0,0 +1,34 @@ + + + + dynamixel_ros_control + 0.0.0 + Provides a hardware interface for dynamixel motors. + Martin Oehler + MIT + Martin Oehler + + ament_cmake + + ament_index_cpp + controller_orchestrator + dynamixel_sdk + hardware_interface + hector_transmission_interface + hector_transmission_interface_msgs + lifecycle_msgs + pluginlib + rclcpp + rclcpp_lifecycle + std_msgs + std_srvs + transmission_interface + yaml_cpp_vendor + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/CMakeLists.txt b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/CMakeLists.txt new file mode 100644 index 0000000..c8d94f8 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.8) +project(hector_controller_spawner) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(controller_manager_msgs REQUIRED) + +add_executable(hector_controller_spawner src/hector_controller_spawner.cpp) + +target_include_directories( + hector_controller_spawner + PUBLIC $ + $) + +ament_target_dependencies(hector_controller_spawner rclcpp std_msgs + controller_manager_msgs) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + # for ROS 2 launch‐testing + find_package(launch_testing_ament_cmake REQUIRED) + # This will install and run your test/controller_spawner_launch.py + add_launch_test(test/test_spawner_basic.test.py TIMEOUT 360) + add_launch_test(test/test_spawner_twice.test.py TIMEOUT 360) + add_launch_test(test/test_spawner_estop.test.py TIMEOUT 360) + add_launch_test(test/test_spawner_chain_detection.test.py TIMEOUT 360) +endif() + +install(TARGETS hector_controller_spawner DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY config launch test + DESTINATION share/${PROJECT_NAME} + OPTIONAL) + +ament_package() diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/config/athena.yaml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/config/athena.yaml new file mode 100644 index 0000000..73d30eb --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/config/athena.yaml @@ -0,0 +1,45 @@ +/**: + # Example configuration !! + ros__parameters: + # Optional emergency‑stop topic. Empty → start immediately. + estop_topic: "" + + # How long to wait between retry attempts (HW + controllers). + retry_delay: 5.0 + + # ---------- hardware interfaces (activate = always) ---------- + hardware_interfaces: + - "athena_flipper_interface" + - "athena_arm_interface" + + # ---------- controllers -------------------------------------- + controllers: + - joint_state_broadcaster + - flipper_trajectory_controller + - self_collision_avoidance_controller + - flipper_velocity_controller + - gripper_trajectory_controller + - arm_trajectory_controller + - moveit_twist_controller + + # Per‑controller options + joint_state_broadcaster: + activate: true + + flipper_trajectory_controller: + activate: false + + self_collision_avoidance_controller: + activate: true + + flipper_velocity_controller: + activate: true + + gripper_trajectory_controller: + activate: true + + arm_trajectory_controller: + activate: true + + moveit_twist_controller: + activate: false diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/launch/hector_controller_spawner_launch.yml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/launch/hector_controller_spawner_launch.yml new file mode 100644 index 0000000..1190c83 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/launch/hector_controller_spawner_launch.yml @@ -0,0 +1,8 @@ +launch: + - node: + pkg: hector_controller_spawner + exec: hector_controller_spawner + name: multi_controller_spawner + output: screen + param: + - from: "$(find-pkg-share hector_controller_spawner)/config/athena.yaml" diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/package.xml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/package.xml new file mode 100644 index 0000000..8580eec --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/package.xml @@ -0,0 +1,30 @@ + + + + hector_controller_spawner + 0.0.0 + Robust hardware interface and controller spawning + + Aljoscha Schmidt + TODO + Aljoscha Schmidt + + ament_cmake + + controller_manager_msgs + rclcpp + std_msgs + + ament_lint_auto + controller_manager + hector_ros_controllers + joint_state_broadcaster + joint_trajectory_controller + launch_testing + launch_testing_ament_cmake + robot_state_publisher + + + ament_cmake + + diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/athena.urdf b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/athena.urdf new file mode 100644 index 0000000..35ad6e0 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/athena.urdf @@ -0,0 +1,2110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + " + + 0 0 0 0 0 0 + /athena/front_lidar_sensor + 10 + front_lidar_sensor_laser_frame + + + + 100 + 1 + 0 + 6.28318 + + + 360 + 1 + -0.12601266555555554 + 0.9637699988888888 + + + + 0.08 + 10.0 + 0.01 + + + 1 + True + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + " + + 0 0 0 0 0 0 + /athena/back_lidar_sensor + 10 + back_lidar_sensor_laser_frame + + + + 100 + 1 + 0 + 6.28318 + + + 360 + 1 + -0.12601266555555554 + 0.9637699988888888 + + + + 0.08 + 10.0 + 0.01 + + + 1 + True + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + athena/front_wideangle_camera/image_raw + front_wideangle_camera_optical_frame + + athena/front_wideangle_camera/camera_info + 4.188786666666666 + + R8G8B8 + 3840 + 3032 + + + 0.01 + 100 + + + equidistant + 2.094393333333333 + true + 512 + + + + + + + + + + + + + + + + + + + + + + 1 + athena/back_wideangle_camera/image_raw + back_wideangle_camera_optical_frame + + athena/back_wideangle_camera/camera_info + 4.188786666666666 + + R8G8B8 + 3840 + 3032 + + + 0.01 + 100 + + + equidistant + 2.094393333333333 + true + 512 + + + + + + + + true + true + 15 + /athena/front_rgbd/rgb/image_raw + front_rgbd_rgb_link + + /athena/front_rgbd/rgb/camera_info + front_rgbd_rgb_optical_frame + 1.570795 + + R8G8B8 + 720 + 480 + + + 0.05 + 10 + + + + + true + true + 15 + /athena/front_rgbd/depth/image_raw + front_rgbd_depth_link + + /athena/front_rgbd/depth/camera_info + front_rgbd_depth_optical_frame + 1.047 + + R_FLOAT32 + 640 + 480 + + + + 0.1 + 10.0 + + + + gaussian + 0.0 + 0.01 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + true + 15 + /athena/back_rgbd/rgb/image_raw + back_rgbd_rgb_link + + /athena/back_rgbd/rgb/camera_info + back_rgbd_rgb_optical_frame + 1.570795 + + R8G8B8 + 720 + 480 + + + 0.05 + 10 + + + + + true + true + 15 + /athena/back_rgbd/depth/image_raw + back_rgbd_depth_link + + /athena/back_rgbd/depth/camera_info + back_rgbd_depth_optical_frame + 1.047 + + R_FLOAT32 + 640 + 480 + + + + 0.1 + 10.0 + + + + gaussian + 0.0 + 0.01 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + + + + + + + + + + + + + + + + 30 + athena/gripper_cam/image_raw + gripper_cam_optical_frame + + athena/gripper_cam/camera_info + 1.085594794740473 + + R8G8B8 + 1640 + 1232 + + + 0.01 + 100 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gripper_servo_joint + + gripper_joint_r2 + 0.909 + 0.0 + + + + gripper_joint_l2 + 0.909 + + + + gripper_joint_r1 + + + + gripper_joint_l1 + + + + + + + + + + + + + + + + + + + + + 30.0 + true + true + gripper_joint_link_l2 + /athena/force_torque_sensor/finger_l/wrench + + child + child_to_parent + + + + + + + + 30.0 + true + true + gripper_joint_link_r2 + /athena/force_torque_sensor/finger_r/wrench + + child + child_to_parent + + + + + + + + mock_components/GenericSystem + true + false + false + false + 0.0 + + + 1 + 9.0 + 255 + + + + + + + + + hector_transmission_interface/AdjustableOffsetTransmission + + + -2.0 + 0.684 + + + + 2 + 9.0 + 255 + + + + + + + + + hector_transmission_interface/AdjustableOffsetTransmission + + + 2.0 + -1.177 + + + + 3 + 9.0 + 255 + + + + + + + + + hector_transmission_interface/AdjustableOffsetTransmission + + + 2.0 + -0.949 + + + + 4 + 9.0 + 255 + + + + + + + + + hector_transmission_interface/AdjustableOffsetTransmission + + + -2.0 + -0.77 + + + + + + + + mock_components/GenericSystem + true + false + false + false + 0.0 + + + + + + 0.0 + + + + + + + + + 0.16 + + + + + + + + + -0.06 + + + + + + + + + 0.0 + + + + + + + + + -1.43815 + + + + + + + + + 0.0 + + + + + + + + + 0.0 + + + + + + + + + /home/aljoscha-schmidt/hector/install/athena_description/share/athena_description/config/athena_controllers.yaml + + athena + + /tf:=/athena/tf + /tf_static:=/athena/tf_static + + + + 20.0 + base_link + athena/odom + athena/odom_covariance + 3 + odom + athena/tf + 0.0 + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + Gazebo/Black + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + Gazebo/Black + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + Gazebo/Black + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + Gazebo/Black + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + Gazebo/Black + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + Gazebo/Black + + + true + + + true + + + + + + main_track_left_link + + + flipper_fl_link + + + flipper_bl_link + + + main_track_right_link + + + flipper_fr_link + + + flipper_br_link + + 0.06 + 0.215 + 0.5 + + -1.5 + 1.5 + + + + -3.0 + 3.0 + + + 50 + athena/cmd_vel_out + athena/steering_efficiency + athena/odom_wheel + + odom + base_link + + + main_track_left_link + + -1.5 + 1.5 + + + + flipper_fl_link + + -1.5 + 1.5 + + + + flipper_bl_link + 3.141592653589793 0 0 + -1.5 + 1.5 + + + + main_track_right_link + + -1.5 + 1.5 + + + + flipper_fr_link + + -1.5 + 1.5 + + + + flipper_br_link + 3.141592653589793 0 0 + -1.5 + 1.5 + + + + + Gazebo/DarkGray + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + 0.2 + 0.2 + Gazebo/DarkGrey + + diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/controller_spawner.yaml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/controller_spawner.yaml new file mode 100644 index 0000000..09cb989 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/controller_spawner.yaml @@ -0,0 +1,40 @@ +/**: + ros__parameters: + # Optional emergency‑stop topic. Empty → start immediately. + #estop_topic: "estop_board/hard_estop" + + # How long to wait between retry attempts (HW + controllers). + retry_delay: 5.0 + + # ---------- hardware interfaces (activate = always) ---------- + hardware_interfaces: + - "athena_flipper_interface" + - "athena_arm_interface" + + # ---------- controllers -------------------------------------- + controllers: + - joint_state_broadcaster + - flipper_trajectory_controller + - flipper_velocity_controller + - gripper_trajectory_controller + - arm_trajectory_controller + - vel_to_pos_controller + + # ---------- Per‑controller options --------------------------- + joint_state_broadcaster: + activate: true + + flipper_trajectory_controller: + activate: false + + flipper_velocity_controller: + activate: true + + vel_to_pos_controller: + activate: true + + gripper_trajectory_controller: + activate: true + + arm_trajectory_controller: + activate: true diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/controller_spawner_chain_detection.yaml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/controller_spawner_chain_detection.yaml new file mode 100644 index 0000000..ec0bb53 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/controller_spawner_chain_detection.yaml @@ -0,0 +1,40 @@ +/**: + ros__parameters: + # Optional emergency‑stop topic. Empty → start immediately. + #estop_topic: "estop_board/hard_estop" + + # How long to wait between retry attempts (HW + controllers). + retry_delay: 5.0 + + # ---------- hardware interfaces (activate = always) ---------- + hardware_interfaces: + - "athena_flipper_interface" + - "athena_arm_interface" + + # ---------- controllers -------------------------------------- + controllers: + - joint_state_broadcaster + - flipper_trajectory_controller + - flipper_velocity_controller + - gripper_trajectory_controller + - arm_trajectory_controller + - vel_to_pos_controller + + # ---------- Per‑controller options --------------------------- + joint_state_broadcaster: + activate: true + + flipper_trajectory_controller: + activate: false + + flipper_velocity_controller: + activate: true + + vel_to_pos_controller: + activate: false # In test must be activated + + gripper_trajectory_controller: + activate: true + + arm_trajectory_controller: + activate: true diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/controller_spawner_with_estop.yaml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/controller_spawner_with_estop.yaml new file mode 100644 index 0000000..3993455 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/controller_spawner_with_estop.yaml @@ -0,0 +1,40 @@ +/**: + ros__parameters: + # Optional emergency‑stop topic. Empty → start immediately. + estop_topic: "estop_board/hard_estop" + + # How long to wait between retry attempts (HW + controllers). + retry_delay: 5.0 + + # ---------- hardware interfaces (activate = always) ---------- + hardware_interfaces: + - "athena_flipper_interface" + - "athena_arm_interface" + + # ---------- controllers -------------------------------------- + controllers: + - joint_state_broadcaster + - flipper_trajectory_controller + - flipper_velocity_controller + - gripper_trajectory_controller + - arm_trajectory_controller + - vel_to_pos_controller + + # ---------- Per‑controller options --------------------------- + joint_state_broadcaster: + activate: true + + flipper_trajectory_controller: + activate: false + + flipper_velocity_controller: + activate: true + + vel_to_pos_controller: + activate: true + + gripper_trajectory_controller: + activate: true + + arm_trajectory_controller: + activate: true diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/controllers.yaml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/controllers.yaml new file mode 100644 index 0000000..962d282 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/config/controllers.yaml @@ -0,0 +1,95 @@ +# THIS PARAMETERS ARE ONLY USED IN THE SIMULATION !!! +# SEE athena_driver_launch/athena_driver_launch_config/configs/controllers.yaml FOR THE REAL ROBOT CONFIGURATION +/**: + gz_ros_control: + ros__parameters: + use_sim_time: true + + controller_manager: + ros__parameters: + update_rate: 50 # Hz + hardware_components_initial_state: + - unconfigured: [ athena_flipper_interface, athena_arm_interface ] + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + flipper_velocity_controller: + type: safety_forward_controller/SafetyForwardController + + flipper_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + arm_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + gripper_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + + vel_to_pos_controller: + type: velocity_to_position_command_controller/VelocityToPositionCommandController + + + vel_to_pos_controller: + ros__parameters: + joints: + - flipper_fl_joint + - flipper_fr_joint + - flipper_bl_joint + - flipper_br_joint + + e_stop_topic: estop_board/hard_estop + + flipper_velocity_controller: + ros__parameters: + joints: + - flipper_fl_joint + - flipper_fr_joint + - flipper_bl_joint + - flipper_br_joint + + passthrough_controller: vel_to_pos_controller + + interface_type: velocity + + flipper_trajectory_controller: + ros__parameters: + joints: + - flipper_fl_joint + - flipper_fr_joint + - flipper_bl_joint + - flipper_br_joint + + command_interfaces: + - position + state_interfaces: + - position + - velocity + + arm_trajectory_controller: + ros__parameters: + joints: + - arm_joint_1 + - arm_joint_2 + - arm_joint_3 + - arm_joint_4 + - arm_joint_5 + - arm_joint_6 + + command_interfaces: + - position + state_interfaces: + - position + - velocity + + gripper_trajectory_controller: + ros__parameters: + joints: + - gripper_servo_joint + + command_interfaces: + - position + state_interfaces: + - position + - velocity diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_controller_spawner.cpp b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_controller_spawner.cpp new file mode 100644 index 0000000..e69de29 diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_spawner_basic.test.py b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_spawner_basic.test.py new file mode 100644 index 0000000..ff69276 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_spawner_basic.test.py @@ -0,0 +1,139 @@ +import os +import unittest +from ament_index_python.packages import get_package_share_directory +import launch +import launch.actions +import launch_ros.actions +import launch_testing +import launch_testing.actions + +import rclpy +from rclpy.node import Node +from controller_manager_msgs.srv import ListControllers, ListHardwareComponents + + +def generate_test_description(): + pkg_share = get_package_share_directory("hector_controller_spawner") + + controller_config = os.path.join(pkg_share, "test", "config", "controllers.yaml") + spawner_config = os.path.join( + pkg_share, "test", "config", "controller_spawner.yaml" + ) + robot_description_file = os.path.join(pkg_share, "test", "config", "athena.urdf") + + for path in [controller_config, spawner_config, robot_description_file]: + if not os.path.isfile(path): + raise FileNotFoundError(f"Missing test file: {path}") + + with open(robot_description_file) as f: + robot_description = f.read() + + robot_state_publisher = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_description}], + ) + + controller_manager = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + output="screen", + parameters=[controller_config], + ) + + spawner_node = launch_ros.actions.Node( + package="hector_controller_spawner", + executable="hector_controller_spawner", + output="screen", + parameters=[spawner_config], + ) + + return ( + launch.LaunchDescription( + [ + robot_state_publisher, + controller_manager, + launch.actions.TimerAction(period=5.0, actions=[spawner_node]), + launch.actions.TimerAction( + period=10.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + {"controller_manager": controller_manager, "spawner_node": spawner_node}, + ) + + +class TestControllerSpawner(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = Node("test_controller_spawner") + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def test_hardware_interfaces_loaded(self): + client = self.node.create_client( + ListHardwareComponents, "/controller_manager/list_hardware_components" + ) + + self.assertTrue(client.wait_for_service(timeout_sec=10.0)) + + request = ListHardwareComponents.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) + + response = future.result() + self.assertIsNotNone(response) + + expected_interfaces = ["athena_flipper_interface", "athena_arm_interface"] + loaded_interfaces = [iface.name for iface in response.component] + + self.assertGreater(len(loaded_interfaces), 0, "No hardware interfaces loaded") + for iface in expected_interfaces: + self.assertIn(iface, loaded_interfaces) + + def test_controllers_loaded_and_activated(self): + client = self.node.create_client( + ListControllers, "/controller_manager/list_controllers" + ) + + self.assertTrue(client.wait_for_service(timeout_sec=10.0)) + + request = ListControllers.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) + + response = future.result() + self.assertIsNotNone(response) + + expected_active = [ + "joint_state_broadcaster", + "flipper_velocity_controller", + "gripper_trajectory_controller", + "arm_trajectory_controller", + "vel_to_pos_controller", + ] + expected_inactive = ["flipper_trajectory_controller"] + + loaded_controllers = {ctrl.name: ctrl.state for ctrl in response.controller} + self.node.get_logger().info(f"Loaded controllers: {loaded_controllers}") + + for controller in expected_active: + self.assertIn(controller, loaded_controllers) + self.assertEqual(loaded_controllers[controller], "active") + + for controller in expected_inactive: + self.assertIn(controller, loaded_controllers) + self.assertEqual(loaded_controllers[controller], "inactive") + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + # You can disable this assert if controller_manager fails to shutdown cleanly + self.assertEqual(proc_info["hector_controller_spawner"].returncode, 0) diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_spawner_chain_detection.test.py b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_spawner_chain_detection.test.py new file mode 100644 index 0000000..696b7f2 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_spawner_chain_detection.test.py @@ -0,0 +1,139 @@ +import os +import unittest +from ament_index_python.packages import get_package_share_directory +import launch +import launch.actions +import launch_ros.actions +import launch_testing +import launch_testing.actions + +import rclpy +from rclpy.node import Node +from controller_manager_msgs.srv import ListControllers, ListHardwareComponents + + +def generate_test_description(): + pkg_share = get_package_share_directory("hector_controller_spawner") + + controller_config = os.path.join(pkg_share, "test", "config", "controllers.yaml") + spawner_config = os.path.join( + pkg_share, "test", "config", "controller_spawner_chain_detection.yaml" + ) + robot_description_file = os.path.join(pkg_share, "test", "config", "athena.urdf") + + for path in [controller_config, spawner_config, robot_description_file]: + if not os.path.isfile(path): + raise FileNotFoundError(f"Missing test file: {path}") + + with open(robot_description_file) as f: + robot_description = f.read() + + robot_state_publisher = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_description}], + ) + + controller_manager = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + output="screen", + parameters=[controller_config], + ) + + spawner_node = launch_ros.actions.Node( + package="hector_controller_spawner", + executable="hector_controller_spawner", + output="screen", + parameters=[spawner_config], + ) + + return ( + launch.LaunchDescription( + [ + robot_state_publisher, + controller_manager, + launch.actions.TimerAction(period=5.0, actions=[spawner_node]), + launch.actions.TimerAction( + period=10.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + {"controller_manager": controller_manager, "spawner_node": spawner_node}, + ) + + +class TestControllerSpawner(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = Node("test_controller_spawner") + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def test_hardware_interfaces_loaded(self): + client = self.node.create_client( + ListHardwareComponents, "/controller_manager/list_hardware_components" + ) + + self.assertTrue(client.wait_for_service(timeout_sec=10.0)) + + request = ListHardwareComponents.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) + + response = future.result() + self.assertIsNotNone(response) + + expected_interfaces = ["athena_flipper_interface", "athena_arm_interface"] + loaded_interfaces = [iface.name for iface in response.component] + + self.assertGreater(len(loaded_interfaces), 0, "No hardware interfaces loaded") + for iface in expected_interfaces: + self.assertIn(iface, loaded_interfaces) + + def test_controllers_loaded_and_activated(self): + client = self.node.create_client( + ListControllers, "/controller_manager/list_controllers" + ) + + self.assertTrue(client.wait_for_service(timeout_sec=10.0)) + + request = ListControllers.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) + + response = future.result() + self.assertIsNotNone(response) + + expected_active = [ + "joint_state_broadcaster", + "flipper_velocity_controller", + "gripper_trajectory_controller", + "arm_trajectory_controller", + "vel_to_pos_controller", + ] + expected_inactive = ["flipper_trajectory_controller"] + + loaded_controllers = {ctrl.name: ctrl.state for ctrl in response.controller} + self.node.get_logger().info(f"Loaded controllers: {loaded_controllers}") + + for controller in expected_active: + self.assertIn(controller, loaded_controllers) + self.assertEqual(loaded_controllers[controller], "active") + + for controller in expected_inactive: + self.assertIn(controller, loaded_controllers) + self.assertEqual(loaded_controllers[controller], "inactive") + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + # You can disable this assert if controller_manager fails to shutdown cleanly + self.assertEqual(proc_info["hector_controller_spawner"].returncode, 0) diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_spawner_estop.test.py b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_spawner_estop.test.py new file mode 100644 index 0000000..ea4e993 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_spawner_estop.test.py @@ -0,0 +1,381 @@ +import os +import unittest +import time +from ament_index_python.packages import get_package_share_directory +import launch +import launch.actions +import launch_ros.actions +import launch_testing +import launch_testing.actions +from lifecycle_msgs.msg import State +import rclpy +from rclpy.node import Node +from std_msgs.msg import Bool +from controller_manager_msgs.srv import ( + ListControllers, + ListHardwareComponents, + SwitchController, + SetHardwareComponentState, +) +from rclpy.qos import QoSProfile, DurabilityPolicy + + +def generate_test_description(): + pkg_share = get_package_share_directory("hector_controller_spawner") + + controller_config = os.path.join(pkg_share, "test", "config", "controllers.yaml") + spawner_config = os.path.join( + pkg_share, "test", "config", "controller_spawner_with_estop.yaml" + ) + robot_description_file = os.path.join(pkg_share, "test", "config", "athena.urdf") + + for path in [controller_config, spawner_config, robot_description_file]: + if not os.path.isfile(path): + raise FileNotFoundError(f"Missing test file: {path}") + + with open(robot_description_file) as f: + robot_description = f.read() + + robot_state_publisher = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_description}], + ) + + controller_manager = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + output="screen", + parameters=[controller_config], + ) + + # Launch the spawner alongside the controller manager + spawner_node = launch_ros.actions.Node( + package="hector_controller_spawner", + executable="hector_controller_spawner", + output="screen", + parameters=[spawner_config], + ) + + return ( + launch.LaunchDescription( + [ + robot_state_publisher, + controller_manager, + launch.actions.TimerAction(period=3.0, actions=[spawner_node]), + launch.actions.TimerAction( + period=5.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + {"controller_manager": controller_manager, "spawner_node": spawner_node}, + ) + + +class TestEStopFunctionality(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = Node("test_estop_functionality") + + # Create e-stop publisher + estop_qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, # Match spawner's QoS + ) + cls.estop_pub = cls.node.create_publisher( + Bool, "estop_board/hard_estop", estop_qos + ) + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def test_complete_estop_workflow(self): + """Test complete e-stop workflow: block loading, allow loading, block again, allow again""" + + # Step 1: Send e-stop true (active/stopped) + self.node.get_logger().info("Step 1: Activating e-stop") + self._publish_estop(True) + time.sleep(2.0) # Allow time for e-stop to be processed + + # Step 2: Check that hardware interfaces and controllers are not active + self.node.get_logger().info( + "Step 2: Checking that hardware/controllers are not active" + ) + hardware_components = self._get_hardware_components() + controllers = self._get_controller_list() + + # Verify no hardware components are active + active_hardware = [ + hw.name for hw in hardware_components if hw.state.label == "active" + ] + self.assertEqual( + len(active_hardware), + 0, + "No hardware interfaces should be active while e-stop is active", + ) + + # Verify no controllers are active + active_controllers = [c.name for c in controllers if c.state == "active"] + self.assertEqual( + len(active_controllers), + 0, + "No controllers should be active while e-stop is active", + ) + + # Step 3: Send e-stop false (inactive/running) and wait + self.node.get_logger().info("Step 3: Releasing e-stop") + self._publish_estop(False) + time.sleep(8.0) # Allow time for spawner to proceed and load everything + + # Step 4: Check that controllers and hardware interfaces are loaded and active + self.node.get_logger().info( + "Step 4: Checking that hardware/controllers are active" + ) + hardware_components = self._get_hardware_components() + controllers = self._get_controller_list() + + # Verify expected hardware components are active + expected_hardware = ["athena_flipper_interface", "athena_arm_interface"] + active_hardware = [ + hw.name for hw in hardware_components if hw.state.label == "active" + ] + + for hw_name in expected_hardware: + self.assertIn( + hw_name, + active_hardware, + f"Hardware interface {hw_name} should be active after e-stop release", + ) + + # Verify expected controllers are active + expected_active_controllers = [ + "joint_state_broadcaster", + "flipper_velocity_controller", + "gripper_trajectory_controller", + "arm_trajectory_controller", + "vel_to_pos_controller", + ] + + active_controllers = [c.name for c in controllers if c.state == "active"] + for controller in expected_active_controllers: + self.assertIn( + controller, + active_controllers, + f"Controller {controller} should be active after e-stop release", + ) + + # Step 5: Send e-stop true and unload hardware interfaces + self.node.get_logger().info( + "Step 5: Activating e-stop again (should deactivate hardware)" + ) + self._publish_estop(True) + self._deactivate_all_controllers() + self._unload_hardware_interfaces() + time.sleep(3.0) # Allow time for deactivation + + # Step 6: Verify hardware interfaces are deactivated + hardware_components = self._get_hardware_components() + controllers = self._get_controller_list() + + # Check that hardware is no longer active + active_hardware_after_estop = [ + hw.name for hw in hardware_components if hw.state.label == "active" + ] + self.assertEqual( + len(active_hardware_after_estop), + 0, + "Hardware interfaces should be deactivated when e-stop is activated again", + ) + + # Check that controllers are no longer active + active_controllers_after_estop = [ + c.name for c in controllers if c.state == "active" + ] + self.assertEqual( + len(active_controllers_after_estop), + 0, + "Controllers should be deactivated when e-stop is activated again", + ) + + # Step 7: Send e-stop false and wait some time + self.node.get_logger().info("Step 6: Releasing e-stop second time") + self._publish_estop(False) + time.sleep(8.0) # Allow time for reactivation + + # Step 8: Check that hardware interfaces and requested controllers are active again + self.node.get_logger().info( + "Step 7: Checking final state - should be active again" + ) + final_hardware_components = self._get_hardware_components() + final_controllers = self._get_controller_list() + + # Verify hardware interfaces are active again + final_active_hardware = [ + hw.name for hw in final_hardware_components if hw.state.label == "active" + ] + for hw_name in expected_hardware: + self.assertIn( + hw_name, + final_active_hardware, + f"Hardware interface {hw_name} should be active again after second e-stop release", + ) + + # Verify controllers are active again + final_active_controllers = [ + c.name for c in final_controllers if c.state == "active" + ] + for controller in expected_active_controllers: + self.assertIn( + controller, + final_active_controllers, + f"Controller {controller} should be active again after second e-stop release", + ) + + self.node.get_logger().info("E-stop workflow test completed successfully") + + def _publish_estop(self, active): + """Helper to publish e-stop message multiple times for reliability""" + estop_msg = Bool() + estop_msg.data = active + + # Publish multiple times to ensure delivery + for _ in range(10): + self.estop_pub.publish(estop_msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(0.1) + + # Extra time for message propagation + time.sleep(0.5) + + def _get_hardware_components(self): + """Helper to get current hardware component list""" + client = self.node.create_client( + ListHardwareComponents, "/controller_manager/list_hardware_components" + ) + + if not client.wait_for_service(timeout_sec=3.0): + self.node.get_logger().warn("Hardware components service not available") + return [] + + request = ListHardwareComponents.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=5.0) + + response = future.result() + return response.component if response else [] + + def _get_controller_list(self): + """Helper to get current controller list""" + client = self.node.create_client( + ListControllers, "/controller_manager/list_controllers" + ) + + if not client.wait_for_service(timeout_sec=3.0): + self.node.get_logger().warn("Controller manager service not available") + return [] + + request = ListControllers.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=5.0) + + response = future.result() + return response.controller if response else [] + + def _unload_hardware_interfaces(self): + """Unload/deactivate hardware interfaces by transitioning them to inactive state""" + active_hardware = ["athena_flipper_interface", "athena_arm_interface"] + # Create service client for setting hardware component state + set_hw_state_client = self.node.create_client( + SetHardwareComponentState, + "/controller_manager/set_hardware_component_state", + ) + + if not set_hw_state_client.wait_for_service(timeout_sec=5.0): + self.node.get_logger().error( + "Hardware component state service not available" + ) + return + + # Deactivate each active hardware interface + for hw_component in active_hardware: + self.node.get_logger().info( + f"Deactivating hardware interface: {hw_component}" + ) + + request = SetHardwareComponentState.Request() + request.name = hw_component + request.target_state = State() + request.target_state.id = ( + State.PRIMARY_STATE_INACTIVE + ) # Transition to inactive + request.target_state.label = "inactive" + + # Call the service + future = set_hw_state_client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=5.0) + + response = future.result() + if response and response.ok: + self.node.get_logger().info( + f"Successfully deactivated hardware interface: {hw_component}" + ) + else: + self.node.get_logger().error( + f"Failed to deactivate hardware interface: {hw_component}" + ) + + def _deactivate_all_controllers(self): + """Deactivate all controllers by switching them off""" + + controllers_to_deactivate = [ + "joint_state_broadcaster", + "flipper_trajectory_controller", + "flipper_velocity_controller", + "gripper_trajectory_controller", + "arm_trajectory_controller", + "vel_to_pos_controller", + ] + + # Create service client for switching controllers + switch_client = self.node.create_client( + SwitchController, "/controller_manager/switch_controller" + ) + + if not switch_client.wait_for_service(timeout_sec=5.0): + self.node.get_logger().error("Switch controller service not available") + return + + self.node.get_logger().info("Deactivating all controllers") + + request = SwitchController.Request() + request.activate_controllers = [] # No controllers to activate + request.deactivate_controllers = ( + controllers_to_deactivate # Controllers to deactivate + ) + request.strictness = ( + SwitchController.Request.BEST_EFFORT + ) # Strict mode - fail if any controller can't be switched + request.activate_asap = False + request.timeout = rclpy.duration.Duration(seconds=5.0).to_msg() + + # Call the service + future = switch_client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) + + response = future.result() + if response and response.ok: + self.node.get_logger().info("Successfully deactivated all controllers") + else: + self.node.get_logger().error("Failed to deactivate controllers") + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + # Check that the spawner completed successfully + self.assertEqual(proc_info["hector_controller_spawner"].returncode, 0) diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_spawner_twice.test.py b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_spawner_twice.test.py new file mode 100644 index 0000000..1777561 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_correct/test/test_spawner_twice.test.py @@ -0,0 +1,235 @@ +import os +import unittest +import subprocess +import time +from ament_index_python.packages import get_package_share_directory +import launch +import launch.actions +import launch_ros.actions +import launch_testing +import launch_testing.actions + +import rclpy +from rclpy.node import Node +from controller_manager_msgs.srv import ListControllers + + +def generate_test_description(): + pkg_share = get_package_share_directory("hector_controller_spawner") + + controller_config = os.path.join(pkg_share, "test", "config", "controllers.yaml") + spawner_config = os.path.join( + pkg_share, "test", "config", "controller_spawner.yaml" + ) + robot_description_file = os.path.join(pkg_share, "test", "config", "athena.urdf") + + for path in [controller_config, spawner_config, robot_description_file]: + if not os.path.isfile(path): + raise FileNotFoundError(f"Missing test file: {path}") + + with open(robot_description_file) as f: + robot_description = f.read() + + robot_state_publisher = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_description}], + ) + + controller_manager = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + output="screen", + parameters=[controller_config], + ) + + # Don't launch spawner automatically - we'll control it manually + return ( + launch.LaunchDescription( + [ + robot_state_publisher, + controller_manager, + launch.actions.TimerAction( + period=5.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + {"controller_manager": controller_manager}, + ) + + +class TestControllerSpawnerIdempotency(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = Node("test_controller_spawner_idempotency") + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def test_spawner_run_twice_idempotent(self): + """Test that running the spawner twice doesn't cause problems""" + + pkg_share = get_package_share_directory("hector_controller_spawner") + spawner_config = os.path.join( + pkg_share, "test", "config", "controller_spawner.yaml" + ) + + # Spawner command + cmd = [ + "ros2", + "run", + "hector_controller_spawner", + "hector_controller_spawner", + "--ros-args", + "--params-file", + spawner_config, + ] + + # 1. Check initial state (no controllers loaded) + initial_controllers = self._get_controller_list() + initial_count = len(initial_controllers) + self.node.get_logger().info(f"Initial controller count: {initial_count}") + + # 2. Run spawner first time + self.node.get_logger().info("Running spawner first time...") + process1 = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + + # Wait for spawner to complete + return_code1 = process1.wait(timeout=30) # 30 second timeout + + stdout1, stderr1 = process1.communicate() + self.assertEqual( + return_code1, 0, f"First spawner run failed with return code {return_code1}" + ) + + # Give time for controllers to be loaded + time.sleep(2.0) + + # 3. Check state after first run + first_run_controllers = self._get_controller_list() + first_run_count = len(first_run_controllers) + self.node.get_logger().info( + f"Controller count after first run: {first_run_count}" + ) + + self.assertGreater( + first_run_count, + initial_count, + "Controllers should be loaded after first spawner run", + ) + + # Store the state for comparison + first_run_active = [ + c.name for c in first_run_controllers if c.state == "active" + ] + first_run_inactive = [ + c.name for c in first_run_controllers if c.state == "inactive" + ] + + # 4. Wait a bit, then run spawner second time + time.sleep(3.0) # Delay between runs + + self.node.get_logger().info("Running spawner second time...") + process2 = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + + # Wait for second spawner to complete + return_code2 = process2.wait(timeout=30) + + stdout2, stderr2 = process2.communicate() + self.assertEqual( + return_code2, + 0, + f"Second spawner run failed with return code {return_code2}", + ) + + # Give time for any changes to take effect + time.sleep(2.0) + + # 5. Check state after second run + second_run_controllers = self._get_controller_list() + second_run_count = len(second_run_controllers) + self.node.get_logger().info( + f"Controller count after second run: {second_run_count}" + ) + + second_run_active = [ + c.name for c in second_run_controllers if c.state == "active" + ] + second_run_inactive = [ + c.name for c in second_run_controllers if c.state == "inactive" + ] + + # 6. Verify that the state is identical (idempotent behavior) + self.assertEqual( + first_run_count, + second_run_count, + "Controller count should be the same after second spawner run", + ) + + # Check that active controllers are the same + self.assertEqual( + set(first_run_active), + set(second_run_active), + "Active controllers should be identical after second run", + ) + + # Check that inactive controllers are the same + self.assertEqual( + set(first_run_inactive), + set(second_run_inactive), + "Inactive controllers should be identical after second run", + ) + + # 7. Verify expected controllers are still in correct states + expected_active = [ + "joint_state_broadcaster", + "flipper_velocity_controller", + "gripper_trajectory_controller", + "arm_trajectory_controller", + "vel_to_pos_controller", + ] + + for controller in expected_active: + self.assertIn( + controller, + second_run_active, + f"Controller {controller} should still be active after second run", + ) + + # Log the outputs for debugging if needed + if stdout1: + self.node.get_logger().info( + f"First spawner stdout: {stdout1.decode()[:200]}..." + ) + if stdout2: + self.node.get_logger().info( + f"Second spawner stdout: {stdout2.decode()[:200]}..." + ) + + def _get_controller_list(self): + """Helper to get current controller list""" + client = self.node.create_client( + ListControllers, "/controller_manager/list_controllers" + ) + + if not client.wait_for_service(timeout_sec=5.0): + return [] + + request = ListControllers.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=5.0) + + response = future.result() + return response.controller if response else [] + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + # Controller manager should shutdown cleanly + pass # Allow flexible exit codes since we're not checking spawner here diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/CMakeLists.txt b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/CMakeLists.txt new file mode 100644 index 0000000..c8d94f8 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.8) +project(hector_controller_spawner) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(controller_manager_msgs REQUIRED) + +add_executable(hector_controller_spawner src/hector_controller_spawner.cpp) + +target_include_directories( + hector_controller_spawner + PUBLIC $ + $) + +ament_target_dependencies(hector_controller_spawner rclcpp std_msgs + controller_manager_msgs) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + # for ROS 2 launch‐testing + find_package(launch_testing_ament_cmake REQUIRED) + # This will install and run your test/controller_spawner_launch.py + add_launch_test(test/test_spawner_basic.test.py TIMEOUT 360) + add_launch_test(test/test_spawner_twice.test.py TIMEOUT 360) + add_launch_test(test/test_spawner_estop.test.py TIMEOUT 360) + add_launch_test(test/test_spawner_chain_detection.test.py TIMEOUT 360) +endif() + +install(TARGETS hector_controller_spawner DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY config launch test + DESTINATION share/${PROJECT_NAME} + OPTIONAL) + +ament_package() diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/config/athena.yaml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/config/athena.yaml new file mode 100644 index 0000000..73d30eb --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/config/athena.yaml @@ -0,0 +1,45 @@ +/**: + # Example configuration !! + ros__parameters: + # Optional emergency‑stop topic. Empty → start immediately. + estop_topic: "" + + # How long to wait between retry attempts (HW + controllers). + retry_delay: 5.0 + + # ---------- hardware interfaces (activate = always) ---------- + hardware_interfaces: + - "athena_flipper_interface" + - "athena_arm_interface" + + # ---------- controllers -------------------------------------- + controllers: + - joint_state_broadcaster + - flipper_trajectory_controller + - self_collision_avoidance_controller + - flipper_velocity_controller + - gripper_trajectory_controller + - arm_trajectory_controller + - moveit_twist_controller + + # Per‑controller options + joint_state_broadcaster: + activate: true + + flipper_trajectory_controller: + activate: false + + self_collision_avoidance_controller: + activate: true + + flipper_velocity_controller: + activate: true + + gripper_trajectory_controller: + activate: true + + arm_trajectory_controller: + activate: true + + moveit_twist_controller: + activate: false diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/launch/hector_controller_spawner_launch.yml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/launch/hector_controller_spawner_launch.yml new file mode 100644 index 0000000..1190c83 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/launch/hector_controller_spawner_launch.yml @@ -0,0 +1,8 @@ +launch: + - node: + pkg: hector_controller_spawner + exec: hector_controller_spawner + name: multi_controller_spawner + output: screen + param: + - from: "$(find-pkg-share hector_controller_spawner)/config/athena.yaml" diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/package.xml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/package.xml new file mode 100644 index 0000000..1364ece --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/package.xml @@ -0,0 +1,28 @@ + + + + hector_controller_spawner + 0.0.0 + Robust hardware interface and controller spawning + + Aljoscha Schmidt + TODO + Aljoscha Schmidt + + ament_cmake + + controller_manager_msgs + rclcpp + std_msgs + + ament_lint_auto + hector_ros_controllers + joint_state_broadcaster + joint_trajectory_controller + launch_testing + launch_testing_ament_cmake + + + ament_cmake + + diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/athena.urdf b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/athena.urdf new file mode 100644 index 0000000..35ad6e0 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/athena.urdf @@ -0,0 +1,2110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + " + + 0 0 0 0 0 0 + /athena/front_lidar_sensor + 10 + front_lidar_sensor_laser_frame + + + + 100 + 1 + 0 + 6.28318 + + + 360 + 1 + -0.12601266555555554 + 0.9637699988888888 + + + + 0.08 + 10.0 + 0.01 + + + 1 + True + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + " + + 0 0 0 0 0 0 + /athena/back_lidar_sensor + 10 + back_lidar_sensor_laser_frame + + + + 100 + 1 + 0 + 6.28318 + + + 360 + 1 + -0.12601266555555554 + 0.9637699988888888 + + + + 0.08 + 10.0 + 0.01 + + + 1 + True + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + athena/front_wideangle_camera/image_raw + front_wideangle_camera_optical_frame + + athena/front_wideangle_camera/camera_info + 4.188786666666666 + + R8G8B8 + 3840 + 3032 + + + 0.01 + 100 + + + equidistant + 2.094393333333333 + true + 512 + + + + + + + + + + + + + + + + + + + + + + 1 + athena/back_wideangle_camera/image_raw + back_wideangle_camera_optical_frame + + athena/back_wideangle_camera/camera_info + 4.188786666666666 + + R8G8B8 + 3840 + 3032 + + + 0.01 + 100 + + + equidistant + 2.094393333333333 + true + 512 + + + + + + + + true + true + 15 + /athena/front_rgbd/rgb/image_raw + front_rgbd_rgb_link + + /athena/front_rgbd/rgb/camera_info + front_rgbd_rgb_optical_frame + 1.570795 + + R8G8B8 + 720 + 480 + + + 0.05 + 10 + + + + + true + true + 15 + /athena/front_rgbd/depth/image_raw + front_rgbd_depth_link + + /athena/front_rgbd/depth/camera_info + front_rgbd_depth_optical_frame + 1.047 + + R_FLOAT32 + 640 + 480 + + + + 0.1 + 10.0 + + + + gaussian + 0.0 + 0.01 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + true + 15 + /athena/back_rgbd/rgb/image_raw + back_rgbd_rgb_link + + /athena/back_rgbd/rgb/camera_info + back_rgbd_rgb_optical_frame + 1.570795 + + R8G8B8 + 720 + 480 + + + 0.05 + 10 + + + + + true + true + 15 + /athena/back_rgbd/depth/image_raw + back_rgbd_depth_link + + /athena/back_rgbd/depth/camera_info + back_rgbd_depth_optical_frame + 1.047 + + R_FLOAT32 + 640 + 480 + + + + 0.1 + 10.0 + + + + gaussian + 0.0 + 0.01 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + + + + + + + + + + + + + + + + 30 + athena/gripper_cam/image_raw + gripper_cam_optical_frame + + athena/gripper_cam/camera_info + 1.085594794740473 + + R8G8B8 + 1640 + 1232 + + + 0.01 + 100 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gripper_servo_joint + + gripper_joint_r2 + 0.909 + 0.0 + + + + gripper_joint_l2 + 0.909 + + + + gripper_joint_r1 + + + + gripper_joint_l1 + + + + + + + + + + + + + + + + + + + + + 30.0 + true + true + gripper_joint_link_l2 + /athena/force_torque_sensor/finger_l/wrench + + child + child_to_parent + + + + + + + + 30.0 + true + true + gripper_joint_link_r2 + /athena/force_torque_sensor/finger_r/wrench + + child + child_to_parent + + + + + + + + mock_components/GenericSystem + true + false + false + false + 0.0 + + + 1 + 9.0 + 255 + + + + + + + + + hector_transmission_interface/AdjustableOffsetTransmission + + + -2.0 + 0.684 + + + + 2 + 9.0 + 255 + + + + + + + + + hector_transmission_interface/AdjustableOffsetTransmission + + + 2.0 + -1.177 + + + + 3 + 9.0 + 255 + + + + + + + + + hector_transmission_interface/AdjustableOffsetTransmission + + + 2.0 + -0.949 + + + + 4 + 9.0 + 255 + + + + + + + + + hector_transmission_interface/AdjustableOffsetTransmission + + + -2.0 + -0.77 + + + + + + + + mock_components/GenericSystem + true + false + false + false + 0.0 + + + + + + 0.0 + + + + + + + + + 0.16 + + + + + + + + + -0.06 + + + + + + + + + 0.0 + + + + + + + + + -1.43815 + + + + + + + + + 0.0 + + + + + + + + + 0.0 + + + + + + + + + /home/aljoscha-schmidt/hector/install/athena_description/share/athena_description/config/athena_controllers.yaml + + athena + + /tf:=/athena/tf + /tf_static:=/athena/tf_static + + + + 20.0 + base_link + athena/odom + athena/odom_covariance + 3 + odom + athena/tf + 0.0 + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + Gazebo/Black + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + Gazebo/Black + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + Gazebo/Black + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + Gazebo/Black + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + Gazebo/Black + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + + + + + 1.0 + 50 + + 0 + 0 + + + + + Gazebo/Black + + + true + + + true + + + + + + main_track_left_link + + + flipper_fl_link + + + flipper_bl_link + + + main_track_right_link + + + flipper_fr_link + + + flipper_br_link + + 0.06 + 0.215 + 0.5 + + -1.5 + 1.5 + + + + -3.0 + 3.0 + + + 50 + athena/cmd_vel_out + athena/steering_efficiency + athena/odom_wheel + + odom + base_link + + + main_track_left_link + + -1.5 + 1.5 + + + + flipper_fl_link + + -1.5 + 1.5 + + + + flipper_bl_link + 3.141592653589793 0 0 + -1.5 + 1.5 + + + + main_track_right_link + + -1.5 + 1.5 + + + + flipper_fr_link + + -1.5 + 1.5 + + + + flipper_br_link + 3.141592653589793 0 0 + -1.5 + 1.5 + + + + + Gazebo/DarkGray + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + 0.2 + 0.2 + Gazebo/DarkGrey + + diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/controller_spawner.yaml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/controller_spawner.yaml new file mode 100644 index 0000000..09cb989 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/controller_spawner.yaml @@ -0,0 +1,40 @@ +/**: + ros__parameters: + # Optional emergency‑stop topic. Empty → start immediately. + #estop_topic: "estop_board/hard_estop" + + # How long to wait between retry attempts (HW + controllers). + retry_delay: 5.0 + + # ---------- hardware interfaces (activate = always) ---------- + hardware_interfaces: + - "athena_flipper_interface" + - "athena_arm_interface" + + # ---------- controllers -------------------------------------- + controllers: + - joint_state_broadcaster + - flipper_trajectory_controller + - flipper_velocity_controller + - gripper_trajectory_controller + - arm_trajectory_controller + - vel_to_pos_controller + + # ---------- Per‑controller options --------------------------- + joint_state_broadcaster: + activate: true + + flipper_trajectory_controller: + activate: false + + flipper_velocity_controller: + activate: true + + vel_to_pos_controller: + activate: true + + gripper_trajectory_controller: + activate: true + + arm_trajectory_controller: + activate: true diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/controller_spawner_chain_detection.yaml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/controller_spawner_chain_detection.yaml new file mode 100644 index 0000000..ec0bb53 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/controller_spawner_chain_detection.yaml @@ -0,0 +1,40 @@ +/**: + ros__parameters: + # Optional emergency‑stop topic. Empty → start immediately. + #estop_topic: "estop_board/hard_estop" + + # How long to wait between retry attempts (HW + controllers). + retry_delay: 5.0 + + # ---------- hardware interfaces (activate = always) ---------- + hardware_interfaces: + - "athena_flipper_interface" + - "athena_arm_interface" + + # ---------- controllers -------------------------------------- + controllers: + - joint_state_broadcaster + - flipper_trajectory_controller + - flipper_velocity_controller + - gripper_trajectory_controller + - arm_trajectory_controller + - vel_to_pos_controller + + # ---------- Per‑controller options --------------------------- + joint_state_broadcaster: + activate: true + + flipper_trajectory_controller: + activate: false + + flipper_velocity_controller: + activate: true + + vel_to_pos_controller: + activate: false # In test must be activated + + gripper_trajectory_controller: + activate: true + + arm_trajectory_controller: + activate: true diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/controller_spawner_with_estop.yaml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/controller_spawner_with_estop.yaml new file mode 100644 index 0000000..3993455 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/controller_spawner_with_estop.yaml @@ -0,0 +1,40 @@ +/**: + ros__parameters: + # Optional emergency‑stop topic. Empty → start immediately. + estop_topic: "estop_board/hard_estop" + + # How long to wait between retry attempts (HW + controllers). + retry_delay: 5.0 + + # ---------- hardware interfaces (activate = always) ---------- + hardware_interfaces: + - "athena_flipper_interface" + - "athena_arm_interface" + + # ---------- controllers -------------------------------------- + controllers: + - joint_state_broadcaster + - flipper_trajectory_controller + - flipper_velocity_controller + - gripper_trajectory_controller + - arm_trajectory_controller + - vel_to_pos_controller + + # ---------- Per‑controller options --------------------------- + joint_state_broadcaster: + activate: true + + flipper_trajectory_controller: + activate: false + + flipper_velocity_controller: + activate: true + + vel_to_pos_controller: + activate: true + + gripper_trajectory_controller: + activate: true + + arm_trajectory_controller: + activate: true diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/controllers.yaml b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/controllers.yaml new file mode 100644 index 0000000..962d282 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/config/controllers.yaml @@ -0,0 +1,95 @@ +# THIS PARAMETERS ARE ONLY USED IN THE SIMULATION !!! +# SEE athena_driver_launch/athena_driver_launch_config/configs/controllers.yaml FOR THE REAL ROBOT CONFIGURATION +/**: + gz_ros_control: + ros__parameters: + use_sim_time: true + + controller_manager: + ros__parameters: + update_rate: 50 # Hz + hardware_components_initial_state: + - unconfigured: [ athena_flipper_interface, athena_arm_interface ] + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + flipper_velocity_controller: + type: safety_forward_controller/SafetyForwardController + + flipper_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + arm_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + gripper_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + + vel_to_pos_controller: + type: velocity_to_position_command_controller/VelocityToPositionCommandController + + + vel_to_pos_controller: + ros__parameters: + joints: + - flipper_fl_joint + - flipper_fr_joint + - flipper_bl_joint + - flipper_br_joint + + e_stop_topic: estop_board/hard_estop + + flipper_velocity_controller: + ros__parameters: + joints: + - flipper_fl_joint + - flipper_fr_joint + - flipper_bl_joint + - flipper_br_joint + + passthrough_controller: vel_to_pos_controller + + interface_type: velocity + + flipper_trajectory_controller: + ros__parameters: + joints: + - flipper_fl_joint + - flipper_fr_joint + - flipper_bl_joint + - flipper_br_joint + + command_interfaces: + - position + state_interfaces: + - position + - velocity + + arm_trajectory_controller: + ros__parameters: + joints: + - arm_joint_1 + - arm_joint_2 + - arm_joint_3 + - arm_joint_4 + - arm_joint_5 + - arm_joint_6 + + command_interfaces: + - position + state_interfaces: + - position + - velocity + + gripper_trajectory_controller: + ros__parameters: + joints: + - gripper_servo_joint + + command_interfaces: + - position + state_interfaces: + - position + - velocity diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_controller_spawner.cpp b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_controller_spawner.cpp new file mode 100644 index 0000000..e69de29 diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_spawner_basic.test.py b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_spawner_basic.test.py new file mode 100644 index 0000000..ff69276 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_spawner_basic.test.py @@ -0,0 +1,139 @@ +import os +import unittest +from ament_index_python.packages import get_package_share_directory +import launch +import launch.actions +import launch_ros.actions +import launch_testing +import launch_testing.actions + +import rclpy +from rclpy.node import Node +from controller_manager_msgs.srv import ListControllers, ListHardwareComponents + + +def generate_test_description(): + pkg_share = get_package_share_directory("hector_controller_spawner") + + controller_config = os.path.join(pkg_share, "test", "config", "controllers.yaml") + spawner_config = os.path.join( + pkg_share, "test", "config", "controller_spawner.yaml" + ) + robot_description_file = os.path.join(pkg_share, "test", "config", "athena.urdf") + + for path in [controller_config, spawner_config, robot_description_file]: + if not os.path.isfile(path): + raise FileNotFoundError(f"Missing test file: {path}") + + with open(robot_description_file) as f: + robot_description = f.read() + + robot_state_publisher = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_description}], + ) + + controller_manager = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + output="screen", + parameters=[controller_config], + ) + + spawner_node = launch_ros.actions.Node( + package="hector_controller_spawner", + executable="hector_controller_spawner", + output="screen", + parameters=[spawner_config], + ) + + return ( + launch.LaunchDescription( + [ + robot_state_publisher, + controller_manager, + launch.actions.TimerAction(period=5.0, actions=[spawner_node]), + launch.actions.TimerAction( + period=10.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + {"controller_manager": controller_manager, "spawner_node": spawner_node}, + ) + + +class TestControllerSpawner(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = Node("test_controller_spawner") + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def test_hardware_interfaces_loaded(self): + client = self.node.create_client( + ListHardwareComponents, "/controller_manager/list_hardware_components" + ) + + self.assertTrue(client.wait_for_service(timeout_sec=10.0)) + + request = ListHardwareComponents.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) + + response = future.result() + self.assertIsNotNone(response) + + expected_interfaces = ["athena_flipper_interface", "athena_arm_interface"] + loaded_interfaces = [iface.name for iface in response.component] + + self.assertGreater(len(loaded_interfaces), 0, "No hardware interfaces loaded") + for iface in expected_interfaces: + self.assertIn(iface, loaded_interfaces) + + def test_controllers_loaded_and_activated(self): + client = self.node.create_client( + ListControllers, "/controller_manager/list_controllers" + ) + + self.assertTrue(client.wait_for_service(timeout_sec=10.0)) + + request = ListControllers.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) + + response = future.result() + self.assertIsNotNone(response) + + expected_active = [ + "joint_state_broadcaster", + "flipper_velocity_controller", + "gripper_trajectory_controller", + "arm_trajectory_controller", + "vel_to_pos_controller", + ] + expected_inactive = ["flipper_trajectory_controller"] + + loaded_controllers = {ctrl.name: ctrl.state for ctrl in response.controller} + self.node.get_logger().info(f"Loaded controllers: {loaded_controllers}") + + for controller in expected_active: + self.assertIn(controller, loaded_controllers) + self.assertEqual(loaded_controllers[controller], "active") + + for controller in expected_inactive: + self.assertIn(controller, loaded_controllers) + self.assertEqual(loaded_controllers[controller], "inactive") + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + # You can disable this assert if controller_manager fails to shutdown cleanly + self.assertEqual(proc_info["hector_controller_spawner"].returncode, 0) diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_spawner_chain_detection.test.py b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_spawner_chain_detection.test.py new file mode 100644 index 0000000..696b7f2 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_spawner_chain_detection.test.py @@ -0,0 +1,139 @@ +import os +import unittest +from ament_index_python.packages import get_package_share_directory +import launch +import launch.actions +import launch_ros.actions +import launch_testing +import launch_testing.actions + +import rclpy +from rclpy.node import Node +from controller_manager_msgs.srv import ListControllers, ListHardwareComponents + + +def generate_test_description(): + pkg_share = get_package_share_directory("hector_controller_spawner") + + controller_config = os.path.join(pkg_share, "test", "config", "controllers.yaml") + spawner_config = os.path.join( + pkg_share, "test", "config", "controller_spawner_chain_detection.yaml" + ) + robot_description_file = os.path.join(pkg_share, "test", "config", "athena.urdf") + + for path in [controller_config, spawner_config, robot_description_file]: + if not os.path.isfile(path): + raise FileNotFoundError(f"Missing test file: {path}") + + with open(robot_description_file) as f: + robot_description = f.read() + + robot_state_publisher = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_description}], + ) + + controller_manager = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + output="screen", + parameters=[controller_config], + ) + + spawner_node = launch_ros.actions.Node( + package="hector_controller_spawner", + executable="hector_controller_spawner", + output="screen", + parameters=[spawner_config], + ) + + return ( + launch.LaunchDescription( + [ + robot_state_publisher, + controller_manager, + launch.actions.TimerAction(period=5.0, actions=[spawner_node]), + launch.actions.TimerAction( + period=10.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + {"controller_manager": controller_manager, "spawner_node": spawner_node}, + ) + + +class TestControllerSpawner(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = Node("test_controller_spawner") + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def test_hardware_interfaces_loaded(self): + client = self.node.create_client( + ListHardwareComponents, "/controller_manager/list_hardware_components" + ) + + self.assertTrue(client.wait_for_service(timeout_sec=10.0)) + + request = ListHardwareComponents.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) + + response = future.result() + self.assertIsNotNone(response) + + expected_interfaces = ["athena_flipper_interface", "athena_arm_interface"] + loaded_interfaces = [iface.name for iface in response.component] + + self.assertGreater(len(loaded_interfaces), 0, "No hardware interfaces loaded") + for iface in expected_interfaces: + self.assertIn(iface, loaded_interfaces) + + def test_controllers_loaded_and_activated(self): + client = self.node.create_client( + ListControllers, "/controller_manager/list_controllers" + ) + + self.assertTrue(client.wait_for_service(timeout_sec=10.0)) + + request = ListControllers.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) + + response = future.result() + self.assertIsNotNone(response) + + expected_active = [ + "joint_state_broadcaster", + "flipper_velocity_controller", + "gripper_trajectory_controller", + "arm_trajectory_controller", + "vel_to_pos_controller", + ] + expected_inactive = ["flipper_trajectory_controller"] + + loaded_controllers = {ctrl.name: ctrl.state for ctrl in response.controller} + self.node.get_logger().info(f"Loaded controllers: {loaded_controllers}") + + for controller in expected_active: + self.assertIn(controller, loaded_controllers) + self.assertEqual(loaded_controllers[controller], "active") + + for controller in expected_inactive: + self.assertIn(controller, loaded_controllers) + self.assertEqual(loaded_controllers[controller], "inactive") + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + # You can disable this assert if controller_manager fails to shutdown cleanly + self.assertEqual(proc_info["hector_controller_spawner"].returncode, 0) diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_spawner_estop.test.py b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_spawner_estop.test.py new file mode 100644 index 0000000..ea4e993 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_spawner_estop.test.py @@ -0,0 +1,381 @@ +import os +import unittest +import time +from ament_index_python.packages import get_package_share_directory +import launch +import launch.actions +import launch_ros.actions +import launch_testing +import launch_testing.actions +from lifecycle_msgs.msg import State +import rclpy +from rclpy.node import Node +from std_msgs.msg import Bool +from controller_manager_msgs.srv import ( + ListControllers, + ListHardwareComponents, + SwitchController, + SetHardwareComponentState, +) +from rclpy.qos import QoSProfile, DurabilityPolicy + + +def generate_test_description(): + pkg_share = get_package_share_directory("hector_controller_spawner") + + controller_config = os.path.join(pkg_share, "test", "config", "controllers.yaml") + spawner_config = os.path.join( + pkg_share, "test", "config", "controller_spawner_with_estop.yaml" + ) + robot_description_file = os.path.join(pkg_share, "test", "config", "athena.urdf") + + for path in [controller_config, spawner_config, robot_description_file]: + if not os.path.isfile(path): + raise FileNotFoundError(f"Missing test file: {path}") + + with open(robot_description_file) as f: + robot_description = f.read() + + robot_state_publisher = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_description}], + ) + + controller_manager = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + output="screen", + parameters=[controller_config], + ) + + # Launch the spawner alongside the controller manager + spawner_node = launch_ros.actions.Node( + package="hector_controller_spawner", + executable="hector_controller_spawner", + output="screen", + parameters=[spawner_config], + ) + + return ( + launch.LaunchDescription( + [ + robot_state_publisher, + controller_manager, + launch.actions.TimerAction(period=3.0, actions=[spawner_node]), + launch.actions.TimerAction( + period=5.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + {"controller_manager": controller_manager, "spawner_node": spawner_node}, + ) + + +class TestEStopFunctionality(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = Node("test_estop_functionality") + + # Create e-stop publisher + estop_qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, # Match spawner's QoS + ) + cls.estop_pub = cls.node.create_publisher( + Bool, "estop_board/hard_estop", estop_qos + ) + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def test_complete_estop_workflow(self): + """Test complete e-stop workflow: block loading, allow loading, block again, allow again""" + + # Step 1: Send e-stop true (active/stopped) + self.node.get_logger().info("Step 1: Activating e-stop") + self._publish_estop(True) + time.sleep(2.0) # Allow time for e-stop to be processed + + # Step 2: Check that hardware interfaces and controllers are not active + self.node.get_logger().info( + "Step 2: Checking that hardware/controllers are not active" + ) + hardware_components = self._get_hardware_components() + controllers = self._get_controller_list() + + # Verify no hardware components are active + active_hardware = [ + hw.name for hw in hardware_components if hw.state.label == "active" + ] + self.assertEqual( + len(active_hardware), + 0, + "No hardware interfaces should be active while e-stop is active", + ) + + # Verify no controllers are active + active_controllers = [c.name for c in controllers if c.state == "active"] + self.assertEqual( + len(active_controllers), + 0, + "No controllers should be active while e-stop is active", + ) + + # Step 3: Send e-stop false (inactive/running) and wait + self.node.get_logger().info("Step 3: Releasing e-stop") + self._publish_estop(False) + time.sleep(8.0) # Allow time for spawner to proceed and load everything + + # Step 4: Check that controllers and hardware interfaces are loaded and active + self.node.get_logger().info( + "Step 4: Checking that hardware/controllers are active" + ) + hardware_components = self._get_hardware_components() + controllers = self._get_controller_list() + + # Verify expected hardware components are active + expected_hardware = ["athena_flipper_interface", "athena_arm_interface"] + active_hardware = [ + hw.name for hw in hardware_components if hw.state.label == "active" + ] + + for hw_name in expected_hardware: + self.assertIn( + hw_name, + active_hardware, + f"Hardware interface {hw_name} should be active after e-stop release", + ) + + # Verify expected controllers are active + expected_active_controllers = [ + "joint_state_broadcaster", + "flipper_velocity_controller", + "gripper_trajectory_controller", + "arm_trajectory_controller", + "vel_to_pos_controller", + ] + + active_controllers = [c.name for c in controllers if c.state == "active"] + for controller in expected_active_controllers: + self.assertIn( + controller, + active_controllers, + f"Controller {controller} should be active after e-stop release", + ) + + # Step 5: Send e-stop true and unload hardware interfaces + self.node.get_logger().info( + "Step 5: Activating e-stop again (should deactivate hardware)" + ) + self._publish_estop(True) + self._deactivate_all_controllers() + self._unload_hardware_interfaces() + time.sleep(3.0) # Allow time for deactivation + + # Step 6: Verify hardware interfaces are deactivated + hardware_components = self._get_hardware_components() + controllers = self._get_controller_list() + + # Check that hardware is no longer active + active_hardware_after_estop = [ + hw.name for hw in hardware_components if hw.state.label == "active" + ] + self.assertEqual( + len(active_hardware_after_estop), + 0, + "Hardware interfaces should be deactivated when e-stop is activated again", + ) + + # Check that controllers are no longer active + active_controllers_after_estop = [ + c.name for c in controllers if c.state == "active" + ] + self.assertEqual( + len(active_controllers_after_estop), + 0, + "Controllers should be deactivated when e-stop is activated again", + ) + + # Step 7: Send e-stop false and wait some time + self.node.get_logger().info("Step 6: Releasing e-stop second time") + self._publish_estop(False) + time.sleep(8.0) # Allow time for reactivation + + # Step 8: Check that hardware interfaces and requested controllers are active again + self.node.get_logger().info( + "Step 7: Checking final state - should be active again" + ) + final_hardware_components = self._get_hardware_components() + final_controllers = self._get_controller_list() + + # Verify hardware interfaces are active again + final_active_hardware = [ + hw.name for hw in final_hardware_components if hw.state.label == "active" + ] + for hw_name in expected_hardware: + self.assertIn( + hw_name, + final_active_hardware, + f"Hardware interface {hw_name} should be active again after second e-stop release", + ) + + # Verify controllers are active again + final_active_controllers = [ + c.name for c in final_controllers if c.state == "active" + ] + for controller in expected_active_controllers: + self.assertIn( + controller, + final_active_controllers, + f"Controller {controller} should be active again after second e-stop release", + ) + + self.node.get_logger().info("E-stop workflow test completed successfully") + + def _publish_estop(self, active): + """Helper to publish e-stop message multiple times for reliability""" + estop_msg = Bool() + estop_msg.data = active + + # Publish multiple times to ensure delivery + for _ in range(10): + self.estop_pub.publish(estop_msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(0.1) + + # Extra time for message propagation + time.sleep(0.5) + + def _get_hardware_components(self): + """Helper to get current hardware component list""" + client = self.node.create_client( + ListHardwareComponents, "/controller_manager/list_hardware_components" + ) + + if not client.wait_for_service(timeout_sec=3.0): + self.node.get_logger().warn("Hardware components service not available") + return [] + + request = ListHardwareComponents.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=5.0) + + response = future.result() + return response.component if response else [] + + def _get_controller_list(self): + """Helper to get current controller list""" + client = self.node.create_client( + ListControllers, "/controller_manager/list_controllers" + ) + + if not client.wait_for_service(timeout_sec=3.0): + self.node.get_logger().warn("Controller manager service not available") + return [] + + request = ListControllers.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=5.0) + + response = future.result() + return response.controller if response else [] + + def _unload_hardware_interfaces(self): + """Unload/deactivate hardware interfaces by transitioning them to inactive state""" + active_hardware = ["athena_flipper_interface", "athena_arm_interface"] + # Create service client for setting hardware component state + set_hw_state_client = self.node.create_client( + SetHardwareComponentState, + "/controller_manager/set_hardware_component_state", + ) + + if not set_hw_state_client.wait_for_service(timeout_sec=5.0): + self.node.get_logger().error( + "Hardware component state service not available" + ) + return + + # Deactivate each active hardware interface + for hw_component in active_hardware: + self.node.get_logger().info( + f"Deactivating hardware interface: {hw_component}" + ) + + request = SetHardwareComponentState.Request() + request.name = hw_component + request.target_state = State() + request.target_state.id = ( + State.PRIMARY_STATE_INACTIVE + ) # Transition to inactive + request.target_state.label = "inactive" + + # Call the service + future = set_hw_state_client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=5.0) + + response = future.result() + if response and response.ok: + self.node.get_logger().info( + f"Successfully deactivated hardware interface: {hw_component}" + ) + else: + self.node.get_logger().error( + f"Failed to deactivate hardware interface: {hw_component}" + ) + + def _deactivate_all_controllers(self): + """Deactivate all controllers by switching them off""" + + controllers_to_deactivate = [ + "joint_state_broadcaster", + "flipper_trajectory_controller", + "flipper_velocity_controller", + "gripper_trajectory_controller", + "arm_trajectory_controller", + "vel_to_pos_controller", + ] + + # Create service client for switching controllers + switch_client = self.node.create_client( + SwitchController, "/controller_manager/switch_controller" + ) + + if not switch_client.wait_for_service(timeout_sec=5.0): + self.node.get_logger().error("Switch controller service not available") + return + + self.node.get_logger().info("Deactivating all controllers") + + request = SwitchController.Request() + request.activate_controllers = [] # No controllers to activate + request.deactivate_controllers = ( + controllers_to_deactivate # Controllers to deactivate + ) + request.strictness = ( + SwitchController.Request.BEST_EFFORT + ) # Strict mode - fail if any controller can't be switched + request.activate_asap = False + request.timeout = rclpy.duration.Duration(seconds=5.0).to_msg() + + # Call the service + future = switch_client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) + + response = future.result() + if response and response.ok: + self.node.get_logger().info("Successfully deactivated all controllers") + else: + self.node.get_logger().error("Failed to deactivate controllers") + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + # Check that the spawner completed successfully + self.assertEqual(proc_info["hector_controller_spawner"].returncode, 0) diff --git a/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_spawner_twice.test.py b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_spawner_twice.test.py new file mode 100644 index 0000000..1777561 --- /dev/null +++ b/tests/examples/launch_pkg_examples/hector_controller_spawner/pkg_missing_test_depends/test/test_spawner_twice.test.py @@ -0,0 +1,235 @@ +import os +import unittest +import subprocess +import time +from ament_index_python.packages import get_package_share_directory +import launch +import launch.actions +import launch_ros.actions +import launch_testing +import launch_testing.actions + +import rclpy +from rclpy.node import Node +from controller_manager_msgs.srv import ListControllers + + +def generate_test_description(): + pkg_share = get_package_share_directory("hector_controller_spawner") + + controller_config = os.path.join(pkg_share, "test", "config", "controllers.yaml") + spawner_config = os.path.join( + pkg_share, "test", "config", "controller_spawner.yaml" + ) + robot_description_file = os.path.join(pkg_share, "test", "config", "athena.urdf") + + for path in [controller_config, spawner_config, robot_description_file]: + if not os.path.isfile(path): + raise FileNotFoundError(f"Missing test file: {path}") + + with open(robot_description_file) as f: + robot_description = f.read() + + robot_state_publisher = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_description}], + ) + + controller_manager = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + output="screen", + parameters=[controller_config], + ) + + # Don't launch spawner automatically - we'll control it manually + return ( + launch.LaunchDescription( + [ + robot_state_publisher, + controller_manager, + launch.actions.TimerAction( + period=5.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + {"controller_manager": controller_manager}, + ) + + +class TestControllerSpawnerIdempotency(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = Node("test_controller_spawner_idempotency") + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def test_spawner_run_twice_idempotent(self): + """Test that running the spawner twice doesn't cause problems""" + + pkg_share = get_package_share_directory("hector_controller_spawner") + spawner_config = os.path.join( + pkg_share, "test", "config", "controller_spawner.yaml" + ) + + # Spawner command + cmd = [ + "ros2", + "run", + "hector_controller_spawner", + "hector_controller_spawner", + "--ros-args", + "--params-file", + spawner_config, + ] + + # 1. Check initial state (no controllers loaded) + initial_controllers = self._get_controller_list() + initial_count = len(initial_controllers) + self.node.get_logger().info(f"Initial controller count: {initial_count}") + + # 2. Run spawner first time + self.node.get_logger().info("Running spawner first time...") + process1 = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + + # Wait for spawner to complete + return_code1 = process1.wait(timeout=30) # 30 second timeout + + stdout1, stderr1 = process1.communicate() + self.assertEqual( + return_code1, 0, f"First spawner run failed with return code {return_code1}" + ) + + # Give time for controllers to be loaded + time.sleep(2.0) + + # 3. Check state after first run + first_run_controllers = self._get_controller_list() + first_run_count = len(first_run_controllers) + self.node.get_logger().info( + f"Controller count after first run: {first_run_count}" + ) + + self.assertGreater( + first_run_count, + initial_count, + "Controllers should be loaded after first spawner run", + ) + + # Store the state for comparison + first_run_active = [ + c.name for c in first_run_controllers if c.state == "active" + ] + first_run_inactive = [ + c.name for c in first_run_controllers if c.state == "inactive" + ] + + # 4. Wait a bit, then run spawner second time + time.sleep(3.0) # Delay between runs + + self.node.get_logger().info("Running spawner second time...") + process2 = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + + # Wait for second spawner to complete + return_code2 = process2.wait(timeout=30) + + stdout2, stderr2 = process2.communicate() + self.assertEqual( + return_code2, + 0, + f"Second spawner run failed with return code {return_code2}", + ) + + # Give time for any changes to take effect + time.sleep(2.0) + + # 5. Check state after second run + second_run_controllers = self._get_controller_list() + second_run_count = len(second_run_controllers) + self.node.get_logger().info( + f"Controller count after second run: {second_run_count}" + ) + + second_run_active = [ + c.name for c in second_run_controllers if c.state == "active" + ] + second_run_inactive = [ + c.name for c in second_run_controllers if c.state == "inactive" + ] + + # 6. Verify that the state is identical (idempotent behavior) + self.assertEqual( + first_run_count, + second_run_count, + "Controller count should be the same after second spawner run", + ) + + # Check that active controllers are the same + self.assertEqual( + set(first_run_active), + set(second_run_active), + "Active controllers should be identical after second run", + ) + + # Check that inactive controllers are the same + self.assertEqual( + set(first_run_inactive), + set(second_run_inactive), + "Inactive controllers should be identical after second run", + ) + + # 7. Verify expected controllers are still in correct states + expected_active = [ + "joint_state_broadcaster", + "flipper_velocity_controller", + "gripper_trajectory_controller", + "arm_trajectory_controller", + "vel_to_pos_controller", + ] + + for controller in expected_active: + self.assertIn( + controller, + second_run_active, + f"Controller {controller} should still be active after second run", + ) + + # Log the outputs for debugging if needed + if stdout1: + self.node.get_logger().info( + f"First spawner stdout: {stdout1.decode()[:200]}..." + ) + if stdout2: + self.node.get_logger().info( + f"Second spawner stdout: {stdout2.decode()[:200]}..." + ) + + def _get_controller_list(self): + """Helper to get current controller list""" + client = self.node.create_client( + ListControllers, "/controller_manager/list_controllers" + ) + + if not client.wait_for_service(timeout_sec=5.0): + return [] + + request = ListControllers.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=5.0) + + response = future.result() + return response.controller if response else [] + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_codes(self, proc_info): + # Controller manager should shutdown cleanly + pass # Allow flexible exit codes since we're not checking spawner here diff --git a/tests/examples/package_xml_examples/corrected_01_fail.xml b/tests/examples/package_xml_examples/corrected_01_fail.xml index 362b82a..2019cdb 100644 --- a/tests/examples/package_xml_examples/corrected_01_fail.xml +++ b/tests/examples/package_xml_examples/corrected_01_fail.xml @@ -4,16 +4,14 @@ srdfdom 2.0.7 Parser for Semantic Robot Description Format (SRDF). - MoveIt Release Team - BSD http://ros.org/wiki/srdfdom - https://github.com/ros-planning/srdfdom/issues https://github.com/ros-planning/srdfdom - - Ioan Sucan + https://github.com/ros-planning/srdfdom/issues Guillaume Walck + Ioan Sucan + ament_cmake ament_cmake_python @@ -36,6 +34,6 @@ ament_lint_cmake - ament_cmake + ament_cmake diff --git a/tests/examples/package_xml_examples/corrected_06_fail.xml b/tests/examples/package_xml_examples/corrected_06_fail.xml index b2a9fc7..5ec3a13 100644 --- a/tests/examples/package_xml_examples/corrected_06_fail.xml +++ b/tests/examples/package_xml_examples/corrected_06_fail.xml @@ -1,33 +1,32 @@ - gazebo_robot_sim_athena - 0.0.1 - - Gazebo Sim (Harmonic) simulation package for the Athena robot. - - todo - MIT - ament_cmake + gazebo_robot_sim_athena + 0.0.1 + Gazebo Sim (Harmonic) simulation package for the Athena robot. + todo + MIT - athena_description - athena_launch_config - gz_ros2_control - hector_gazebo_plugins - hector_gazebo_simulation - hector_launch_manager - hector_ros_controllers - launch - launch_ros - rclpy - robot_state_publisher - ros2_control - ros2_controllers - ros_gz_bridge - ros_gz_sim - srdf_publisher - xacro + ament_cmake - - ament_cmake - + athena_description + athena_launch_config + gz_ros2_control + hector_gazebo_plugins + hector_gazebo_simulation + hector_launch_manager + hector_ros_controllers + launch + launch_ros + rclpy + robot_state_publisher + ros2_control + ros2_controllers + ros_gz_bridge + ros_gz_sim + srdf_publisher + xacro + + + ament_cmake + diff --git a/tests/examples/package_xml_examples/corrected_07_fail.xml b/tests/examples/package_xml_examples/corrected_07_fail.xml index ce4f195..6d7a8dc 100644 --- a/tests/examples/package_xml_examples/corrected_07_fail.xml +++ b/tests/examples/package_xml_examples/corrected_07_fail.xml @@ -5,8 +5,8 @@ 0.0.0 Package for managing gamepad inputs TODO + TODO: License declaration - ament_cmake pluginlib diff --git a/tests/examples/package_xml_examples/corrected_13_fail.xml b/tests/examples/package_xml_examples/corrected_13_fail.xml new file mode 100644 index 0000000..0736082 --- /dev/null +++ b/tests/examples/package_xml_examples/corrected_13_fail.xml @@ -0,0 +1,28 @@ + + + moveit_state_server_msgs + 0.0.0 + Message and action definitions for MoveIt state server interaction. + Aljoscha Schmidt + + MIT + Aljoscha Schmidt + + ament_cmake + + rosidl_default_generators + + action_msgs + geometry_msgs + sensor_msgs + std_msgs + + rosidl_default_runtime + + + rosidl_interface_packages + + + ament_cmake + + diff --git a/tests/examples/package_xml_examples/original_02_correct.xml b/tests/examples/package_xml_examples/original_02_correct.xml index 362b82a..ae7d398 100644 --- a/tests/examples/package_xml_examples/original_02_correct.xml +++ b/tests/examples/package_xml_examples/original_02_correct.xml @@ -4,18 +4,16 @@ srdfdom 2.0.7 Parser for Semantic Robot Description Format (SRDF). - MoveIt Release Team - BSD http://ros.org/wiki/srdfdom - https://github.com/ros-planning/srdfdom/issues https://github.com/ros-planning/srdfdom - - Ioan Sucan + https://github.com/ros-planning/srdfdom/issues Guillaume Walck + Ioan Sucan + ament_cmake - ament_cmake_python + rosidl_default_generators console_bridge_vendor libboost-dev @@ -34,8 +32,9 @@ ament_cmake_pytest ament_lint_auto ament_lint_cmake + rosidl_interface_packages - ament_cmake + ament_cmake diff --git a/tests/examples/package_xml_examples/original_04_fail.xml b/tests/examples/package_xml_examples/original_04_fail.xml index b97c33e..be37fcc 100644 --- a/tests/examples/package_xml_examples/original_04_fail.xml +++ b/tests/examples/package_xml_examples/original_04_fail.xml @@ -9,8 +9,7 @@ ament_cmake - ament_lint_auto - ament_lint_common + ament_lint_autoament_lint_common pluginlib rclcpp diff --git a/tests/examples/package_xml_examples/original_07_fail.xml b/tests/examples/package_xml_examples/original_07_fail.xml index 3e2cea7..f045a6a 100644 --- a/tests/examples/package_xml_examples/original_07_fail.xml +++ b/tests/examples/package_xml_examples/original_07_fail.xml @@ -6,8 +6,8 @@ 0.0.0 TODO TODO: License declaration - - ament_cmake + +ament_cmake pluginlib rclcpp @@ -19,4 +19,4 @@ ament_cmake - + diff --git a/tests/examples/package_xml_examples/original_13_fail.xml b/tests/examples/package_xml_examples/original_13_fail.xml new file mode 100644 index 0000000..9aaf0ae --- /dev/null +++ b/tests/examples/package_xml_examples/original_13_fail.xml @@ -0,0 +1,26 @@ + + + moveit_state_server_msgs + 0.0.0 + Message and action definitions for MoveIt state server interaction. + + Aljoscha Schmidt + Aljoscha Schmidt + MIT + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + + std_msgs + geometry_msgs + sensor_msgs + action_msgs + + +rosidl_interface_packages + + ament_cmake + + diff --git a/tests/test_package_xml_linting_and_formatting.py b/tests/test_package_xml_linting_and_formatting.py index c2fc7ef..2eed0dc 100644 --- a/tests/test_package_xml_linting_and_formatting.py +++ b/tests/test_package_xml_linting_and_formatting.py @@ -3,7 +3,7 @@ import tempfile import shutil import subprocess -from lxml import etree as ET +import lxml.etree as ET from package_xml_validation.package_xml_validator import ( PackageXmlValidator, @@ -142,6 +142,9 @@ def test_xml_formatting(self): all_valid_check = formatter.check_and_format_files([original_path]) if self._is_correct_file(fname): + if not all_valid_check: + with open(original_path) as f_after: + print(f"File content after check:\n{f_after.read()}") self.assertTrue( all_valid_check, f"Expected correct file {fname} to pass in check-only mode.", @@ -216,7 +219,7 @@ def test_xml_formatting(self): comparison = self._compare_xml_files(final_path, corrected_path) if not comparison: # print corrected file to console - with open(final_path, "r") as f_corrected: + with open(final_path) as f_corrected: corrected_content = f_corrected.read() print(f"Corrected file content:\n{corrected_content}") self.assertTrue( diff --git a/tests/test_pkg_missing_launch_deps.py b/tests/test_pkg_missing_launch_deps.py new file mode 100644 index 0000000..ae935cf --- /dev/null +++ b/tests/test_pkg_missing_launch_deps.py @@ -0,0 +1,150 @@ +import os +import unittest +import tempfile +import shutil +import subprocess +import lxml.etree as ET +from package_xml_validation.package_xml_validator import ( + PackageXmlValidator, +) + + +def validate_xml_with_xmllint(xml_file): + """Validate XML file against the ROS package_format3.xsd schema using xmllint.""" + schema_url = "http://download.ros.org/schema/package_format3.xsd" + try: + result = subprocess.run( + ["xmllint", "--noout", "--schema", schema_url, xml_file], + capture_output=True, + text=True, + ) + if result.returncode != 0: + print(f"XML validation error in {xml_file}:\n{result.stderr}") + return False + return True + except FileNotFoundError: + print( + "Error: xmllint not found. Please ensure it's installed and in your PATH." + ) + return False + + +class TestPackageXmlValidator(unittest.TestCase): + @classmethod + def setUpClass(cls): + """ + We assume the example files are in 'tests/examples'. + Adjust the directory path to match your actual setup. + """ + current_dir = os.path.dirname(__file__) + cls.examples_dir = os.path.join(current_dir, "examples", "launch_pkg_examples") + cls.formatter = PackageXmlValidator( + check_only=False, + verbose=True, + auto_fill_missing_deps=True, + check_rosdeps=False, + ) + + def setUp(self): + """ + Create a temporary directory for each test, copy example files into it, + so we can safely modify them during tests. + """ + self.test_dir = tempfile.mkdtemp(prefix="xml_tests_") + + # copy contents of examples_dir to test_dir + shutil.copytree(self.examples_dir, self.test_dir, dirs_exist_ok=True) + + def tearDown(self): + """Clean up the temporary directory after each test.""" + shutil.rmtree(self.test_dir) + + def prettyprint(self, element, **kwargs): + xml = ET.tostring(element, pretty_print=True, **kwargs) + print(xml.decode(), end="") + + def _compare_xml_files(self, file1: str, file2: str) -> bool: + """ + Compare two XML files for equality. + Using xml parser to ignore whitespace and comments. + file1: is modified by the formatter + file2: is the expected corrected file + """ + try: + tree1 = ET.parse(file1) + tree2 = ET.parse(file2) + + # Normalize the XML trees + root1 = tree1.getroot() + root2 = tree2.getroot() + + for elem1, elem2 in zip(root1.iter(), root2.iter()): + # Ignore comments and whitespace + if ET.iselement(elem1) and ET.iselement(elem2): + if elem1.tag != elem2.tag or elem1.text != elem2.text: + return False + elif ET.iselement(elem1) and not ET.iselement(elem2): + return False + elif not ET.iselement(elem1) and ET.iselement(elem2): + return False + # make sure there are no more than 2 \n in the tai + elif elem2.tail.count("\n") > 2: + return False + return True + except ET.XMLSyntaxError as e: + print(f"XML Syntax Error: {e}") + return False + + def test_xml_formatting(self): + """ + Iterate over all example packages in the test directory, + """ + example_pkgs = os.listdir(self.examples_dir) + for example_pkg in example_pkgs: + correct_xml = os.path.join( + self.examples_dir, example_pkg, "pkg_correct", "package.xml" + ) + build_type_dir = os.path.join(self.test_dir, example_pkg) + for pkg in os.listdir(build_type_dir): + xml_file = os.path.join(build_type_dir, pkg, "package.xml") + + # Use subTest to continue testing other files even if this one fails + with self.subTest(example_pkg=example_pkg, pkg=pkg, xml_file=xml_file): + # apply the formatter + with open(xml_file) as f: + xml_content = f.read() + valid = self.formatter.check_and_format_files([xml_file]) + msg = "" + + if not pkg == "pkg_correct": + if valid: + with open(xml_file) as f: + msg = f"Formatted XML file {xml_file}:\n'{f.read()}'" + self.assertFalse( + valid, + f"XML file {xml_file} is expected to be invalid but was valid. {msg} \n vs original: \n{xml_content}", + ) + else: + if not valid: + with open(xml_file) as f: + msg = f"Invalid XML file {xml_file}:\n{f.read()}" + self.assertTrue( + valid, + f"XML file {xml_file} is expected to be valid but was invalid. {msg} \n vs original: \n{xml_content}", + ) + + self.assertTrue( + self._compare_xml_files(xml_file, correct_xml), + f"XML files do not match: {xml_file} != {correct_xml}", + ) + + # validate the XML file with xmllint + self.assertTrue( + validate_xml_with_xmllint(xml_file), + f"XML file {xml_file} failed xmllint validation.", + ) + + +if __name__ == "__main__": + # Run the tests + unittest.main() diff --git a/tests/test_export_tag_correction.py b/tests/test_pky_type_dependent_validation.py similarity index 98% rename from tests/test_export_tag_correction.py rename to tests/test_pky_type_dependent_validation.py index 7a3dbff..c419689 100644 --- a/tests/test_export_tag_correction.py +++ b/tests/test_pky_type_dependent_validation.py @@ -3,7 +3,7 @@ import tempfile import shutil import subprocess -from lxml import etree as ET +import lxml.etree as ET from package_xml_validation.package_xml_validator import ( PackageXmlValidator, @@ -96,7 +96,7 @@ def test_xml_formatting(self): """ Iterate over all example packages in the test directory, """ - build_types = ["ament_cmake"] # "ament_python" + build_types = ["ament_cmake", "msg_pkg"] # "ament_python" for build_type in build_types: correct_xml = os.path.join( self.examples_dir, build_type, "pkg_correct", "package.xml"