diff --git a/aip_urdf_compiler/CMakeLists.txt b/aip_urdf_compiler/CMakeLists.txt new file mode 100644 index 00000000..459dd61d --- /dev/null +++ b/aip_urdf_compiler/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_urdf_compiler) + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +# Install cmake directory +install( + DIRECTORY cmake templates scripts + DESTINATION share/${PROJECT_NAME} +) + +# Export the package's share directory path + +# Add the config extras +ament_package( + CONFIG_EXTRAS "cmake/aip_cmake_urdf_compile.cmake" +) diff --git a/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake b/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake new file mode 100644 index 00000000..2b6c2450 --- /dev/null +++ b/aip_urdf_compiler/cmake/aip_cmake_urdf_compile.cmake @@ -0,0 +1,33 @@ + + + +macro(aip_cmake_urdf_compile) + # Set the correct paths + find_package(PythonInterp REQUIRED) # cspell: ignore Interp + set(aip_urdf_compiler_BASE_DIR "${aip_urdf_compiler_DIR}/../") + set(PYTHON_SCRIPT "${aip_urdf_compiler_BASE_DIR}/scripts/compile_urdf.py") + set(PYTHON_TEMPLATE_DIRECTORY "${aip_urdf_compiler_BASE_DIR}/templates") + set(PYTHON_CALIBRATION_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/config") + set(PYTHON_XACRO_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/urdf") + + message(STATUS "PYTHON_SCRIPT path: ${PYTHON_SCRIPT}") + message(STATUS "PYTHON_TEMPLATE_DIRECTORY path: ${PYTHON_TEMPLATE_DIRECTORY}") + + # Verify that the required files exist + if(NOT EXISTS "${PYTHON_SCRIPT}") + message(FATAL_ERROR "Could not find compile_urdf.py at ${PYTHON_SCRIPT}") + endif() + + # Create a custom command to run the Python script + add_custom_target(xacro_compilation ALL + DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag + ) + + add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/python_script_run_flag + COMMAND ${PYTHON_EXECUTABLE} ${PYTHON_SCRIPT} ${PYTHON_TEMPLATE_DIRECTORY} ${PYTHON_CALIBRATION_DIRECTORY} ${PYTHON_XACRO_DIRECTORY} ${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + DEPENDS ${PYTHON_SCRIPT} + COMMENT "Running Python script for URDF creation" + ) +endmacro() diff --git a/aip_urdf_compiler/package.xml b/aip_urdf_compiler/package.xml new file mode 100644 index 00000000..c6355e42 --- /dev/null +++ b/aip_urdf_compiler/package.xml @@ -0,0 +1,17 @@ + + + aip_urdf_compiler + 0.1.0 + The aip_urdf_compiler package + + Yukihiro Saito + Yuxuan Liu + Apache 2 + + ament_cmake_auto + python3-jinja2 + + + ament_cmake + + diff --git a/aip_urdf_compiler/readme.md b/aip_urdf_compiler/readme.md new file mode 100644 index 00000000..24143490 --- /dev/null +++ b/aip_urdf_compiler/readme.md @@ -0,0 +1,142 @@ +# aip_urdf_compiler + +## Overview + +The aip_urdf_compiler package provides tools for dynamically generating URDF (Unified Robot Description Format) files from configuration files during the build process. It simplifies sensor model management by automatically URDF models from sensor configurations. + +## Key Features + +- Dynamic URDF generation during colcon build +- Automated sensor transform processing +- Support for multiple sensor types and configurations + +## Usage + +### Package Integration + +To use aip_urdf_compiler in your description package: + +- Add the dependency in `package.xml`: + + ```xml + aip_urdf_compiler + ``` + +- Add the following to `CMakeLists.txt`: + + ```cmake + find_package(aip_urdf_compiler REQUIRED) + aip_cmake_urdf_compile() + ``` + +- Configure your sensors in `config/sensors.yaml` with metadata values (Note: do not need to add meta values in `individual_params`): + + - `type`: Required string, corresponding to the string value from [existing sensors](#existing-sensors) + - `frame_id`: Optional string, overwrites the TF frame ID. + +- Clean up existing `.xacro` files and add to `.gitignore`: + + ```gitignore + # In your URDF folder + *.xacro + ``` + +### Existing Sensors + +```python +class LinkType(enum.Enum): + """Enum class for the type of the link.""" + + CAMERA = "monocular_camera" + IMU = "imu" + LIVOX = "livox_horizon" + PANDAR_40P = "pandar_40p" + PANDAR_OT128 = "pandar_ot128" + PANDAR_XT32 = "pandar_xt32" + PANDAR_QT = "pandar_qt" + PANDAR_QT128 = "pandar_qt128" + VELODYNE16 = "velodyne_16" + VLS128 = "velodyne_128" + RADAR = "radar" + GNSS = "gnss" + JOINT_UNITS = "units" +``` + +## Architecture + +### Components + +1. **aip_urdf_compiler** + + - Main package handling URDF generation + - Processes configuration files + - Manages build-time compilation + +2. **aip_cmake_urdf_compile** + + - CMake macro implementation + - Creates build targets + - Ensures URDF regeneration on each build + +3. **compile_urdf.py** + - Configuration parser + - Transform processor + - URDF generator + +### Compilation Process + +1. **Configuration Reading** + + - Parses `config/sensors.yaml` + - Extracts transformation data + - Validates configurations + +2. **Transform Processing** + + - Processes each sensor transform + - Determines sensor types and frame IDs + - Generates appropriate macro strings + - Creates `sensors.xacro` + +3. **Joint Unit Processing** + - Handles joint unit transforms + - Processes related YAML files + - Generates separate URDF xacro files + +## Adding New Sensors + +1. Add sensor descriptions (xacro module files) in either: + + - Your target package + - `common_sensor_description` package + +2. Update the following in `compile_urdf.py`: + - `LinkType` enumeration + - `link_dict` mapping + +## Troubleshooting + +### Debug Logs + +Check build logs for debugging information: + +```bash +cat $workspace/log/build_/aip_{project}_description/streams.log +``` + +### Common Issues + +1. Missing sensor definitions + + - Ensure sensor type is defined in `LinkType` + - Verify xacro file exists in description package + +2. TF Trees errors + - Check frame_id values in sensors.yaml + - Verify transform chain completeness + +## Contributing + +1. Follow ROS coding standards +2. Test URDF generation with various configurations +3. Update documentation for new features diff --git a/aip_urdf_compiler/scripts/compile_urdf.py b/aip_urdf_compiler/scripts/compile_urdf.py new file mode 100644 index 00000000..d43f9cf7 --- /dev/null +++ b/aip_urdf_compiler/scripts/compile_urdf.py @@ -0,0 +1,506 @@ +#!/usr/bin/python3 +""" +XACRO Compiler Script. + +This script compiles XACRO files for robot sensor configurations. It processes calibration data +from YAML files and generates XACRO files that define the sensor transforms and configurations +for a robot's URDF description. + +The script handles various types of sensors including cameras, IMUs, LiDARs (Velodyne, Pandar, Livox), +and radar units, generating appropriate XACRO macros for each sensor type. +""" + +import enum +import functools +import os +from typing import Callable +from typing import Dict +from typing import Union +import warnings + +from jinja2 import Template +import yaml + + +def load_yaml(file_path: str) -> Dict: + """ + Load and parse a YAML file. + + Args: + file_path (str): Path to the YAML file + + Returns: + Dict: Parsed YAML content or None if parsing fails + """ + try: + with open(file_path, "r") as stream: + content = yaml.safe_load(stream) + if content is None: + raise ValueError(f"YAML file is empty or invalid: {file_path}") + if not isinstance(content, dict): + raise ValueError(f"YAML file must contain a dictionary: {file_path}") + return content + + except FileNotFoundError: + raise FileNotFoundError(f"YAML file not found: {file_path}") + except yaml.YAMLError as exc: + raise yaml.YAMLError(f"Failed to parse YAML file {file_path}: {str(exc)}") + except Exception as e: # Add general exception handling + raise RuntimeError(f"Unexpected error reading YAML file {file_path}: {str(e)}") + + +class Transformation: + """ + Represents a coordinate transformation between two frames. + + Stores translation (x,y,z) and rotation (roll,pitch,yaw) parameters + along with frame information and sensor type. + """ + + def __init__(self, transformation: Dict, base_frame: str, child_frame: str): + """ + Initialize a transformation from a dictionary of parameters. + + Args: + transformation (Dict): Dictionary containing transformation parameters + base_frame (str): Name of the parent/base frame + child_frame (str): Name of the child frame + + Raises: + KeyError: If required transformation parameters are missing + """ + try: + self.x = transformation["x"] + self.y = transformation["y"] + self.z = transformation["z"] + self.roll = transformation["roll"] + self.pitch = transformation["pitch"] + self.yaw = transformation["yaw"] + self.base_frame = base_frame + self.child_frame = child_frame + self.type: str = transformation.get("type", "") + + self.name = self.child_frame.replace("_base_link", "").replace("_link", "") + + if len(self.type) == 0: + self.type = determine_link_type(self.name).value + warnings.warn( + f"Warning: Link type not explicitly defined for '{self.name}'. Determining type from link name and obtained {self.type}" + ) + + self.frame_id: str = transformation.get("frame_id", "") + if len(self.frame_id) == 0: + if ( + "pandar" in self.type + or "livox" in self.type + or "camera" in self.type + or "velodyne" in self.type.lower() + ): + # For common sensor descriptions, LiDAR and camera macros will automatically + # be attached with a "base_link" name + self.frame_id = self.name + else: + self.frame_id = self.child_frame + + except KeyError as e: + print(f"Error: Key {e} not in transformation dictionary") + raise e + + def serialize_single(self, key: str) -> str: + """ + Generate a serialized string for a single transformation parameter. + + Args: + key (str): Parameter key (x, y, z, roll, pitch, or yaw) + + Returns: + str: Serialized parameter string for use in XACRO template + """ + return f"${{calibration['{self.base_frame}']['{self.child_frame}']['{key}']}}" + + def serialize(self) -> str: + """ + Generate a complete serialized string for all transformation parameters. + + Returns: + str: Complete serialized transformation string for XACRO template + """ + return f""" + name=\"{self.name}\" + parent=\"{self.base_frame}\" + x=\"{self.serialize_single('x')}\" + y=\"{self.serialize_single('y')}\" + z=\"{self.serialize_single('z')}\" + roll=\"{self.serialize_single('roll')}\" + pitch=\"{self.serialize_single('pitch')}\" + yaw=\"{self.serialize_single('yaw')}\" + """ + + +class Calibration: + """ + Represents a complete set of calibration data for all sensors. + + Contains transformations for all sensors relative to a single base frame. + """ + + def __init__(self, calibration: Dict): + """ + Initialize calibration data from a dictionary. + + Args: + calibration (Dict): Dictionary containing calibration data + + Raises: + AssertionError: If calibration format is invalid + """ + self.base_dict: Dict = calibration + assert len(calibration.keys()) == 1, "Calibration file should have only one base frame" + assert isinstance( + list(calibration.keys())[0], str + ), "Calibration file should have only one base frame with key as a string" + self.base_frame: str = list(calibration.keys())[0] + + assert isinstance( + calibration[self.base_frame], dict + ), "Calibration file should have only one base frame with value as a dictionary" + + self.transforms: Dict[str, Transformation] = {} + + for key in calibration[self.base_frame]: + assert isinstance(key, str), "child frames should be strings" + try: + self.transforms[key] = Transformation( + calibration[self.base_frame][key], self.base_frame, key + ) + except KeyError as e: + print(f"Error: Key {e} not in calibration dictionary of {key}") + raise e + + +class LinkType(enum.Enum): + """Enum class for the type of the link.""" + + CAMERA = "monocular_camera" + IMU = "imu" + LIVOX = "livox_horizon" + PANDAR_40P = "pandar_40p" + PANDAR_OT128 = "pandar_ot128" + PANDAR_XT32 = "pandar_xt32" + PANDAR_QT = "pandar_qt" + PANDAR_QT128 = "pandar_qt128" + VELODYNE16 = "velodyne_16" + VLS128 = "velodyne_128" + RADAR = "radar" + GNSS = "gnss" + JOINT_UNITS = "units" + + +def obtain_link_type(link: Transformation) -> LinkType: + """Output the LinkType of the target link.""" + if len(link.type) > 0: + # use explicit type string to obtain link + link_type_lower = link.type.lower() + + # Check each enum value for a match + for type_enum in LinkType: + if link_type_lower == type_enum.value.lower(): + return type_enum + # if there is no match, or the type is not defined: + return determine_link_type(link.child_frame) + + +def determine_link_type(link_name: str) -> LinkType: + """Produce a guess of the type of the link based on its name.""" + if "cam" in link_name: + return LinkType.CAMERA + + if "imu" in link_name: + return LinkType.IMU + + if "gnss" in link_name: + return LinkType.GNSS + + if "livox" in link_name: + return LinkType.LIVOX + + if "velodyne" in link_name: + if "top" in link_name: + return LinkType.VLS128 + else: + return LinkType.VELODYNE16 + + if "radar" in link_name or "ars" in link_name: + return LinkType.RADAR + + if "pandar_40p" in link_name: + return LinkType.PANDAR_40P + + if "pandar_qt" in link_name: + return LinkType.PANDAR_QT + + if "hesai_top" in link_name: + return LinkType.PANDAR_OT128 + + if "hesai_front" in link_name: + return LinkType.PANDAR_XT32 + + if "hesai" in link_name: + return LinkType.PANDAR_XT32 + + else: + print(f"Link type not found for {link_name}, suspected to be a joint unit") + return LinkType.JOINT_UNITS + + +BASE_STRING = """""" + +VLD_STRING = """ + + """ + + +def base_string_func(macro_type: str, transform: Transformation) -> str: + if macro_type == "monocular_camera_macro": + extra = """fps=\"30\" + width=\"800\" + height=\"400\" + namespace=\"\" + fov=\"1.3\"""" + elif macro_type == "imu_macro": + extra = """fps=\"100\" + namespace=\"\"""" + else: + extra = "" + return BASE_STRING.format( + type=macro_type, + base_frame=transform.base_frame, + child_frame=transform.frame_id, # pandar + x=transform.serialize_single("x"), + y=transform.serialize_single("y"), + z=transform.serialize_single("z"), + roll=transform.serialize_single("roll"), + pitch=transform.serialize_single("pitch"), + yaw=transform.serialize_single("yaw"), + extra=extra, + ) + + +def VLP16_func(transform: Transformation) -> str: + return VLD_STRING.format( + type="VLP-16", + base_frame=transform.base_frame, + child_frame=transform.frame_id, + x=transform.serialize_single("x"), + y=transform.serialize_single("y"), + z=transform.serialize_single("z"), + roll=transform.serialize_single("roll"), + pitch=transform.serialize_single("pitch"), + yaw=transform.serialize_single("yaw"), + ) + + +def VLS128_func(transform: Transformation) -> str: + return VLD_STRING.format( + type="VLS-128", + base_frame=transform.base_frame, + child_frame=transform.frame_id, + x=transform.serialize_single("x"), + y=transform.serialize_single("y"), + z=transform.serialize_single("z"), + roll=transform.serialize_single("roll"), + pitch=transform.serialize_single("pitch"), + yaw=transform.serialize_single("yaw"), + ) + + +""" +link_dicts maps the LinkType to its required include files and the template strings. +including_file is the path to the required sub module xacro +string_api is a function that outputs a template string from a transform +""" + +link_dicts: Dict[LinkType, Dict[str, Union[str, Callable[[Transformation], str]]]] = { + LinkType.CAMERA: { + "including_file": "$(find camera_description)/urdf/monocular_camera.xacro", + "string_api": functools.partial(base_string_func, "monocular_camera_macro"), + }, + LinkType.IMU: { + "including_file": "$(find imu_description)/urdf/imu.xacro", + "string_api": functools.partial(base_string_func, "imu_macro"), + }, + LinkType.GNSS: { # for now, GNSS will also use the imu xacro files. + "including_file": "$(find imu_description)/urdf/imu.xacro", + "string_api": functools.partial(base_string_func, "imu_macro"), + }, + LinkType.VELODYNE16: { + "including_file": "$(find velodyne_description)/urdf/VLP-16.urdf.xacro", + "string_api": VLP16_func, + }, + LinkType.VLS128: { + "including_file": "$(find vls_description)/urdf/VLS-128.urdf.xacro", + "string_api": VLS128_func, + }, + LinkType.PANDAR_40P: { + "including_file": "$(find pandar_description)/urdf/pandar_40p.xacro", + "string_api": functools.partial(base_string_func, "Pandar40P"), + }, + LinkType.PANDAR_OT128: { + "including_file": "$(find pandar_description)/urdf/pandar_ot128.xacro", + "string_api": functools.partial(base_string_func, "PandarOT-128"), + }, + LinkType.PANDAR_XT32: { + "including_file": "$(find pandar_description)/urdf/pandar_xt32.xacro", + "string_api": functools.partial(base_string_func, "PandarXT-32"), + }, + LinkType.PANDAR_QT: { + "including_file": "$(find pandar_description)/urdf/pandar_qt.xacro", + "string_api": functools.partial(base_string_func, "PandarQT"), + }, + LinkType.PANDAR_QT128: { + "including_file": "$(find pandar_description)/urdf/pandar_qt128.xacro", + "string_api": functools.partial(base_string_func, "PandarQT-128"), + }, + LinkType.LIVOX: { + "including_file": "$(find livox_description)/urdf/livox_horizon.xacro", + "string_api": functools.partial(base_string_func, "livox_horizon_macro"), + }, + LinkType.RADAR: { + "including_file": "$(find radar_description)/urdf/radar.xacro", + "string_api": functools.partial(base_string_func, "radar_macro"), + }, + LinkType.JOINT_UNITS: { + "including_file": "{filename}.xacro", + }, +} + + +def main( + template_directory: str, + calibration_directory: str, + output_directory: str, + project_name: str, +): + os.makedirs(output_directory, exist_ok=True) + # Load the template + with open(os.path.join(template_directory, "sensors.xacro.template"), "r") as file: + base_template = Template(file.read()) + + # Render the template + print("Processing the main sensors_calibration.yaml") + calibration_path = os.path.join(calibration_directory, "sensors_calibration.yaml") + calib_yaml = load_yaml(calibration_path) + calib = Calibration(calib_yaml) + + render_meta_data = {} + render_meta_data["default_config_path"] = f"$(find {project_name})/config" + render_meta_data["sensor_calibration_yaml_path"] = "$(arg config_dir)/sensors_calibration.yaml" + render_meta_data["sensor_units_includes"] = [] + render_meta_data["sensor_units"] = [] + render_meta_data["isolated_sensors_includes"] = [] + render_meta_data["isolated_sensors"] = [] + + include_text = set() + sensor_items = [] + for _, transform in calib.transforms.items(): + link_type: LinkType = obtain_link_type(transform) + if link_type == LinkType.JOINT_UNITS: + print(f"Collected joint sensor unit {transform.name}, which will be further rendered.") + render_meta_data["sensor_units_includes"].append( + link_dicts[link_type]["including_file"].format(filename=transform.name) + ) + render_meta_data["sensor_units"].append( + { + "base_frame": transform.base_frame, + "child_frame": transform.child_frame, + "macro_name": f"{transform.name}_macro", + "name": transform.name, + } + ) + else: + print(f"Collected {transform.name}.") + include_text.add(link_dicts[link_type]["including_file"]) + sensor_items.append(link_dicts[link_type]["string_api"](transform)) + + render_meta_data["isolated_sensors_includes"] = list(include_text) + render_meta_data["isolated_sensors"] = sensor_items + + rendered = base_template.render(render_meta_data) + + print("=====================================") + # Save the rendered template + with open(os.path.join(output_directory, "sensors.xacro"), "w") as file: + file.write(rendered) + + # Write Sensor Units into separate files + with open(os.path.join(template_directory, "sensor_unit.xacro.template"), "r") as file: + sensor_units_template = Template(file.read()) + + for i, sensor_unit in enumerate(render_meta_data["sensor_units"]): + print(f"Processing {sensor_unit['name']}") + sensor_unit_calib_path = os.path.join( + calibration_directory, f"{sensor_unit['name']}_calibration.yaml" + ) + sensor_unit_calib_yaml = load_yaml(sensor_unit_calib_path) + sensor_unit_calib = Calibration(sensor_unit_calib_yaml) + sensor_unit_render_meta_data = {} + sensor_unit_render_meta_data["unit_macro_name"] = sensor_unit["macro_name"] + sensor_unit_render_meta_data["default_config_path"] = render_meta_data[ + "default_config_path" + ] + sensor_unit_render_meta_data["joint_unit_name"] = sensor_unit["name"] + sensor_unit_render_meta_data["current_base_link"] = sensor_unit_calib.base_frame + sensor_unit_isolated_sensors = [] + for _, transform in sensor_unit_calib.transforms.items(): + link_type: LinkType = obtain_link_type(transform) + include_text.add(link_dicts[link_type]["including_file"]) + print(f"collected {transform.name}") + sensor_unit_isolated_sensors.append(link_dicts[link_type]["string_api"](transform)) + sensor_unit_render_meta_data["isolated_sensors_includes"] = list(include_text) + sensor_unit_render_meta_data["isolated_sensors"] = sensor_unit_isolated_sensors + + rendered = sensor_units_template.render(sensor_unit_render_meta_data) + with open(os.path.join(output_directory, f'{sensor_unit["name"]}.xacro'), "w") as file: + file.write(rendered) + print("=====================================") + + return 0 + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser(description="Process four positional arguments.") + + # Add four positional arguments + parser.add_argument("template_directory", type=str, help="The first argument") + parser.add_argument("calibration_directory", type=str, help="The second argument") + parser.add_argument("output_directory", type=str, help="The third argument") + parser.add_argument("project_name", type=str, help="The fourth argument") + + # Parse the arguments + args = parser.parse_args() + + main( + args.template_directory, + args.calibration_directory, + args.output_directory, + args.project_name, + ) diff --git a/aip_urdf_compiler/templates/sensor_unit.xacro.template b/aip_urdf_compiler/templates/sensor_unit.xacro.template new file mode 100644 index 00000000..f0b44710 --- /dev/null +++ b/aip_urdf_compiler/templates/sensor_unit.xacro.template @@ -0,0 +1,31 @@ + + + + {% for item in isolated_sensors_includes %} + + {% endfor %} + + + + + + + + + + + + + + + + + + + + {% for item in isolated_sensors %} + {{ item }} + {% endfor %} + + + diff --git a/aip_urdf_compiler/templates/sensors.xacro.template b/aip_urdf_compiler/templates/sensors.xacro.template new file mode 100644 index 00000000..318e48bd --- /dev/null +++ b/aip_urdf_compiler/templates/sensors.xacro.template @@ -0,0 +1,31 @@ + + + + + + + {% for item in sensor_units_includes %} + + {% endfor %} + {% for item in isolated_sensors_includes %} + + {% endfor %} + + {% for item in sensor_units %} + + + {% endfor %} + + {% for item in isolated_sensors %} + + {{ item }} + {% endfor %} + diff --git a/aip_urdf_compiler/tests/urdf_diff.py b/aip_urdf_compiler/tests/urdf_diff.py new file mode 100644 index 00000000..ead01163 --- /dev/null +++ b/aip_urdf_compiler/tests/urdf_diff.py @@ -0,0 +1,274 @@ +#!/usr/bin/python3 +""" +URDF Model Difference Analyzer. + +This script analyzes and compares two XML/Xacro files, specifically designed for ROS URDF/Xacro files +containing sensor configurations. It identifies and reports differences in included files, sensor configurations, +and parameter changes between two versions of a file. + +The analyzer is particularly useful for: +- Tracking changes in sensor configurations +- Validating URDF/Xacro file modifications +- Documenting sensor setup modifications +- Quality assurance of robot configuration changes + +Author: [Your Name] +Date: [Current Date] +""" + +import argparse +from collections import OrderedDict +from typing import Dict +from typing import List +import xml.etree.ElementTree as ET + + +class XacroAnalyzer: + """ + A class to analyze differences between two Xacro/XML files. + + This class provides functionality to compare two Xacro files and identify changes + in includes, sensor configurations, and parameters. + + Attributes: + original_text (str): Content of the original file + new_text (str): Content of the new file + original_root (ET.Element): XML tree root of the original file + new_root (ET.Element): XML tree root of the new file + ns (dict): Namespace dictionary for Xacro XML parsing + """ + + def __init__(self, original_file: str, new_file: str): + """ + Initialize the XacroAnalyzer with two files to compare. + + Args: + original_file (str): Path to the original Xacro file + new_file (str): Path to the new Xacro file + """ + self.original_text = self._read_file(original_file) + self.new_text = self._read_file(new_file) + self.original_root = ET.fromstring(self.original_text) + self.new_root = ET.fromstring(self.new_text) + self.ns = {"xacro": "http://ros.org/wiki/xacro"} + + @staticmethod + def _read_file(filename: str) -> str: + """ + Read content from a file. + + Args: + filename (str): Path to the file to read + + Returns: + str: Content of the file + """ + with open(filename, "r", encoding="utf-8") as f: + return f.read() + + def _get_includes(self, root: ET.Element) -> List[str]: + """ + Extract all include statements from an XML root. + + Args: + root (ET.Element): XML root element to search + + Returns: + List[str]: Sorted list of included filenames + """ + includes = [] + for include in root.findall(".//xacro:include", self.ns): + includes.append(include.get("filename")) + return sorted(includes) + + def _sort_params(self, params: Dict) -> OrderedDict: + """ + Sort parameters in a standardized order with common parameters first. + + Args: + params (Dict): Dictionary of parameters to sort + + Returns: + OrderedDict: Sorted parameters with common parameters first + """ + common_params = ["name", "parent", "x", "y", "z", "roll", "pitch", "yaw"] + sorted_params = OrderedDict() + + # Add common params in specific order + for param in common_params: + if param in params: + sorted_params[param] = params[param] + + # Add remaining params alphabetically + for param in sorted(params.keys()): + if param not in common_params: + sorted_params[param] = params[param] + + return sorted_params + + def _get_sensors(self, root: ET.Element) -> Dict[str, List[Dict]]: + """ + Extract all sensor configurations from an XML root. + + Args: + root (ET.Element): XML root element to search + + Returns: + Dict[str, List[Dict]]: Dictionary of sensor types mapping to their configurations + """ + sensors = {"cameras": [], "lidars": [], "imus": [], "radars": []} + + sensor_patterns = { + "cameras": ["camera", "monocular"], + "lidars": ["hesai", "velodyne", "pandar", "lidar"], + "imus": ["imu", "gnss"], + "radars": ["radar"], + } + + for macro in root.findall(".//*[@name]"): + name = macro.get("name", "") + params = dict(macro.attrib) + + sensor_type = None + for type_name, patterns in sensor_patterns.items(): + if any(pattern in name.lower() for pattern in patterns): + sensor_type = type_name + break + + if sensor_type: + sensors[sensor_type].append({"name": name, "params": self._sort_params(params)}) + + for sensor_type in sensors: + sensors[sensor_type].sort(key=lambda x: x["name"]) + return sensors + + def _format_params(self, params: OrderedDict) -> str: + """ + Format parameters for readable output. + + Args: + params (OrderedDict): Parameters to format + + Returns: + str: Formatted parameter string + """ + return "\n".join([f' {k}="{v}"' for k, v in params.items()]) + + def analyze_differences(self) -> str: + """ + Analyze and report differences between the two Xacro files. + + This method performs a comprehensive comparison of: + - Included files + - Sensor configurations + - Parameter changes + + Returns: + str: Formatted report of all differences found + """ + output = [] + + # 1. Compare includes + output.append("# Key Differences\n") + output.append("## 1. Include Files") + original_includes = self._get_includes(self.original_root) + new_includes = self._get_includes(self.new_root) + + added_includes = set(new_includes) - set(original_includes) + removed_includes = set(original_includes) - set(new_includes) + + if added_includes or removed_includes: + output.append("### Changes:") + if added_includes: + output.append("**Added:**") + for inc in sorted(added_includes): + output.append(f"- {inc}") + if removed_includes: + output.append("**Removed:**") + for inc in sorted(removed_includes): + output.append(f"- {inc}") + output.append("") + + # 2. Compare sensors + original_sensors = self._get_sensors(self.original_root) + new_sensors = self._get_sensors(self.new_root) + + output.append("## 2. Sensor Configuration Changes") + + for sensor_type in ["cameras", "lidars", "imus", "radars"]: + orig_sensor = original_sensors[sensor_type] + new_sensor = new_sensors[sensor_type] + + if orig_sensor or new_sensor: + output.append(f"\n### {sensor_type.title()}") + + # Compare sensor names + orig_names = [s["name"] for s in orig_sensor] + new_names = [s["name"] for s in new_sensor] + + if orig_names != new_names: + output.append("#### Name Changes:") + output.append( + f"**Original ({len(orig_names)})**: " + ", ".join(sorted(orig_names)) + ) + output.append(f"**New ({len(new_names)})**: " + ", ".join(sorted(new_names))) + + # Compare parameters + if orig_sensor and new_sensor: + output.append("\n#### Parameter Changes:") + + # Compare parameters of first sensor of each type + orig_params = orig_sensor[0]["params"] + new_params = new_sensor[0]["params"] + + added_params = set(new_params.keys()) - set(orig_params.keys()) + removed_params = set(orig_params.keys()) - set(new_params.keys()) + changed_params = { + k: (orig_params[k], new_params[k]) + for k in set(orig_params.keys()) & set(new_params.keys()) + if orig_params[k] != new_params[k] + } + + if added_params: + output.append("**Added parameters:**") + for param in sorted(added_params): + output.append(f'- {param}: "{new_params[param]}"') + + if removed_params: + output.append("**Removed parameters:**") + for param in sorted(removed_params): + output.append(f'- {param}: "{orig_params[param]}"') + + if changed_params: + output.append("**Modified parameters:**") + for param, (old_val, new_val) in sorted(changed_params.items()): + output.append(f'- {param}: "{old_val}" → "{new_val}"') + + return "\n".join(output) + + +def main(): + # Create argument parser + parser = argparse.ArgumentParser( + description="Compare two XACRO files and analyze their differences" + ) + + # Add arguments + parser.add_argument( + "--original", "-o", required=True, help="Path to the original sensors.xacro file" + ) + + parser.add_argument("--new", "-n", required=True, help="Path to the new sensors.xacro file") + + # Parse arguments + args = parser.parse_args() + + # Create analyzer instance with provided file paths + analyzer = XacroAnalyzer(args.original, args.new) + + # Print analysis results + print(analyzer.analyze_differences()) + + +if __name__ == "__main__": + main() diff --git a/aip_x2_gen2_description/CMakeLists.txt b/aip_x2_gen2_description/CMakeLists.txt new file mode 100644 index 00000000..68dbdf53 --- /dev/null +++ b/aip_x2_gen2_description/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_x2_gen2_description) + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_package(INSTALL_TO_SHARE + urdf + config +) diff --git a/aip_x2_gen2_description/config/sensor_kit_calibration.yaml b/aip_x2_gen2_description/config/sensor_kit_calibration.yaml new file mode 100644 index 00000000..4a37420d --- /dev/null +++ b/aip_x2_gen2_description/config/sensor_kit_calibration.yaml @@ -0,0 +1,173 @@ +sensor_kit_base_link: + # left upper + left_upper/lidar_base_link: + x: 0.0 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 3.141592 # 180 [deg] to turn the connector toward the vehicle + left_lower/lidar_base_link: + x: 0.0 + y: -0.025 + z: -0.115 + roll: 0.6981 # 40[deg] + pitch: 0.0 + yaw: 3.141592 # 180 [deg] to turn the connector toward the vehicle + left_front/camera_link: + x: 0.12758 + y: -0.04589 + z: -0.0866 + roll: 0.0 + pitch: 0.08722 # 5 [deg] + yaw: 0.85478 # 49 [deg] + left_rear/camera_link: + x: -0.12842 + y: -0.04589 + z: -0.0866 + roll: 0.0 + pitch: 0.08722 # 5 [deg] + yaw: 2.2678 # 130 [deg] + left_center/camera_link: + x: 0.0 + y: -0.04089 + z: -0.18666 + roll: 0.0 + pitch: 0.933278 # 53.50 [deg] + yaw: 1.570796 # 90 [deg] + + # right upper + right_upper/lidar_base_link: + x: 0.0 + y: -2.15178 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 # to turn the connector toward the vehicle + right_lower/lidar_base_link: + x: 0.0 + y: -2.12678 + z: -0.115 + roll: 0.6981 # 40 [deg] + pitch: 0.0 + yaw: 0.0 # to turn the connector toward the vehicle + right_front/camera_link: + x: 0.12758 + y: -2.10589 + z: -0.0866 + roll: 0.0 + pitch: 0.08722 # 5 [deg] + yaw: -0.85478 # -49 [deg] + right_rear/camera_link: + x: -0.12842 + y: -2.10589 + z: -0.0866 + roll: 0.0 + pitch: 0.08722 # 5 [deg] + yaw: -2.2678 # -130 [deg] + right_center/camera_link: + x: 0.0 + y: -2.11089 + z: -0.18666 + roll: 0.0 + pitch: 0.933278 # 53.50 [deg] + yaw: -1.570796 # -90 [deg] + + # front upper + top_front_left/imu_link: + x: 0.562 + y: -0.99974 + z: -0.06146 + roll: 3.141592 + pitch: 0.0 + yaw: 0.0 + top_front_right/imu_link: + x: 0.562 + y: -1.13974 + z: -0.06146 + roll: 3.141592 + pitch: 0.0 + yaw: 0.0 + top_front_right/camera_link: + x: 0.73758 + y: -1.26439 + z: -0.06666 + roll: 0.0 + pitch: 0.8373 # 48 [deg] + yaw: 0.0 + top_front_center_right/camera_link: + x: 0.73058 + y: -1.18899 + z: -0.02166 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + top_front_center_left/camera_link: + x: 0.73058 + y: -1.03819 + z: -0.02166 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + top_front_left/camera_link: + x: 0.73058 + y: -0.96279 + z: -0.02166 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + top_front/gnss_link: + x: 0.30133 + y: -1.07589 + z: 0.02861 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + # rear upper + top_rear_center/camera_link: + x: -6.06359 + y: -1.20389 + z: -0.14078 + roll: 0.0 + pitch: 0.0 + yaw: 3.141592 # 180 [deg] to turn the connector toward the vehicle + top_rear/gnss_link: + x: -5.77517 + y: -1.07589 + z: 0.02861 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + # front lower + front_upper/lidar_base_link: + x: 0.94058 + y: -1.07589 + z: -1.91826 + roll: 0.0 + pitch: 0.0 + yaw: 1.570796 # 90 [deg] to turn the connector toward the vehicle + front_lower/lidar_base_link: + x: 0.96758 + y: -1.07589 + z: -2.15234 + roll: 0.6981 # 40[deg] + pitch: 0.0 + yaw: 1.570796 # 90 [deg] to turn the connector toward the vehicle + + # rear lower + rear_upper/lidar_base_link: + x: -6.08042 + y: -1.07589 + z: -1.91826 + roll: 0.0 + pitch: 0.0 + yaw: -1.570796 # 90 [deg] to turn the connector toward the vehicle + rear_lower/lidar_base_link: + x: -6.10742 + y: -1.07589 + z: -2.15234 + roll: 0.6981 # 40[deg] + pitch: 0.0 + yaw: -1.570796 # 90 [deg] to turn the connector toward the vehicle diff --git a/aip_x2_gen2_description/config/sensors_calibration.yaml b/aip_x2_gen2_description/config/sensors_calibration.yaml new file mode 100644 index 00000000..1b5105f0 --- /dev/null +++ b/aip_x2_gen2_description/config/sensors_calibration.yaml @@ -0,0 +1,57 @@ +base_link: + sensor_kit_base_link: + x: 4.66244 + y: 1.07589 + z: 2.78926 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + # radar + front_center/radar_link: + x: 5.69207 + y: 0.0 + z: 0.78894 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + + front_left/radar_link: + x: 5.37204 + y: 1.08537 + z: 0.73431 + roll: 0.0 + pitch: 0.0 + yaw: 1.2211 # 70 [deg] + + front_right/radar_link: + x: 5.37204 + y: -1.06642 + z: 0.73431 + roll: 0.0 + pitch: 0.0 + yaw: -1.2211 # 70 [deg] + + rear_center/radar_link: + x: -1.50704 + y: 0.0 + z: 0.78894 + roll: 0.0 + pitch: 0.0 + yaw: 3.141592 # 180 [deg] + + rear_left/radar_link: + x: -1.27564 + y: 1.07767 + z: 0.5487 + roll: 0.0 + pitch: 0.0 + yaw: 1.9189 # 110 [deg] + + rear_right/radar_link: + x: -1.27564 + y: -1.07768 + z: 0.5487 + roll: 0.0 + pitch: 0.0 + yaw: -1.9189 # 110 [deg] diff --git a/aip_x2_gen2_description/package.xml b/aip_x2_gen2_description/package.xml new file mode 100644 index 00000000..b35ad7a3 --- /dev/null +++ b/aip_x2_gen2_description/package.xml @@ -0,0 +1,17 @@ + + + aip_x2_gen2_description + 0.1.0 + The aip_x2_gen2_description package + + Tomohito Ando + Apache 2 + + ament_cmake_auto + + pandar_description + + + ament_cmake + + diff --git a/aip_x2_gen2_description/urdf/gnss.xacro b/aip_x2_gen2_description/urdf/gnss.xacro new file mode 100644 index 00000000..55002be0 --- /dev/null +++ b/aip_x2_gen2_description/urdf/gnss.xacro @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_description/urdf/radar.xacro b/aip_x2_gen2_description/urdf/radar.xacro new file mode 100644 index 00000000..8b0f8d4b --- /dev/null +++ b/aip_x2_gen2_description/urdf/radar.xacro @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_description/urdf/sensor_kit.xacro b/aip_x2_gen2_description/urdf/sensor_kit.xacro new file mode 100644 index 00000000..2db48d09 --- /dev/null +++ b/aip_x2_gen2_description/urdf/sensor_kit.xacro @@ -0,0 +1,345 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_xx1_gen2_description/urdf/sensors.xacro b/aip_x2_gen2_description/urdf/sensors.xacro similarity index 70% rename from aip_xx1_gen2_description/urdf/sensors.xacro rename to aip_x2_gen2_description/urdf/sensors.xacro index 79c2c15f..34b10729 100644 --- a/aip_xx1_gen2_description/urdf/sensors.xacro +++ b/aip_x2_gen2_description/urdf/sensors.xacro @@ -1,11 +1,12 @@ - - - + + + - + - - - - - - + - + + + + - + + + + diff --git a/aip_x2_gen2_launch/CMakeLists.txt b/aip_x2_gen2_launch/CMakeLists.txt new file mode 100644 index 00000000..f3997ac7 --- /dev/null +++ b/aip_x2_gen2_launch/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.5) +project(aip_x2_gen2_launch) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + data + config +) diff --git a/aip_x2_gen2_launch/config/ARS548.param.yaml b/aip_x2_gen2_launch/config/ARS548.param.yaml new file mode 100644 index 00000000..1fe5e7e2 --- /dev/null +++ b/aip_x2_gen2_launch/config/ARS548.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + host_ip: 10.13.1.166 + sensor_ip: 10.13.1.114 + data_port: 42102 + base_frame: base_link + object_frame: base_link + launch_hw: true + multicast_ip: 224.0.2.2 + sensor_model: ARS548 + configuration_host_port: 42401 + configuration_sensor_port: 42101 + use_sensor_time: false + configuration_vehicle_length: 7.2369 + configuration_vehicle_width: 2.2916 + configuration_vehicle_height: 3.08 + configuration_vehicle_wheelbase: 4.76012 diff --git a/aip_x2_gen2_launch/config/asterx_sb3_rover.param.yaml b/aip_x2_gen2_launch/config/asterx_sb3_rover.param.yaml new file mode 100644 index 00000000..559b7ea5 --- /dev/null +++ b/aip_x2_gen2_launch/config/asterx_sb3_rover.param.yaml @@ -0,0 +1,82 @@ +/**: + ros__parameters: + device: tcp://192.168.20.102:28784 + + frame_id: gnss_link + aux1_frame_id: aux1 + get_spatial_config_from_tf: false + use_ros_axis_orientation: false + receiver_type: gnss + multi_antenna: true + + datum: Default + + att_offset: + heading: 0.0 + pitch: 0.0 + + ant_type: "SEPPOLANT_MC.V2 NONE" + ant_serial_nr: Unknown + ant_aux1_type: "SEPPOLANT_MC.V2 NONE" + ant_aux1_serial_nr: Unknown + + polling_period: + pvt: 200 + rest: 200 + + use_gnss_time: false + + rtk_settings: + ntrip_1: + id: "" + caster: "" + caster_port: 2101 + username: "" + password: "" + mountpoint: "" + version: "v2" + tls: false + fingerprint: "" + rtk_standard: "auto" + send_gga: "auto" + keep_open: true + ip_server_1: + id: "IPS1" + port: 28785 + rtk_standard: "RTCMv3" + send_gga: "auto" + keep_open: true + serial_1: + port: "" + baud_rate: 115200 + rtk_standard: "auto" + send_gga: "auto" + keep_open: true + + publish: + # For both GNSS and INS Rxs + navsatfix: true + gpsfix: false + gpgga: false + gprmc: false + gpst: false + measepoch: false + pvtcartesian: false + pvtgeodetic: true + basevectorcart: false + basevectorgeod: false + poscovcartesian: false + poscovgeodetic: true + velcovgeodetic: false + atteuler: false + attcoveuler: false + pose: false + twist: false + diagnostics: true + # For GNSS Rx only + gpgsa: false + gpgsv: false + + # logger + + activate_debug_log: false diff --git a/aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml b/aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml new file mode 100644 index 00000000..ea0285a5 --- /dev/null +++ b/aip_x2_gen2_launch/config/blockage_diagnostics_param_file.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + blockage_ratio_threshold: 0.2 + blockage_count_threshold: 50 + blockage_buffering_frames: 2 + blockage_buffering_interval: 1 + enable_dust_diag: false + publish_debug_image: false + dust_ratio_threshold: 0.2 + dust_count_threshold: 10 + dust_kernel_size: 2 + dust_buffering_frames: 10 + dust_buffering_interval: 1 + blockage_kernel: 10 diff --git a/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml b/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml new file mode 100644 index 00000000..f2ffdcbd --- /dev/null +++ b/aip_x2_gen2_launch/config/diagnostic_aggregator/sensor_kit.param.yaml @@ -0,0 +1,100 @@ +/**: + ros__parameters: + sensing: + type: diagnostic_aggregator/AnalyzerGroup + path: sensing + analyzers: + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": sensing_topic_status"] + timeout: 1.0 + + lidar: + type: diagnostic_aggregator/AnalyzerGroup + path: lidar + analyzers: + performance_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: performance_monitoring + analyzers: + blockage: + type: diagnostic_aggregator/GenericAnalyzer + path: blockage + contains: [": blockage_validation"] + timeout: 1.0 + visibility: + type: diagnostic_aggregator/GenericAnalyzer + path: visibility + contains: ["left_upper: visibility_validation"] + timeout: 1.0 + concat_status: + type: diagnostic_aggregator/GenericAnalyzer + path: concat_status + contains: [": concat_status"] + timeout: 1.0 + pandar: + type: diagnostic_aggregator/AnalyzerGroup + path: pandar + analyzers: + health_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: health_monitoring + analyzers: + connection: + type: diagnostic_aggregator/GenericAnalyzer + path: connection + contains: [": pandar_connection"] + timeout: 5.0 + temperature: + type: diagnostic_aggregator/GenericAnalyzer + path: temperature + contains: [": pandar_temperature"] + timeout: 5.0 + ptp: + type: diagnostic_aggregator/GenericAnalyzer + path: ptp + contains: [": pandar_ptp"] + timeout: 5.0 + + dust: + type: diagnostic_aggregator/GenericAnalyzer + path: dust + contains: [": dust_validation"] + timeout: 1.0 + + gnss: + type: diagnostic_aggregator/AnalyzerGroup + path: gnss + analyzers: + septentrio: + type: diagnostic_aggregator/AnalyzerGroup + path: septentrio + analyzers: + health_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: health_monitoring + analyzers: + gnss: + type: diagnostic_aggregator/GenericAnalyzer + path: gnss + startswith: ["gnss"] + contains: [": gnss"] + timeout: 5.0 + imu: + type: diagnostic_aggregator/AnalyzerGroup + path: imu + analyzers: + bias_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: bias_monitoring + analyzers: + gyro_bias_validator: + type: diagnostic_aggregator/GenericAnalyzer + path: gyro_bias_validator + contains: [": gyro_bias_validator"] + timeout: 1.0 diff --git a/aip_x2_gen2_launch/config/dual_return_filter.param.yaml b/aip_x2_gen2_launch/config/dual_return_filter.param.yaml new file mode 100644 index 00000000..75168e7b --- /dev/null +++ b/aip_x2_gen2_launch/config/dual_return_filter.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + roi_mode: "Fixed_azimuth_ROI" # description="options: `No_ROI`, `Fixed_xyz_ROI` or `Fixed_azimuth_ROI`"/> + weak_first_local_noise_threshold: 2 # description="for No_ROI roi_mode, recommended value is 10" /> + visibility_error_threshold: 0.95 + visibility_warn_threshold: 0.97 + max_distance: 10.0 + x_max: 18.0 + x_min: -12.0 + y_max: 2.0 + y_min: -2.0 + z_max: 10.0 + z_min: 0.0 diff --git a/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml b/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml new file mode 100644 index 00000000..7686ebf3 --- /dev/null +++ b/aip_x2_gen2_launch/config/dummy_diag_publisher/sensor_kit.param.yaml @@ -0,0 +1,69 @@ +# Description: +# name: diag name +# is_active: Force update or not +# status: diag status set by dummy diag publisher "OK, Warn, Error, Stale" +# +# Note: +# +# default values are: +# is_active: "true" +# status: "OK" +--- +/**: + ros__parameters: + required_diags: + # gnss + "topic_state_monitor_gnss_pose: gnss_topic_status": default + "septentrio_driver: Quality indicators": default + + # imu + "imu_monitor: yaw_rate_status": default + "topic_state_monitor_imu_data: imu_topic_status": default + "gyro_bias_validator: gyro_bias_validator": default + + # lidar + "/sensing/lidar/front_lower/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/front_upper/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/left_lower/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/left_upper/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/right_lower/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/right_upper/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/rear_lower/hesai_ros_wrapper_node: hesai_status": default + "/sensing/lidar/rear_upper/hesai_ros_wrapper_node: hesai_status": default + + "/sensing/lidar/front_lower/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/front_upper/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/left_lower/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/left_upper/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/right_lower/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/right_upper/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/rear_lower/hesai_ros_wrapper_node: hesai_ptp": default + "/sensing/lidar/rear_upper/hesai_ros_wrapper_node: hesai_ptp": default + + "/sensing/lidar/front_lower/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/front_upper/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/left_lower/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/left_upper/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/right_lower/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/right_upper/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/rear_lower/hesai_ros_wrapper_node: hesai_temperature": default + "/sensing/lidar/rear_upper/hesai_ros_wrapper_node: hesai_temperature": default + "concatenate_data: concat_status": default + + # camera + "v4l2_camera_camera0: capture_status": default + "v4l2_camera_camera1: capture_status": default + "v4l2_camera_camera2: capture_status": default + "v4l2_camera_camera3: capture_status": default + "v4l2_camera_camera4: capture_status": default + "v4l2_camera_camera5: capture_status": default + "v4l2_camera_camera6: capture_status": default + "v4l2_camera_camera7: capture_status": default + + # radar + "topic_state_monitor_radar_front_center: radar_front_center_topic_status": default + "topic_state_monitor_radar_front_left: radar_front_left_topic_status": default + "topic_state_monitor_radar_front_right: radar_front_right_topic_status": default + "topic_state_monitor_radar_rear_center: radar_rear_center_topic_status": default + "topic_state_monitor_radar_rear_left: radar_rear_left_topic_status": default + "topic_state_monitor_radar_rear_right: radar_rear_right_topic_status": default diff --git a/aip_x2_gen2_launch/config/gyro_bias_estimator.param.yaml b/aip_x2_gen2_launch/config/gyro_bias_estimator.param.yaml new file mode 100644 index 00000000..d552569f --- /dev/null +++ b/aip_x2_gen2_launch/config/gyro_bias_estimator.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + gyro_bias_threshold: 0.008 # [rad/s] + timer_callback_interval_sec: 0.5 # [sec] + diagnostics_updater_interval_sec: 0.5 # [sec] + straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/aip_x2_gen2_launch/config/mosaic_x5_rover.param.yaml b/aip_x2_gen2_launch/config/mosaic_x5_rover.param.yaml new file mode 100644 index 00000000..566ee6ba --- /dev/null +++ b/aip_x2_gen2_launch/config/mosaic_x5_rover.param.yaml @@ -0,0 +1,82 @@ +/**: + ros__parameters: + device: tcp://192.168.100.101:28784 + + frame_id: gnss_link + aux1_frame_id: aux1 + get_spatial_config_from_tf: false + use_ros_axis_orientation: false + receiver_type: gnss + multi_antenna: false + + datum: Default + + att_offset: + heading: 0.0 + pitch: 0.0 + + ant_type: Unknown + ant_serial_nr: Unknown + ant_aux1_type: Unknown + ant_aux1_serial_nr: Unknown + + polling_period: + pvt: 200 + rest: 200 + + use_gnss_time: false + + rtk_settings: + ntrip_1: + id: "" + caster: "" + caster_port: 2101 + username: "" + password: "" + mountpoint: "" + version: "v2" + tls: false + fingerprint: "" + rtk_standard: "auto" + send_gga: "auto" + keep_open: true + ip_server_1: + id: "IPS1" + port: 28785 + rtk_standard: "RTCMv3" + send_gga: "auto" + keep_open: true + serial_1: + port: "" + baud_rate: 115200 + rtk_standard: "auto" + send_gga: "auto" + keep_open: true + + publish: + # For both GNSS and INS Rxs + navsatfix: true + gpsfix: false + gpgga: false + gprmc: false + gpst: false + measepoch: false + pvtcartesian: false + pvtgeodetic: true + basevectorcart: false + basevectorgeod: false + poscovcartesian: false + poscovgeodetic: true + velcovgeodetic: false + atteuler: false + attcoveuler: false + pose: false + twist: false + diagnostics: true + # For GNSS Rx only + gpgsa: false + gpgsv: false + + # logger + + activate_debug_log: false diff --git a/aip_x2_gen2_launch/config/point_filters/default/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/default/point_filters_full.param.yaml new file mode 100644 index 00000000..7ff58606 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/default/point_filters_full.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + point_filters: | + { + } diff --git a/aip_x2_gen2_launch/config/point_filters/default/point_filters_left_upper.param.yaml b/aip_x2_gen2_launch/config/point_filters/default/point_filters_left_upper.param.yaml new file mode 100644 index 00000000..1d5d525b --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/default/point_filters_left_upper.param.yaml @@ -0,0 +1,47 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [15, 143.95, 147.1], + [72, 319, 322], + [73, 318, 323], + [74, 318, 323], + [75, 318, 323], + [76, 318, 324], + [77, 318, 325], + [77, 318, 325], + [78, 318, 325], + [79, 318, 325], + [80, 318, 325], + [81, 318, 325], + [82, 318, 325], + [83, 318, 325], + [84, 319, 325], + [85, 319, 325], + [86, 319, 325], + [87, 320, 325], + [88, 321, 325], + [89, 322, 323], + + [91, 287.55, 289.90], + [92, 287.55, 289.75], + [109, 314.74, 316.74], + [110, 312.74, 317.34], + [111, 312.73, 315.93], + [112, 308.48, 313.28], + [113, 305.68, 312.28], + [114, 308.58, 310.58], + [117, 305.77, 313.77], + [118, 303.81, 308.53], + [119, 302.09, 307.53], + [120, 298.42, 307.63], + [121, 299.63, 312.03], + [122, 298.42, 310.02], + [123, 298.42, 310.22], + [124, 298.16, 308.76], + [125, 295.94, 307.54], + [126, 292.12, 303.92], + [127, 289.71, 297.31] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/default/point_filters_one_half.param.yaml b/aip_x2_gen2_launch/config/point_filters/default/point_filters_one_half.param.yaml new file mode 100644 index 00000000..42db0ed7 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/default/point_filters_one_half.param.yaml @@ -0,0 +1,71 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [1, 0, 360], + [3, 0, 360], + [5, 0, 360], + [7, 0, 360], + [9, 0, 360], + [11, 0, 360], + [13, 0, 360], + [15, 0, 360], + [17, 0, 360], + [19, 0, 360], + [21, 0, 360], + [23, 0, 360], + [25, 0, 360], + [27, 0, 360], + [29, 0, 360], + [31, 0, 360], + [33, 0, 360], + [35, 0, 360], + [37, 0, 360], + [39, 0, 360], + [41, 0, 360], + [43, 0, 360], + [45, 0, 360], + [47, 0, 360], + [49, 0, 360], + [51, 0, 360], + [53, 0, 360], + [55, 0, 360], + [57, 0, 360], + [59, 0, 360], + [61, 0, 360], + [63, 0, 360], + [65, 0, 360], + [67, 0, 360], + [69, 0, 360], + [71, 0, 360], + [73, 0, 360], + [75, 0, 360], + [77, 0, 360], + [79, 0, 360], + [81, 0, 360], + [83, 0, 360], + [85, 0, 360], + [87, 0, 360], + [89, 0, 360], + [91, 0, 360], + [93, 0, 360], + [95, 0, 360], + [97, 0, 360], + [99, 0, 360], + [101, 0, 360], + [103, 0, 360], + [105, 0, 360], + [107, 0, 360], + [109, 0, 360], + [111, 0, 360], + [113, 0, 360], + [115, 0, 360], + [117, 0, 360], + [119, 0, 360], + [121, 0, 360], + [123, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/default/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/default/point_filters_one_third.param.yaml new file mode 100644 index 00000000..075381cc --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/default/point_filters_one_third.param.yaml @@ -0,0 +1,92 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/default/point_filters_right_upper.param.yaml b/aip_x2_gen2_launch/config/point_filters/default/point_filters_right_upper.param.yaml new file mode 100644 index 00000000..d47a15dd --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/default/point_filters_right_upper.param.yaml @@ -0,0 +1,42 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [71, 36, 39], + [72, 36, 39], + [73, 36, 39], + [74, 36, 41], + [75, 36, 41], + [76, 36, 41], + [77, 34, 41], + [77, 34, 41], + [78, 33, 41], + [79, 33, 41], + [80, 33, 41], + [81, 33, 41], + [82, 33, 41], + [83, 33, 41], + [84, 33, 40], + [85, 33, 40], + [86, 33, 39], + [87, 33, 39], + [88, 34, 37], + [88, 36, 37], + [88, 35, 37], + + [110, 43.17, 45.17], + [110, 87.78, 89.97], + [117, 49.80, 51.80], + [118, 49.60, 51.60], + [119, 50.40, 52.60], + [120, 49.28, 52.88], + [121, 46.08, 55.08], + [122, 48.07, 56.87], + [123, 48.07, 58.67], + [124, 51.80, 60.20], + [125, 50.98, 63.58], + [126, 52.56, 65.76], + [127, 58.57, 69.17] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/default/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/default/point_filters_two_third.param.yaml new file mode 100644 index 00000000..45a57f55 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/default/point_filters_two_third.param.yaml @@ -0,0 +1,50 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_full.param.yaml new file mode 100644 index 00000000..cd534e3a --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_full.param.yaml @@ -0,0 +1,44 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 128], [0, 232,360], + [1, 0, 128], [1, 236,360], + [2, 0, 125], [2, 238,360], + [3, 0, 123], [3, 240,360], + [4, 0, 121], [4, 242,360], + [5, 0, 121], [5, 242,360], + [6, 0, 120], [6, 246,360], + [7, 0, 120], [7, 246,360], + [8, 0, 120], [8, 248,360], + [9, 0, 115], [9, 248,360], + [10, 0, 115], [10, 250,360], + [11, 0, 115], [11, 250,360], + [12, 0, 115], [12, 252,360], + [13, 0, 110], [13, 252,360], + [14, 0, 108], [14, 252,360], + [15, 0, 108], [15, 252,360], + [16, 0, 108], [16, 252,360], + [17, 0, 108], [17, 252,360], + [18, 0, 106], [18, 254,360], + [19, 0, 106], [19, 254,360], + [20, 0, 105], [20, 258,360], + [21, 0, 105], [21, 258,360], + [22, 0, 105], [22, 260,360], + [23, 0, 105], [23, 260,360], + [24, 0, 103], [24, 260,360], + [25, 0, 103], [25, 260,360], + [26, 0, 100], [26, 260,360], + [27, 0, 100], [27, 260,360], + [28, 0, 100], [28, 260,360], + [29, 0, 100], [29, 260,360], + [30, 0, 94], [30, 266,360], + [31, 0, 94], [31, 266,360], + [32, 0, 94], [32, 266,360], + [33, 0, 94], [33, 266,360], + [34, 0, 94], [34, 266,360], + [35, 0, 94], [35, 266,360], + [36, 0, 94], [36, 266,360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_one_third.param.yaml new file mode 100644 index 00000000..f0607ac6 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_one_third.param.yaml @@ -0,0 +1,129 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 128], [0, 232,360], + [1, 0, 128], [1, 236,360], + [2, 0, 125], [2, 238,360], + [3, 0, 123], [3, 240,360], + [4, 0, 121], [4, 242,360], + [5, 0, 121], [5, 242,360], + [6, 0, 120], [6, 246,360], + [7, 0, 120], [7, 246,360], + [8, 0, 120], [8, 248,360], + [9, 0, 115], [9, 248,360], + [10, 0, 115], [10, 250,360], + [11, 0, 115], [11, 250,360], + [12, 0, 115], [12, 252,360], + [13, 0, 110], [13, 252,360], + [14, 0, 108], [14, 252,360], + [15, 0, 108], [15, 252,360], + [16, 0, 108], [16, 252,360], + [17, 0, 108], [17, 252,360], + [18, 0, 106], [18, 254,360], + [19, 0, 106], [19, 254,360], + [20, 0, 105], [20, 258,360], + [21, 0, 105], [21, 258,360], + [22, 0, 105], [22, 260,360], + [23, 0, 105], [23, 260,360], + [24, 0, 103], [24, 260,360], + [25, 0, 103], [25, 260,360], + [26, 0, 100], [26, 260,360], + [27, 0, 100], [27, 260,360], + [28, 0, 100], [28, 260,360], + [29, 0, 100], [29, 260,360], + [30, 0, 94], [30, 266,360], + [31, 0, 94], [31, 266,360], + [32, 0, 94], [32, 266,360], + [33, 0, 94], [33, 266,360], + [34, 0, 94], [34, 266,360], + [35, 0, 94], [35, 266,360], + [36, 0, 94], [36, 266,360], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_two_third.param.yaml new file mode 100644 index 00000000..dc4e10a9 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/front_lower/point_filters_two_third.param.yaml @@ -0,0 +1,87 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 128], [0, 232,360], + [1, 0, 128], [1, 236,360], + [2, 0, 125], [2, 238,360], + [3, 0, 123], [3, 240,360], + [4, 0, 121], [4, 242,360], + [5, 0, 121], [5, 242,360], + [6, 0, 120], [6, 246,360], + [7, 0, 120], [7, 246,360], + [8, 0, 120], [8, 248,360], + [9, 0, 115], [9, 248,360], + [10, 0, 115], [10, 250,360], + [11, 0, 115], [11, 250,360], + [12, 0, 115], [12, 252,360], + [13, 0, 110], [13, 252,360], + [14, 0, 108], [14, 252,360], + [15, 0, 108], [15, 252,360], + [16, 0, 108], [16, 252,360], + [17, 0, 108], [17, 252,360], + [18, 0, 106], [18, 254,360], + [19, 0, 106], [19, 254,360], + [20, 0, 105], [20, 258,360], + [21, 0, 105], [21, 258,360], + [22, 0, 105], [22, 260,360], + [23, 0, 105], [23, 260,360], + [24, 0, 103], [24, 260,360], + [25, 0, 103], [25, 260,360], + [26, 0, 100], [26, 260,360], + [27, 0, 100], [27, 260,360], + [28, 0, 100], [28, 260,360], + [29, 0, 100], [29, 260,360], + [30, 0, 94], [30, 266,360], + [31, 0, 94], [31, 266,360], + [32, 0, 94], [32, 266,360], + [33, 0, 94], [33, 266,360], + [34, 0, 94], [34, 266,360], + [35, 0, 94], [35, 266,360], + [36, 0, 94], [36, 266,360], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml new file mode 100644 index 00000000..9ba6571d --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_full.param.yaml @@ -0,0 +1,103 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 93],[0, 266, 360], + [1, 0, 93],[1, 266, 360], + [2, 0, 93],[2, 266, 360], + [3, 0, 93],[3, 266, 360], + [4, 268, 360], + [5, 268, 360], + [6, 268, 360], + [7, 268, 360], + [8, 268, 360], + [10, 272, 360], + [11, 272, 360], + [12, 272, 360], + [13, 272, 360], + [14, 266, 360], + [15, 266, 360], + [16, 266, 360], + [17, 0, 96],[17, 266, 360], + [18, 0, 96],[18, 263, 360], + [19, 0, 96],[19, 263, 360], + [20, 0, 96],[20, 263, 360], + [21, 0, 96],[21, 263, 360], + [22, 270, 360], + [23, 270, 360], + [24, 270, 360], + [29, 0, 96], + [30, 0, 96],[30, 267, 360], + [31, 0, 96],[31, 267, 360], + [32, 0, 96],[32, 267, 360], + [33, 0, 96], + [38, 272, 360], + [45, 0, 93],[45, 267, 360], + [50, 0, 98], + [51, 0, 98], + [52, 0, 98], + [53, 0, 98], + [54, 0, 98], + [57, 0, 93], + [58, 0, 93],[58, 261, 360], + [59, 0, 93],[59, 261, 360], + [60, 0, 93],[60, 261, 360], + [61, 0, 93], + [72, 269, 360], + [73, 0, 88],[73, 269, 360], + [74, 0, 88],[74, 269, 360], + [75, 0, 90], + [76, 0, 94], + [77, 0, 94], + [78, 0, 94], + [79, 0, 94], + [80, 0, 94], + [81, 0, 90], + [82, 0, 90], + [83, 0, 90], + [84, 0, 88], + [85, 0, 88], + [86, 0, 88], + [88, 273, 360], + [89, 273, 360], + [90, 273, 360], + [91, 0, 93],[91, 273, 360], + [92, 0, 93],[92, 268, 360], + [93, 0, 93],[93, 273, 360], + [94, 0, 93],[94, 273, 360], + [95, 0, 93],[95, 273, 360], + [96, 0, 93],[96, 273, 360], + [97, 0, 93],[97, 273, 360], + [98, 0, 93],[98, 273, 360], + [99, 0, 93],[99, 273, 360], + [100, 0, 93.5],[100, 273, 360], + [101, 273, 360], + [102, 273, 360], + [103, 274, 360], + [104, 274, 360], + [105, 275, 360], + [106, 0, 95],[106, 275, 360], + [107, 0, 95],[107, 275, 360], + [108, 0, 95],[108, 275, 360], + [109, 0, 95], + [110, 0, 95], + [111, 0, 90], + [112, 0, 93], + [113, 0, 93], + [114, 0, 93], + [115, 0, 93], + [116, 0, 93], + [117, 0, 93], + [118, 0, 96],[118, 265, 360], + [119, 0, 96],[119, 265, 360], + [120, 0, 98],[120, 265, 360], + [121, 0, 98],[121, 265, 360], + [122, 0, 98],[122, 258, 360], + [123, 0, 98],[123, 258, 360], + [124, 0, 98],[124, 258, 360], + [125, 0, 100],[125, 256, 360], + [126, 0, 100],[126, 256, 360], + [127, 0, 100],[127, 256, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml new file mode 100644 index 00000000..70496d1e --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_one_third.param.yaml @@ -0,0 +1,188 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 93],[0, 266, 360], + [1, 0, 93],[1, 266, 360], + [2, 0, 93],[2, 266, 360], + [3, 0, 93],[3, 266, 360], + [4, 268, 360], + [5, 268, 360], + [6, 268, 360], + [7, 268, 360], + [8, 268, 360], + [10, 272, 360], + [11, 272, 360], + [12, 272, 360], + [13, 272, 360], + [14, 266, 360], + [15, 266, 360], + [16, 266, 360], + [17, 0, 96],[17, 266, 360], + [18, 0, 96],[18, 263, 360], + [19, 0, 96],[19, 263, 360], + [20, 0, 96],[20, 263, 360], + [21, 0, 96],[21, 263, 360], + [22, 270, 360], + [23, 270, 360], + [24, 270, 360], + [29, 0, 96], + [30, 0, 96],[30, 267, 360], + [31, 0, 96],[31, 267, 360], + [32, 0, 96],[32, 267, 360], + [33, 0, 96], + [38, 272, 360], + [45, 0, 93],[45, 267, 360], + [50, 0, 98], + [51, 0, 98], + [52, 0, 98], + [53, 0, 98], + [54, 0, 98], + [57, 0, 93], + [58, 0, 93],[58, 261, 360], + [59, 0, 93],[59, 261, 360], + [60, 0, 93],[60, 261, 360], + [61, 0, 93], + [72, 269, 360], + [73, 0, 88],[73, 269, 360], + [74, 0, 88],[74, 269, 360], + [75, 0, 90], + [76, 0, 94], + [77, 0, 94], + [78, 0, 94], + [79, 0, 94], + [80, 0, 94], + [81, 0, 90], + [82, 0, 90], + [83, 0, 90], + [84, 0, 88], + [85, 0, 88], + [86, 0, 88], + [88, 273, 360], + [89, 273, 360], + [90, 273, 360], + [91, 0, 93],[91, 273, 360], + [92, 0, 93],[92, 268, 360], + [93, 0, 93],[93, 273, 360], + [94, 0, 93],[94, 273, 360], + [95, 0, 93],[95, 273, 360], + [96, 0, 93],[96, 273, 360], + [97, 0, 93],[97, 273, 360], + [98, 0, 93],[98, 273, 360], + [99, 0, 93],[99, 273, 360], + [100, 0, 93.5],[100, 273, 360], + [101, 273, 360], + [102, 273, 360], + [103, 274, 360], + [104, 274, 360], + [105, 275, 360], + [106, 0, 95],[106, 275, 360], + [107, 0, 95],[107, 275, 360], + [108, 0, 95],[108, 275, 360], + [109, 0, 95], + [110, 0, 95], + [111, 0, 90], + [112, 0, 93], + [113, 0, 93], + [114, 0, 93], + [115, 0, 93], + [116, 0, 93], + [117, 0, 93], + [118, 0, 96],[118, 265, 360], + [119, 0, 96],[119, 265, 360], + [120, 0, 98],[120, 265, 360], + [121, 0, 98],[121, 265, 360], + [122, 0, 98],[122, 258, 360], + [123, 0, 98],[123, 258, 360], + [124, 0, 98],[124, 258, 360], + [125, 0, 100],[125, 256, 360], + [126, 0, 100],[126, 256, 360], + [127, 0, 100],[127, 256, 360], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml new file mode 100644 index 00000000..3ae4770d --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/front_upper/point_filters_two_third.param.yaml @@ -0,0 +1,146 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 93],[0, 266, 360], + [1, 0, 93],[1, 266, 360], + [2, 0, 93],[2, 266, 360], + [3, 0, 93],[3, 266, 360], + [4, 268, 360], + [5, 268, 360], + [6, 268, 360], + [7, 268, 360], + [8, 268, 360], + [10, 272, 360], + [11, 272, 360], + [12, 272, 360], + [13, 272, 360], + [14, 266, 360], + [15, 266, 360], + [16, 266, 360], + [17, 0, 96],[17, 266, 360], + [18, 0, 96],[18, 263, 360], + [19, 0, 96],[19, 263, 360], + [20, 0, 96],[20, 263, 360], + [21, 0, 96],[21, 263, 360], + [22, 270, 360], + [23, 270, 360], + [24, 270, 360], + [29, 0, 96], + [30, 0, 96],[30, 267, 360], + [31, 0, 96],[31, 267, 360], + [32, 0, 96],[32, 267, 360], + [33, 0, 96], + [38, 272, 360], + [45, 0, 93],[45, 267, 360], + [50, 0, 98], + [51, 0, 98], + [52, 0, 98], + [53, 0, 98], + [54, 0, 98], + [57, 0, 93], + [58, 0, 93],[58, 261, 360], + [59, 0, 93],[59, 261, 360], + [60, 0, 93],[60, 261, 360], + [61, 0, 93], + [72, 269, 360], + [73, 0, 88],[73, 269, 360], + [74, 0, 88],[74, 269, 360], + [75, 0, 90], + [76, 0, 94], + [77, 0, 94], + [78, 0, 94], + [79, 0, 94], + [80, 0, 94], + [81, 0, 90], + [82, 0, 90], + [83, 0, 90], + [84, 0, 88], + [85, 0, 88], + [86, 0, 88], + [88, 273, 360], + [89, 273, 360], + [90, 273, 360], + [91, 0, 93],[91, 273, 360], + [92, 0, 93],[92, 268, 360], + [93, 0, 93],[93, 273, 360], + [94, 0, 93],[94, 273, 360], + [95, 0, 93],[95, 273, 360], + [96, 0, 93],[96, 273, 360], + [97, 0, 93],[97, 273, 360], + [98, 0, 93],[98, 273, 360], + [99, 0, 93],[99, 273, 360], + [100, 0, 93.5],[100, 273, 360], + [101, 273, 360], + [102, 273, 360], + [103, 274, 360], + [104, 274, 360], + [105, 275, 360], + [106, 0, 95],[106, 275, 360], + [107, 0, 95],[107, 275, 360], + [108, 0, 95],[108, 275, 360], + [109, 0, 95], + [110, 0, 95], + [111, 0, 90], + [112, 0, 93], + [113, 0, 93], + [114, 0, 93], + [115, 0, 93], + [116, 0, 93], + [117, 0, 93], + [118, 0, 96],[118, 265, 360], + [119, 0, 96],[119, 265, 360], + [120, 0, 98],[120, 265, 360], + [121, 0, 98],[121, 265, 360], + [122, 0, 98],[122, 258, 360], + [123, 0, 98],[123, 258, 360], + [124, 0, 98],[124, 258, 360], + [125, 0, 100],[125, 256, 360], + [126, 0, 100],[126, 256, 360], + [127, 0, 100],[127, 256, 360], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml new file mode 100644 index 00000000..943a4c5d --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_full.param.yaml @@ -0,0 +1,62 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [1, 0, 360], + [2, 0, 360], + [3, 0, 170],[3, 199.5, 360], + [4, 0, 155],[4, 203, 360], + [5, 0, 152],[5, 206, 217],[5, 220, 360], + [6, 0, 146],[6, 217, 360], + [7, 0, 143],[7, 230, 360], + [8, 0, 143],[8, 228.5, 360], + [9, 0, 142],[9, 228.5, 360], + [10, 0, 141],[10, 235, 360], + [11, 0, 140],[11, 235, 360], + [12, 0, 140],[12, 235, 360], + [13, 0, 135],[13, 235, 360], + [14, 0, 133],[14, 235, 360], + [15, 0, 131],[15, 235, 360], + [16, 0, 129],[16, 235, 360], + [17, 0, 127],[17, 235, 360], + [18, 0, 127],[18, 235, 360], + [19, 0, 126],[19, 235, 360], + [20, 0, 125],[20, 235, 360], + [21, 0, 125],[21, 235, 360], + [22, 0, 124],[22, 235, 360], + [23, 0, 124],[23, 235, 360], + [24, 0, 122],[24, 235, 360], + [25, 0, 122],[25, 235, 360], + [26, 0, 121],[26, 235, 360], + [27, 0, 120],[27, 235, 360], + [28, 0, 119],[28, 239, 360], + [29, 0, 118],[29, 240, 360], + [30, 0, 110],[30, 240, 360], + [31, 0, 109],[31, 240, 267], + [32, 0, 109],[32, 244, 267], + [33, 0, 108],[33, 244, 267], + [34, 0, 108],[34, 244, 265], + [35, 0, 107],[35, 244, 265], + [36, 0, 107],[36, 244, 265], + [37, 0, 106],[37, 244, 265], + [38, 0, 106],[38, 244, 265], + [39, 0, 105],[39, 244, 265], + [40, 0, 105],[40, 244, 265], + [41, 0, 104],[41, 244, 265], + [42, 0, 104],[42, 244, 265], + [43, 0, 103],[43, 244, 265], + [44, 0, 102], + [45, 0, 101], + [46, 0, 96], + [47, 0, 95], + [48, 0, 95], + [49, 0, 94], + [50, 0, 93], + [51, 0, 93], + [52, 0, 92], + [53, 0, 92], + [54, 0, 92] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml new file mode 100644 index 00000000..66e55a5f --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_one_third.param.yaml @@ -0,0 +1,147 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [1, 0, 360], + [2, 0, 170],[2, 192, 360], + [3, 0, 160],[3, 199.5, 360], + [4, 0, 155],[4, 203, 360], + [5, 0, 152],[5, 206, 217],[5, 220, 360], + [6, 0, 146],[6, 217, 360], + [7, 0, 143],[7, 230, 360], + [8, 0, 139],[8, 228.5, 360], + [9, 0, 135],[9, 228.5, 360], + [10, 0, 133],[10, 235, 360], + [11, 0, 132],[11, 235, 360], + [12, 0, 131],[12, 235, 360], + [13, 0, 129],[13, 235, 360], + [14, 0, 127],[14, 235, 360], + [15, 0, 126],[15, 235, 360], + [16, 0, 124],[16, 235, 360], + [17, 0, 122],[17, 235, 360], + [18, 0, 121],[18, 235, 360], + [19, 0, 120],[19, 235, 360], + [20, 0, 119],[20, 235, 360], + [21, 0, 118],[21, 235, 360], + [22, 0, 117],[22, 235, 360], + [23, 0, 116],[23, 235, 360], + [24, 0, 115],[24, 235, 360], + [25, 0, 114],[25, 235, 360], + [26, 0, 113],[26, 235, 360], + [27, 0, 112],[27, 235, 360], + [28, 0, 119],[28, 239, 360], + [29, 0, 118],[29, 240, 360], + [30, 0, 110],[30, 240, 360], + [31, 0, 109],[31, 240, 267], + [32, 0, 109],[32, 244, 267], + [33, 0, 108],[33, 244, 267], + [34, 0, 108],[34, 244, 265], + [35, 0, 107],[35, 244, 265], + [36, 0, 107],[36, 244, 265], + [37, 0, 106],[37, 244, 265], + [38, 0, 106],[38, 244, 265], + [39, 0, 105],[39, 244, 265], + [40, 0, 105],[40, 244, 265], + [41, 0, 104],[41, 244, 265], + [42, 0, 104],[42, 244, 265], + [43, 0, 103],[43, 244, 265], + [44, 0, 102], + [45, 0, 101], + [46, 0, 96], + [47, 0, 95], + [48, 0, 95], + [49, 0, 94], + [50, 0, 93], + [51, 0, 93], + [52, 0, 92], + [53, 0, 92], + [54, 0, 92] + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml new file mode 100644 index 00000000..e21ce758 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/left_lower/point_filters_two_third.param.yaml @@ -0,0 +1,105 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [1, 0, 360], + [2, 0, 170],[2, 192, 360], + [3, 0, 160],[3, 199.5, 360], + [4, 0, 155],[4, 203, 360], + [5, 0, 152],[5, 206, 217],[5, 220, 360], + [6, 0, 146],[6, 217, 360], + [7, 0, 143],[7, 230, 360], + [8, 0, 139],[8, 228.5, 360], + [9, 0, 135],[9, 228.5, 360], + [10, 0, 133],[10, 235, 360], + [11, 0, 132],[11, 235, 360], + [12, 0, 131],[12, 235, 360], + [13, 0, 129],[13, 235, 360], + [14, 0, 127],[14, 235, 360], + [15, 0, 126],[15, 235, 360], + [16, 0, 124],[16, 235, 360], + [17, 0, 122],[17, 235, 360], + [18, 0, 121],[18, 235, 360], + [19, 0, 120],[19, 235, 360], + [20, 0, 119],[20, 235, 360], + [21, 0, 118],[21, 235, 360], + [22, 0, 117],[22, 235, 360], + [23, 0, 116],[23, 235, 360], + [24, 0, 115],[24, 235, 360], + [25, 0, 114],[25, 235, 360], + [26, 0, 113],[26, 235, 360], + [27, 0, 112],[27, 235, 360], + [28, 0, 119],[28, 239, 360], + [29, 0, 118],[29, 240, 360], + [30, 0, 110],[30, 240, 360], + [31, 0, 109],[31, 240, 267], + [32, 0, 109],[32, 244, 267], + [33, 0, 108],[33, 244, 267], + [34, 0, 108],[34, 244, 265], + [35, 0, 107],[35, 244, 265], + [36, 0, 107],[36, 244, 265], + [37, 0, 106],[37, 244, 265], + [38, 0, 106],[38, 244, 265], + [39, 0, 105],[39, 244, 265], + [40, 0, 105],[40, 244, 265], + [41, 0, 104],[41, 244, 265], + [42, 0, 104],[42, 244, 265], + [43, 0, 103],[43, 244, 265], + [44, 0, 102], + [45, 0, 101], + [46, 0, 96], + [47, 0, 95], + [48, 0, 95], + [49, 0, 94], + [50, 0, 93], + [51, 0, 93], + [52, 0, 92], + [53, 0, 92], + [54, 0, 92], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_full.param.yaml new file mode 100644 index 00000000..0303a1d4 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_full.param.yaml @@ -0,0 +1,72 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [11, 322, 336],[11, 354, 360], + [15, 143.95, 147.1],[15, 318, 360], + [19, 330, 360], + [23, 330, 360], + [31, 348, 360], + [38, 350, 360], + [45, 339, 360], + [52, 339, 360], + [59, 99, 101.5],[59, 330, 360], + [60, 330, 341], + [61, 330, 341], + [62, 330, 341], + [63, 330, 341], + [64, 330, 341], + [65, 330, 341], + [66, 319, 360], + [67, 330, 342], + [68, 330, 342], + [69, 330, 342], + [70, 317, 360], + [71, 331, 341], + [72, 319, 322],[72, 331, 341], + [73, 316, 360], + [74, 318, 341], + [75, 318, 341], + [76, 318, 341], + [77, 318, 360], + [77, 318, 360], + [78, 318, 360], + [79, 315, 360], + [80, 318, 360], + [81, 318, 360], + [82, 318, 360], + [83, 318, 360], + [84, 319, 360], + [85, 319, 360], + [86, 319, 360], + [87, 320, 360], + [88, 315, 360], + [89, 322, 323], + [91, 287.55, 289.90], + [92, 287.55, 289.75],[92, 315, 360], + [96, 316, 360], + [100, 316, 360], + [104, 316, 360], + [108, 314, 360], + [109, 314.74, 60], + [110, 312.74, 360], + [111, 312.73, 360], + [112, 308.48, 360], + [113, 305.68, 360], + [114, 308.58, 360], + [115, 307, 360], + [116, 303, 360], + [117, 303, 360], + [118, 303.81, 360], + [119, 302.09, 360], + [120, 298.42, 360], + [121, 90, 91],[121, 299.63, 360], + [122, 90, 92],[122, 298.42, 360], + [123, 90, 92],[123, 298.42, 360], + [124, 90, 93],[124, 293, 360], + [125, 90, 93],[125, 293, 360], + [126, 90, 93],[126, 292.12, 360], + [127, 90, 93],[127, 289.71, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_one_third.param.yaml new file mode 100644 index 00000000..d6117158 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_one_third.param.yaml @@ -0,0 +1,157 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [11, 322, 336],[11, 354, 360], + [15, 143.95, 147.1],[15, 318, 360], + [19, 330, 360], + [23, 330, 360], + [31, 348, 360], + [38, 350, 360], + [45, 339, 360], + [52, 339, 360], + [59, 99, 101.5],[59, 330, 360], + [60, 330, 341], + [61, 330, 341], + [62, 330, 341], + [63, 330, 341], + [64, 330, 341], + [65, 330, 341], + [66, 319, 360], + [67, 330, 342], + [68, 330, 342], + [69, 330, 342], + [70, 317, 360], + [71, 331, 341], + [72, 319, 322],[72, 331, 341], + [73, 316, 360], + [74, 318, 341], + [75, 318, 341], + [76, 318, 341], + [77, 318, 360], + [77, 318, 360], + [78, 318, 360], + [79, 315, 360], + [80, 318, 360], + [81, 318, 360], + [82, 318, 360], + [83, 318, 360], + [84, 319, 360], + [85, 319, 360], + [86, 319, 360], + [87, 320, 360], + [88, 315, 360], + [89, 322, 323], + [91, 287.55, 289.90], + [92, 287.55, 289.75],[92, 315, 360], + [96, 316, 360], + [100, 316, 360], + [104, 316, 360], + [108, 314, 360], + [109, 314.74, 60], + [110, 312.74, 360], + [111, 312.73, 360], + [112, 308.48, 360], + [113, 305.68, 360], + [114, 308.58, 360], + [115, 307, 360], + [116, 303, 360], + [117, 303, 360], + [118, 303.81, 360], + [119, 302.09, 360], + [120, 298.42, 360], + [121, 90, 91],[121, 299.63, 360], + [122, 90, 92],[122, 298.42, 360], + [123, 90, 92],[123, 298.42, 360], + [124, 90, 93],[124, 293, 360], + [125, 90, 93],[125, 293, 360], + [126, 90, 93],[126, 292.12, 360], + [127, 90, 93],[127, 289.71, 360], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_two_third.param.yaml new file mode 100644 index 00000000..2022217a --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/left_upper/point_filters_two_third.param.yaml @@ -0,0 +1,115 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [11, 322, 336],[11, 354, 360], + [15, 143.95, 147.1],[15, 318, 360], + [19, 330, 360], + [23, 330, 360], + [31, 348, 360], + [38, 350, 360], + [45, 339, 360], + [52, 339, 360], + [59, 99, 101.5],[59, 330, 360], + [60, 330, 341], + [61, 330, 341], + [62, 330, 341], + [63, 330, 341], + [64, 330, 341], + [65, 330, 341], + [66, 319, 360], + [67, 330, 342], + [68, 330, 342], + [69, 330, 342], + [70, 317, 360], + [71, 331, 341], + [72, 319, 322],[72, 331, 341], + [73, 316, 360], + [74, 318, 341], + [75, 318, 341], + [76, 318, 341], + [77, 318, 360], + [77, 318, 360], + [78, 318, 360], + [79, 315, 360], + [80, 318, 360], + [81, 318, 360], + [82, 318, 360], + [83, 318, 360], + [84, 319, 360], + [85, 319, 360], + [86, 319, 360], + [87, 320, 360], + [88, 315, 360], + [89, 322, 323], + [91, 287.55, 289.90], + [92, 287.55, 289.75],[92, 315, 360], + [96, 316, 360], + [100, 316, 360], + [104, 316, 360], + [108, 314, 360], + [109, 314.74, 60], + [110, 312.74, 360], + [111, 312.73, 360], + [112, 308.48, 360], + [113, 305.68, 360], + [114, 308.58, 360], + [115, 307, 360], + [116, 303, 360], + [117, 303, 360], + [118, 303.81, 360], + [119, 302.09, 360], + [120, 298.42, 360], + [121, 90, 91],[121, 299.63, 360], + [122, 90, 92],[122, 298.42, 360], + [123, 90, 92],[123, 298.42, 360], + [124, 90, 93],[124, 293, 360], + [125, 90, 93],[125, 293, 360], + [126, 90, 93],[126, 292.12, 360], + [127, 90, 93],[127, 289.71, 360], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_full.param.yaml new file mode 100644 index 00000000..ee183856 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_full.param.yaml @@ -0,0 +1,55 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60, 148], [0, 212,360], + [1, 60, 144], [1, 216,360], + [2, 60, 142], [2, 218,360], + [3, 60, 140], [3, 220,360], + [4, 60, 138], [4, 222,360], + [5, 60, 138], [5, 222,360], + [6, 60, 134], [6, 226,360], + [7, 60, 134], [7, 226,360], + [8, 60, 132], [8, 228,360], + [9, 60, 132], [9, 228,360], + [10, 60, 130], [10, 230,360], + [11, 60, 120], [11, 240,360], + [12, 60, 118], [12, 242,360], + [13, 60, 118], [13, 242,360], + [14, 60, 118], [14, 242,360], + [15, 60, 118], [15, 242,360], + [16, 60, 118], [16, 242,360], + [17, 60, 118], [17, 242,360], + [18, 60, 116], [18, 244,360], + [19, 60, 116], [19, 244,360], + [20, 60, 112], [20, 248,360], + [21, 60, 112], [21, 248,360], + [22, 60, 110], [22, 250,360], + [23, 60, 110], [23, 250,360], + [24, 60, 110], [24, 250,360], + [25, 60, 110], [25, 250,360], + [26, 60, 110], [26, 250,360], + [27, 60, 106], [27, 254,360], + [28, 60, 106], [28, 254,360], + [29, 60, 106], [29, 254,360], + [30, 60, 104], [30, 256,360], + [31, 60, 104], [31, 256,360], + [32, 60, 104], [32, 256,360], + [33, 60, 104], [33, 256,360], + [34, 60, 104], [34, 256,360], + [35, 60, 100], [35, 260,360], + [36, 60, 100], [36, 260,360], + [37, 60, 100], [37, 260,360], + [38, 60, 100], [38, 260,360], + [39, 60, 96], [39, 264,360], + [40, 60, 96], [40, 264,360], + [41, 60, 96], [41, 264,360], + [42, 60, 96], [42, 264,360], + [43, 60, 96], [43, 264,360], + [44, 60, 94], [44, 266,360], + [45, 60, 94], [45, 266,360], + [46, 60, 94], [46, 266,360], + [47, 60, 94], [47, 266,360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_one_third.param.yaml new file mode 100644 index 00000000..d4fe2b58 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_one_third.param.yaml @@ -0,0 +1,140 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60, 148], [0, 212,360], + [1, 60, 144], [1, 216,360], + [2, 60, 142], [2, 218,360], + [3, 60, 140], [3, 220,360], + [4, 60, 138], [4, 222,360], + [5, 60, 138], [5, 222,360], + [6, 60, 134], [6, 226,360], + [7, 60, 134], [7, 226,360], + [8, 60, 132], [8, 228,360], + [9, 60, 132], [9, 228,360], + [10, 60, 130], [10, 230,360], + [11, 60, 120], [11, 240,360], + [12, 60, 118], [12, 242,360], + [13, 60, 118], [13, 242,360], + [14, 60, 118], [14, 242,360], + [15, 60, 118], [15, 242,360], + [16, 60, 118], [16, 242,360], + [17, 60, 118], [17, 242,360], + [18, 60, 116], [18, 244,360], + [19, 60, 116], [19, 244,360], + [20, 60, 112], [20, 248,360], + [21, 60, 112], [21, 248,360], + [22, 60, 110], [22, 250,360], + [23, 60, 110], [23, 250,360], + [24, 60, 110], [24, 250,360], + [25, 60, 110], [25, 250,360], + [26, 60, 110], [26, 250,360], + [27, 60, 106], [27, 254,360], + [28, 60, 106], [28, 254,360], + [29, 60, 106], [29, 254,360], + [30, 60, 104], [30, 256,360], + [31, 60, 104], [31, 256,360], + [32, 60, 104], [32, 256,360], + [33, 60, 104], [33, 256,360], + [34, 60, 104], [34, 256,360], + [35, 60, 100], [35, 260,360], + [36, 60, 100], [36, 260,360], + [37, 60, 100], [37, 260,360], + [38, 60, 100], [38, 260,360], + [39, 60, 96], [39, 264,360], + [40, 60, 96], [40, 264,360], + [41, 60, 96], [41, 264,360], + [42, 60, 96], [42, 264,360], + [43, 60, 96], [43, 264,360], + [44, 60, 94], [44, 266,360], + [45, 60, 94], [45, 266,360], + [46, 60, 94], [46, 266,360], + [47, 60, 94], [47, 266,360], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_two_third.param.yaml new file mode 100644 index 00000000..b77ffde6 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/rear_lower/point_filters_two_third.param.yaml @@ -0,0 +1,98 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 60, 148], [0, 212,360], + [1, 60, 144], [1, 216,360], + [2, 60, 142], [2, 218,360], + [3, 60, 140], [3, 220,360], + [4, 60, 138], [4, 222,360], + [5, 60, 138], [5, 222,360], + [6, 60, 134], [6, 226,360], + [7, 60, 134], [7, 226,360], + [8, 60, 132], [8, 228,360], + [9, 60, 132], [9, 228,360], + [10, 60, 130], [10, 230,360], + [11, 60, 120], [11, 240,360], + [12, 60, 118], [12, 242,360], + [13, 60, 118], [13, 242,360], + [14, 60, 118], [14, 242,360], + [15, 60, 118], [15, 242,360], + [16, 60, 118], [16, 242,360], + [17, 60, 118], [17, 242,360], + [18, 60, 116], [18, 244,360], + [19, 60, 116], [19, 244,360], + [20, 60, 112], [20, 248,360], + [21, 60, 112], [21, 248,360], + [22, 60, 110], [22, 250,360], + [23, 60, 110], [23, 250,360], + [24, 60, 110], [24, 250,360], + [25, 60, 110], [25, 250,360], + [26, 60, 110], [26, 250,360], + [27, 60, 106], [27, 254,360], + [28, 60, 106], [28, 254,360], + [29, 60, 106], [29, 254,360], + [30, 60, 104], [30, 256,360], + [31, 60, 104], [31, 256,360], + [32, 60, 104], [32, 256,360], + [33, 60, 104], [33, 256,360], + [34, 60, 104], [34, 256,360], + [35, 60, 100], [35, 260,360], + [36, 60, 100], [36, 260,360], + [37, 60, 100], [37, 260,360], + [38, 60, 100], [38, 260,360], + [39, 60, 96], [39, 264,360], + [40, 60, 96], [40, 264,360], + [41, 60, 96], [41, 264,360], + [42, 60, 96], [42, 264,360], + [43, 60, 96], [43, 264,360], + [44, 60, 94], [44, 266,360], + [45, 60, 94], [45, 266,360], + [46, 60, 94], [46, 266,360], + [47, 60, 94], [47, 266,360], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_full.param.yaml new file mode 100644 index 00000000..abbbc01c --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_full.param.yaml @@ -0,0 +1,125 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 95],[0, 270, 360], + [1, 0, 95],[1, 270, 360], + [2, 0, 95],[2, 270, 360], + [3, 0, 95],[3, 270, 360], + [4, 0, 98],[4, 270, 360], + [5, 0, 98],[5, 270, 360], + [6, 0, 98],[6, 270, 360], + [7, 0, 98],[7, 270, 360], + [8, 0, 98],[8, 270, 360], + [9, 0, 99],[9, 270, 360], + [10, 0, 99],[10, 270, 360], + [11, 0, 99],[11, 270, 360], + [12, 0, 99],[12, 270, 360], + [13, 0, 104],[13, 270, 360], + [14, 0, 104],[14, 265, 360], + [15, 0, 104],[15, 265, 360], + [16, 0, 104],[16, 265, 360], + [17, 0, 104],[17, 263, 360], + [18, 0, 103],[18, 263, 360], + [19, 0, 103],[19, 263, 360], + [20, 0, 103],[20, 263, 360], + [21, 0, 103],[21, 263, 360], + [22, 0, 102],[22, 264, 360], + [23, 0, 102],[23, 264, 360], + [24, 0, 102],[24, 264, 360], + [25, 0, 102], + [29, 0, 106], + [30, 0, 106],[30,264, 360], + [31, 0, 106],[31,264, 360], + [32, 0, 106],[32,264, 360], + [33, 0, 106], + [34, 0, 90], + [35, 0, 90], + [36, 0, 102], + [37, 0, 102], + [38, 0, 102], + [39, 0, 102], + [40, 0, 102], + [41, 0, 90], + [42, 0, 90], + [43, 0, 101], + [44, 0, 101],[44, 262, 360], + [45, 0, 101],[45, 262, 360], + [46, 0, 101],[46, 262, 360], + [47, 0, 101],[47, 262, 360], + [50, 0, 106],[50, 268, 360], + [51, 0, 106],[51, 268, 360], + [52, 0, 106],[52, 268, 360], + [53, 0, 106],[53, 268, 360], + [54, 0, 106],[54, 268, 360], + [57, 0, 106],[57, 269, 360], + [58, 0, 106],[58, 269, 360], + [59, 0, 106],[59, 269, 360], + [60, 0, 106],[60, 269, 360], + [61, 0, 106],[61, 269, 360], + [62, 0, 100], + [63, 0, 100], + [64, 0, 100], + [65, 0, 100],[65, 259.5, 360], + [66, 0, 100],[66, 259.5, 360], + [67, 0, 100],[67, 259.5, 360], + [68, 0, 100], + [72, 265, 360], + [73, 265, 360], + [74, 265, 360], + [75, 265, 360], + [76, 265, 360], + [77, 265, 360], + [78, 0, 98],[78, 262, 360], + [79, 0, 98],[79, 262, 360], + [80, 0, 98],[80, 262, 360], + [81, 0, 98],[81, 262, 360], + [82, 0, 98],[82, 262, 360], + [83, 0, 98],[83, 262, 360], + [84, 0, 98],[84, 262, 360], + [85, 0, 98],[85, 262, 360], + [86, 0, 98],[86, 262, 360], + [87, 0, 100],[87, 262, 360], + [88, 0, 100],[88, 262, 360], + [89, 0, 100],[89, 262, 360], + [90, 0, 103], [90, 262, 360], + [91, 0, 103], [91, 266, 360], + [92, 0, 103], [92, 266, 360], + [93, 0, 103], [93, 266, 360], + [94, 0, 106], [94, 266, 360], + [95, 0, 106],[95, 266, 360], + [96, 0, 106],[96, 266, 360], + [97, 0, 106],[97, 266, 360], + [98, 0, 108],[98, 268, 360], + [99, 0, 108],[99, 268, 360], + [100, 0, 108],[100, 268, 360], + [101, 0, 108],[101, 268, 360], + [102, 0, 108],[102, 268, 360], + [103, 0, 108],[103, 268, 360], + [104, 0, 108],[104, 268, 360], + [105, 0, 108],[105, 268, 360], + [106, 0, 108],[106, 266, 360], + [107, 0, 106],[107, 266, 360], + [108, 0, 106],[108, 266, 360], + [109, 0, 106],[109, 266, 360], + [110, 0, 106],[110, 266, 360], + [111, 0, 106],[111, 266, 360], + [112, 0, 106],[112, 266, 360], + [113, 0, 106],[113, 266, 360], + [114, 0, 106],[114, 266, 360], + [115, 0, 106],[115, 266, 360], + [116, 0, 106],[116, 266, 360], + [117, 0, 107],[117, 266, 360], + [118, 0, 107],[118, 264, 360], + [119, 0, 107],[119, 264, 360], + [120, 0, 107],[120, 260, 360], + [121, 0, 107],[121, 260, 360], + [122, 0, 107],[122, 260, 360], + [123, 0, 110], [123, 260, 360], + [124, 0, 110], [124, 258, 360], + [125, 0, 110], [125, 258, 360], + [126, 0, 110], [126, 258, 360], + [127, 0, 110], [127, 258, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_one_third.param.yaml new file mode 100644 index 00000000..331627ca --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_one_third.param.yaml @@ -0,0 +1,210 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 95],[0, 270, 360], + [1, 0, 95],[1, 270, 360], + [2, 0, 95],[2, 270, 360], + [3, 0, 95],[3, 270, 360], + [4, 0, 98],[4, 270, 360], + [5, 0, 98],[5, 270, 360], + [6, 0, 98],[6, 270, 360], + [7, 0, 98],[7, 270, 360], + [8, 0, 98],[8, 270, 360], + [9, 0, 99],[9, 270, 360], + [10, 0, 99],[10, 270, 360], + [11, 0, 99],[11, 270, 360], + [12, 0, 99],[12, 270, 360], + [13, 0, 104],[13, 270, 360], + [14, 0, 104],[14, 265, 360], + [15, 0, 104],[15, 265, 360], + [16, 0, 104],[16, 265, 360], + [17, 0, 104],[17, 263, 360], + [18, 0, 103],[18, 263, 360], + [19, 0, 103],[19, 263, 360], + [20, 0, 103],[20, 263, 360], + [21, 0, 103],[21, 263, 360], + [22, 0, 102],[22, 264, 360], + [23, 0, 102],[23, 264, 360], + [24, 0, 102],[24, 264, 360], + [25, 0, 102], + [29, 0, 106], + [30, 0, 106],[30,264, 360], + [31, 0, 106],[31,264, 360], + [32, 0, 106],[32,264, 360], + [33, 0, 106], + [34, 0, 90], + [35, 0, 90], + [36, 0, 102], + [37, 0, 102], + [38, 0, 102], + [39, 0, 102], + [40, 0, 102], + [41, 0, 90], + [42, 0, 90], + [43, 0, 101], + [44, 0, 101],[44, 262, 360], + [45, 0, 101],[45, 262, 360], + [46, 0, 101],[46, 262, 360], + [47, 0, 101],[47, 262, 360], + [50, 0, 106],[50, 268, 360], + [51, 0, 106],[51, 268, 360], + [52, 0, 106],[52, 268, 360], + [53, 0, 106],[53, 268, 360], + [54, 0, 106],[54, 268, 360], + [57, 0, 106],[57, 269, 360], + [58, 0, 106],[58, 269, 360], + [59, 0, 106],[59, 269, 360], + [60, 0, 106],[60, 269, 360], + [61, 0, 106],[61, 269, 360], + [62, 0, 100], + [63, 0, 100], + [64, 0, 100], + [65, 0, 100],[65, 259.5, 360], + [66, 0, 100],[66, 259.5, 360], + [67, 0, 100],[67, 259.5, 360], + [68, 0, 100], + [72, 265, 360], + [73, 265, 360], + [74, 265, 360], + [75, 265, 360], + [76, 265, 360], + [77, 265, 360], + [78, 0, 98],[78, 262, 360], + [79, 0, 98],[79, 262, 360], + [80, 0, 98],[80, 262, 360], + [81, 0, 98],[81, 262, 360], + [82, 0, 98],[82, 262, 360], + [83, 0, 98],[83, 262, 360], + [84, 0, 98],[84, 262, 360], + [85, 0, 98],[85, 262, 360], + [86, 0, 98],[86, 262, 360], + [87, 0, 100],[87, 262, 360], + [88, 0, 100],[88, 262, 360], + [89, 0, 100],[89, 262, 360], + [90, 0, 103], [90, 262, 360], + [91, 0, 103], [91, 266, 360], + [92, 0, 103], [92, 266, 360], + [93, 0, 103], [93, 266, 360], + [94, 0, 106], [94, 266, 360], + [95, 0, 106],[95, 266, 360], + [96, 0, 106],[96, 266, 360], + [97, 0, 106],[97, 266, 360], + [98, 0, 108],[98, 268, 360], + [99, 0, 108],[99, 268, 360], + [100, 0, 108],[100, 268, 360], + [101, 0, 108],[101, 268, 360], + [102, 0, 108],[102, 268, 360], + [103, 0, 108],[103, 268, 360], + [104, 0, 108],[104, 268, 360], + [105, 0, 108],[105, 268, 360], + [106, 0, 108],[106, 266, 360], + [107, 0, 106],[107, 266, 360], + [108, 0, 106],[108, 266, 360], + [109, 0, 106],[109, 266, 360], + [110, 0, 106],[110, 266, 360], + [111, 0, 106],[111, 266, 360], + [112, 0, 106],[112, 266, 360], + [113, 0, 106],[113, 266, 360], + [114, 0, 106],[114, 266, 360], + [115, 0, 106],[115, 266, 360], + [116, 0, 106],[116, 266, 360], + [117, 0, 107],[117, 266, 360], + [118, 0, 107],[118, 264, 360], + [119, 0, 107],[119, 264, 360], + [120, 0, 107],[120, 260, 360], + [121, 0, 107],[121, 260, 360], + [122, 0, 107],[122, 260, 360], + [123, 0, 110], [123, 260, 360], + [124, 0, 110], [124, 258, 360], + [125, 0, 110], [125, 258, 360], + [126, 0, 110], [126, 258, 360], + [127, 0, 110], [127, 258, 360], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_two_third.param.yaml new file mode 100644 index 00000000..a31c771c --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/rear_upper/point_filters_two_third.param.yaml @@ -0,0 +1,168 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 95],[0, 270, 360], + [1, 0, 95],[1, 270, 360], + [2, 0, 95],[2, 270, 360], + [3, 0, 95],[3, 270, 360], + [4, 0, 98],[4, 270, 360], + [5, 0, 98],[5, 270, 360], + [6, 0, 98],[6, 270, 360], + [7, 0, 98],[7, 270, 360], + [8, 0, 98],[8, 270, 360], + [9, 0, 99],[9, 270, 360], + [10, 0, 99],[10, 270, 360], + [11, 0, 99],[11, 270, 360], + [12, 0, 99],[12, 270, 360], + [13, 0, 104],[13, 270, 360], + [14, 0, 104],[14, 265, 360], + [15, 0, 104],[15, 265, 360], + [16, 0, 104],[16, 265, 360], + [17, 0, 104],[17, 263, 360], + [18, 0, 103],[18, 263, 360], + [19, 0, 103],[19, 263, 360], + [20, 0, 103],[20, 263, 360], + [21, 0, 103],[21, 263, 360], + [22, 0, 102],[22, 264, 360], + [23, 0, 102],[23, 264, 360], + [24, 0, 102],[24, 264, 360], + [25, 0, 102], + [29, 0, 106], + [30, 0, 106],[30,264, 360], + [31, 0, 106],[31,264, 360], + [32, 0, 106],[32,264, 360], + [33, 0, 106], + [34, 0, 90], + [35, 0, 90], + [36, 0, 102], + [37, 0, 102], + [38, 0, 102], + [39, 0, 102], + [40, 0, 102], + [41, 0, 90], + [42, 0, 90], + [43, 0, 101], + [44, 0, 101],[44, 262, 360], + [45, 0, 101],[45, 262, 360], + [46, 0, 101],[46, 262, 360], + [47, 0, 101],[47, 262, 360], + [50, 0, 106],[50, 268, 360], + [51, 0, 106],[51, 268, 360], + [52, 0, 106],[52, 268, 360], + [53, 0, 106],[53, 268, 360], + [54, 0, 106],[54, 268, 360], + [57, 0, 106],[57, 269, 360], + [58, 0, 106],[58, 269, 360], + [59, 0, 106],[59, 269, 360], + [60, 0, 106],[60, 269, 360], + [61, 0, 106],[61, 269, 360], + [62, 0, 100], + [63, 0, 100], + [64, 0, 100], + [65, 0, 100],[65, 259.5, 360], + [66, 0, 100],[66, 259.5, 360], + [67, 0, 100],[67, 259.5, 360], + [68, 0, 100], + [72, 265, 360], + [73, 265, 360], + [74, 265, 360], + [75, 265, 360], + [76, 265, 360], + [77, 265, 360], + [78, 0, 98],[78, 262, 360], + [79, 0, 98],[79, 262, 360], + [80, 0, 98],[80, 262, 360], + [81, 0, 98],[81, 262, 360], + [82, 0, 98],[82, 262, 360], + [83, 0, 98],[83, 262, 360], + [84, 0, 98],[84, 262, 360], + [85, 0, 98],[85, 262, 360], + [86, 0, 98],[86, 262, 360], + [87, 0, 100],[87, 262, 360], + [88, 0, 100],[88, 262, 360], + [89, 0, 100],[89, 262, 360], + [90, 0, 103], [90, 262, 360], + [91, 0, 103], [91, 266, 360], + [92, 0, 103], [92, 266, 360], + [93, 0, 103], [93, 266, 360], + [94, 0, 106], [94, 266, 360], + [95, 0, 106],[95, 266, 360], + [96, 0, 106],[96, 266, 360], + [97, 0, 106],[97, 266, 360], + [98, 0, 108],[98, 268, 360], + [99, 0, 108],[99, 268, 360], + [100, 0, 108],[100, 268, 360], + [101, 0, 108],[101, 268, 360], + [102, 0, 108],[102, 268, 360], + [103, 0, 108],[103, 268, 360], + [104, 0, 108],[104, 268, 360], + [105, 0, 108],[105, 268, 360], + [106, 0, 108],[106, 266, 360], + [107, 0, 106],[107, 266, 360], + [108, 0, 106],[108, 266, 360], + [109, 0, 106],[109, 266, 360], + [110, 0, 106],[110, 266, 360], + [111, 0, 106],[111, 266, 360], + [112, 0, 106],[112, 266, 360], + [113, 0, 106],[113, 266, 360], + [114, 0, 106],[114, 266, 360], + [115, 0, 106],[115, 266, 360], + [116, 0, 106],[116, 266, 360], + [117, 0, 107],[117, 266, 360], + [118, 0, 107],[118, 264, 360], + [119, 0, 107],[119, 264, 360], + [120, 0, 107],[120, 260, 360], + [121, 0, 107],[121, 260, 360], + [122, 0, 107],[122, 260, 360], + [123, 0, 110], [123, 260, 360], + [124, 0, 110], [124, 258, 360], + [125, 0, 110], [125, 258, 360], + [126, 0, 110], [126, 258, 360], + [127, 0, 110], [127, 258, 360], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml new file mode 100644 index 00000000..ec2a65e9 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_full.param.yaml @@ -0,0 +1,60 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [1, 0, 360], + [2, 0, 360], + [3, 0, 165],[3, 190, 360], + [4, 0, 158],[4, 203, 360], + [5, 0, 157],[5, 205, 360], + [6, 0, 156],[6, 211, 360], + [7, 0, 154],[7, 214, 360], + [8, 0, 152],[8, 217, 360], + [9, 0, 152],[9, 220, 360], + [10, 0, 151],[10, 221, 360], + [11, 0, 151],[11, 226, 360], + [12, 0, 151],[12, 229, 360], + [13, 0, 151],[13, 231, 360], + [14, 0, 150],[14, 232, 360], + [15, 0, 150],[15, 233, 360], + [16, 0, 150],[16, 234, 360], + [17, 0, 147],[17, 235, 360], + [18, 0, 148],[18, 235, 360], + [19, 0, 146],[19, 236, 360], + [20, 97, 143],[20, 240, 360], + [21, 102, 125],[21, 241, 360], + [22, 105, 125],[22, 241, 360], + [23, 105, 125],[23, 244, 360], + [24, 105, 125],[24, 245, 360], + [25, 106, 124],[25, 246, 360], + [26, 107, 123],[26, 247, 360], + [27, 108, 122],[27, 248, 360], + [28, 109, 120],[28, 248, 360], + [29, 110, 118],[29, 250, 360], + [30, 111, 117],[30, 250, 360], + [31, 112.5, 116],[31, 251, 360], + [32, 251, 360], + [33, 252, 360], + [34, 252, 360], + [35, 253, 360], + [36, 253, 360], + [37, 254, 360], + [38, 254, 360], + [39, 255, 360], + [40, 255, 360], + [41, 256, 360], + [42, 256, 360], + [43, 257, 360], + [44, 258, 360], + [45, 259, 360], + [46, 264, 360], + [47, 265, 360], + [48, 265, 360], + [49, 266, 360], + [50, 267, 360], + [51, 267, 360], + [52, 268, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml new file mode 100644 index 00000000..6e755be6 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_one_third.param.yaml @@ -0,0 +1,145 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [1, 0, 360], + [2, 0, 360], + [3, 0, 165],[3, 190, 360], + [4, 0, 158],[4, 203, 360], + [5, 0, 157],[5, 205, 360], + [6, 0, 156],[6, 211, 360], + [7, 0, 154],[7, 214, 360], + [8, 0, 152],[8, 217, 360], + [9, 0, 152],[9, 220, 360], + [10, 0, 151],[10, 221, 360], + [11, 0, 151],[11, 226, 360], + [12, 0, 151],[12, 229, 360], + [13, 0, 151],[13, 231, 360], + [14, 0, 150],[14, 232, 360], + [15, 0, 150],[15, 233, 360], + [16, 0, 150],[16, 234, 360], + [17, 0, 147],[17, 235, 360], + [18, 0, 148],[18, 235, 360], + [19, 0, 146],[19, 236, 360], + [20, 97, 143],[20, 240, 360], + [21, 102, 125],[21, 241, 360], + [22, 105, 125],[22, 241, 360], + [23, 105, 125],[23, 244, 360], + [24, 105, 125],[24, 245, 360], + [25, 106, 124],[25, 246, 360], + [26, 107, 123],[26, 247, 360], + [27, 108, 122],[27, 248, 360], + [28, 109, 120],[28, 248, 360], + [29, 110, 118],[29, 250, 360], + [30, 111, 117],[30, 250, 360], + [31, 112.5, 116],[31, 251, 360], + [32, 251, 360], + [33, 252, 360], + [34, 252, 360], + [35, 253, 360], + [36, 253, 360], + [37, 254, 360], + [38, 254, 360], + [39, 255, 360], + [40, 255, 360], + [41, 256, 360], + [42, 256, 360], + [43, 257, 360], + [44, 258, 360], + [45, 259, 360], + [46, 264, 360], + [47, 265, 360], + [48, 265, 360], + [49, 266, 360], + [50, 267, 360], + [51, 267, 360], + [52, 268, 360], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml new file mode 100644 index 00000000..54889649 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/right_lower/point_filters_two_third.param.yaml @@ -0,0 +1,103 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 360], + [1, 0, 360], + [2, 0, 360], + [3, 0, 165],[3, 190, 360], + [4, 0, 158],[4, 203, 360], + [5, 0, 157],[5, 205, 360], + [6, 0, 156],[6, 211, 360], + [7, 0, 154],[7, 214, 360], + [8, 0, 152],[8, 217, 360], + [9, 0, 152],[9, 220, 360], + [10, 0, 151],[10, 221, 360], + [11, 0, 151],[11, 226, 360], + [12, 0, 151],[12, 229, 360], + [13, 0, 151],[13, 231, 360], + [14, 0, 150],[14, 232, 360], + [15, 0, 150],[15, 233, 360], + [16, 0, 150],[16, 234, 360], + [17, 0, 147],[17, 235, 360], + [18, 0, 148],[18, 235, 360], + [19, 0, 146],[19, 236, 360], + [20, 97, 143],[20, 240, 360], + [21, 102, 125],[21, 241, 360], + [22, 105, 125],[22, 241, 360], + [23, 105, 125],[23, 244, 360], + [24, 105, 125],[24, 245, 360], + [25, 106, 124],[25, 246, 360], + [26, 107, 123],[26, 247, 360], + [27, 108, 122],[27, 248, 360], + [28, 109, 120],[28, 248, 360], + [29, 110, 118],[29, 250, 360], + [30, 111, 117],[30, 250, 360], + [31, 112.5, 116],[31, 251, 360], + [32, 251, 360], + [33, 252, 360], + [34, 252, 360], + [35, 253, 360], + [36, 253, 360], + [37, 254, 360], + [38, 254, 360], + [39, 255, 360], + [40, 255, 360], + [41, 256, 360], + [42, 256, 360], + [43, 257, 360], + [44, 258, 360], + [45, 259, 360], + [46, 264, 360], + [47, 265, 360], + [48, 265, 360], + [49, 266, 360], + [50, 267, 360], + [51, 267, 360], + [52, 268, 360], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_full.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_full.param.yaml new file mode 100644 index 00000000..5e5fb040 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_full.param.yaml @@ -0,0 +1,122 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 15], + [1, 0, 15], + [2, 0, 15], + [3, 0, 15], + [17, 0, 10], + [18, 0, 10], + [19, 0, 10], + [20, 0, 10], + [21, 0, 10], + [22, 0, 10], + [23, 0, 10], + [24, 0, 10], + [25, 0, 10], + [26, 0, 10], + [27, 0, 10], + [28, 0, 10], + [29, 0, 10], + [30, 0, 15], + [31, 0, 15], + [32, 0, 15], + [33, 0, 10], + [34, 0, 10], + [35, 0, 10], + [36, 0, 10], + [37, 0, 10], + [38, 0, 10], + [39, 0, 10], + [40, 0, 10], + [41, 0, 10], + [42, 0, 10], + [43, 0, 10], + [44, 0, 10], + [45, 0, 25], + [46, 0, 25], + [47, 0, 25], + [48, 0, 26], + [49, 0, 26], + [50, 0, 27], + [51, 0, 40], + [52, 0, 40], + [53, 0, 40], + [54, 0, 40], + [55, 0, 40], + [56, 0, 40], + [57, 0, 45], + [58, 0, 45], + [59, 0, 45], + [60, 0, 45], + [61, 0, 40], + [62, 0, 35], + [63, 0, 35], + [64, 0, 35], + [65, 0, 35], + [66, 0, 40], + [67, 0, 40],[67, 269, 360], + [68, 0, 42],[68, 269, 360], + [69, 0, 42],[69, 269, 360], + [70, 0, 42],[70, 269, 360], + [71, 0, 44],[71, 269, 360], + [72, 0, 44],[72, 269, 360], + [73, 0, 44],[73, 269, 360], + [74, 0, 44],[74, 269, 360], + [75, 0, 44],[75, 269, 360], + [76, 0, 45],[76, 269, 360], + [77, 0, 45],[77, 269, 360], + [78, 0, 45],[78, 269, 360], + [79, 0, 45],[79, 269, 360], + [80, 0, 45],[80, 269, 360], + [81, 0, 45],[81, 269, 360], + [82, 0, 45],[82, 269, 360], + [83, 0, 45],[83, 269, 360], + [84, 0, 45],[84, 269, 360], + [85, 0, 45],[85, 269, 360], + [86, 0, 45],[86, 269, 360], + [87, 0, 45],[87, 269, 360], + [88, 0, 45],[88, 269, 360], + [89, 0, 45],[89, 269, 360], + [90, 0, 45],[90, 269, 360], + [91, 0, 45],[91, 269, 360], + [92, 0, 45],[92, 269, 360], + [93, 0, 44],[93, 269, 360], + [94, 0, 44],[94, 269, 360], + [95, 0, 44],[95, 269, 360], + [96, 0, 44],[96, 269, 360], + [97, 0, 44],[97, 269, 360], + [98, 0, 44],[98, 269, 360], + [99, 0, 44],[99, 269, 360], + [100, 0, 46],[100, 269, 360], + [101, 0, 46],[101, 269, 360], + [102, 0, 46],[102, 269, 360], + [103, 0, 44],[103, 269, 360], + [104, 0, 44],[104, 269, 360], + [105, 0, 44],[105, 269, 360], + [106, 0, 44],[106, 269, 360], + [107, 0, 44],[107, 269, 360], + [108, 0, 46],[108, 269, 360], + [109, 0, 47],[109, 269, 360], + [110, 0, 48],[110, 87.78, 89.97],[110, 269, 360], + [111, 0, 49],[111, 269, 360], + [112, 0, 50],[112, 269, 360], + [113, 0, 52],[113, 269, 360], + [114, 0, 53],[114, 269, 360], + [115, 0, 54],[115, 269, 360], + [116, 0, 55],[116, 269, 360], + [117, 0, 56],[117, 269, 360], + [118, 0, 57],[118, 269, 360], + [119, 0, 58],[119, 269, 360], + [120, 0, 59],[120, 269, 360], + [121, 0, 61],[121, 269, 360], + [122, 0, 63],[122, 268, 360], + [123, 0, 66],[123, 268, 360], + [124, 0, 69],[124, 268, 360], + [125, 0, 69],[125, 267, 360], + [126, 0, 71],[126, 267, 360], + [127, 0, 72],[127, 267, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_one_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_one_third.param.yaml new file mode 100644 index 00000000..e903fa97 --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_one_third.param.yaml @@ -0,0 +1,207 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 15], + [1, 0, 15], + [2, 0, 15], + [3, 0, 15], + [17, 0, 10], + [18, 0, 10], + [19, 0, 10], + [20, 0, 10], + [21, 0, 10], + [22, 0, 10], + [23, 0, 10], + [24, 0, 10], + [25, 0, 10], + [26, 0, 10], + [27, 0, 10], + [28, 0, 10], + [29, 0, 10], + [30, 0, 15], + [31, 0, 15], + [32, 0, 15], + [33, 0, 10], + [34, 0, 10], + [35, 0, 10], + [36, 0, 10], + [37, 0, 10], + [38, 0, 10], + [39, 0, 10], + [40, 0, 10], + [41, 0, 10], + [42, 0, 10], + [43, 0, 10], + [44, 0, 10], + [45, 0, 25], + [46, 0, 25], + [47, 0, 25], + [48, 0, 26], + [49, 0, 26], + [50, 0, 27], + [51, 0, 40], + [52, 0, 40], + [53, 0, 40], + [54, 0, 40], + [55, 0, 40], + [56, 0, 40], + [57, 0, 45], + [58, 0, 45], + [59, 0, 45], + [60, 0, 45], + [61, 0, 40], + [62, 0, 35], + [63, 0, 35], + [64, 0, 35], + [65, 0, 35], + [66, 0, 40], + [67, 0, 40],[67, 269, 360], + [68, 0, 42],[68, 269, 360], + [69, 0, 42],[69, 269, 360], + [70, 0, 42],[70, 269, 360], + [71, 0, 44],[71, 269, 360], + [72, 0, 44],[72, 269, 360], + [73, 0, 44],[73, 269, 360], + [74, 0, 44],[74, 269, 360], + [75, 0, 44],[75, 269, 360], + [76, 0, 45],[76, 269, 360], + [77, 0, 45],[77, 269, 360], + [78, 0, 45],[78, 269, 360], + [79, 0, 45],[79, 269, 360], + [80, 0, 45],[80, 269, 360], + [81, 0, 45],[81, 269, 360], + [82, 0, 45],[82, 269, 360], + [83, 0, 45],[83, 269, 360], + [84, 0, 45],[84, 269, 360], + [85, 0, 45],[85, 269, 360], + [86, 0, 45],[86, 269, 360], + [87, 0, 45],[87, 269, 360], + [88, 0, 45],[88, 269, 360], + [89, 0, 45],[89, 269, 360], + [90, 0, 45],[90, 269, 360], + [91, 0, 45],[91, 269, 360], + [92, 0, 45],[92, 269, 360], + [93, 0, 44],[93, 269, 360], + [94, 0, 44],[94, 269, 360], + [95, 0, 44],[95, 269, 360], + [96, 0, 44],[96, 269, 360], + [97, 0, 44],[97, 269, 360], + [98, 0, 44],[98, 269, 360], + [99, 0, 44],[99, 269, 360], + [100, 0, 46],[100, 269, 360], + [101, 0, 46],[101, 269, 360], + [102, 0, 46],[102, 269, 360], + [103, 0, 44],[103, 269, 360], + [104, 0, 44],[104, 269, 360], + [105, 0, 44],[105, 269, 360], + [106, 0, 44],[106, 269, 360], + [107, 0, 44],[107, 269, 360], + [108, 0, 46],[108, 269, 360], + [109, 0, 47],[109, 269, 360], + [110, 0, 48],[110, 87.78, 89.97],[110, 269, 360], + [111, 0, 49],[111, 269, 360], + [112, 0, 50],[112, 269, 360], + [113, 0, 52],[113, 269, 360], + [114, 0, 53],[114, 269, 360], + [115, 0, 54],[115, 269, 360], + [116, 0, 55],[116, 269, 360], + [117, 0, 56],[117, 269, 360], + [118, 0, 57],[118, 269, 360], + [119, 0, 58],[119, 269, 360], + [120, 0, 59],[120, 269, 360], + [121, 0, 61],[121, 269, 360], + [122, 0, 63],[122, 268, 360], + [123, 0, 66],[123, 268, 360], + [124, 0, 69],[124, 268, 360], + [125, 0, 69],[125, 267, 360], + [126, 0, 71],[126, 267, 360], + [127, 0, 72],[127, 267, 360], + [1, 0, 360], + [2, 0, 360], + [4, 0, 360], + [5, 0, 360], + [7, 0, 360], + [8, 0, 360], + [10, 0, 360], + [11, 0, 360], + [13, 0, 360], + [14, 0, 360], + [16, 0, 360], + [17, 0, 360], + [19, 0, 360], + [20, 0, 360], + [22, 0, 360], + [23, 0, 360], + [25, 0, 360], + [26, 0, 360], + [28, 0, 360], + [29, 0, 360], + [31, 0, 360], + [32, 0, 360], + [34, 0, 360], + [35, 0, 360], + [37, 0, 360], + [38, 0, 360], + [40, 0, 360], + [41, 0, 360], + [43, 0, 360], + [44, 0, 360], + [46, 0, 360], + [47, 0, 360], + [49, 0, 360], + [50, 0, 360], + [52, 0, 360], + [53, 0, 360], + [55, 0, 360], + [56, 0, 360], + [58, 0, 360], + [59, 0, 360], + [61, 0, 360], + [62, 0, 360], + [64, 0, 360], + [65, 0, 360], + [67, 0, 360], + [68, 0, 360], + [70, 0, 360], + [71, 0, 360], + [73, 0, 360], + [74, 0, 360], + [76, 0, 360], + [77, 0, 360], + [79, 0, 360], + [80, 0, 360], + [82, 0, 360], + [83, 0, 360], + [85, 0, 360], + [86, 0, 360], + [88, 0, 360], + [89, 0, 360], + [91, 0, 360], + [92, 0, 360], + [94, 0, 360], + [95, 0, 360], + [97, 0, 360], + [98, 0, 360], + [100, 0, 360], + [101, 0, 360], + [103, 0, 360], + [104, 0, 360], + [106, 0, 360], + [107, 0, 360], + [109, 0, 360], + [110, 0, 360], + [112, 0, 360], + [113, 0, 360], + [115, 0, 360], + [116, 0, 360], + [118, 0, 360], + [119, 0, 360], + [121, 0, 360], + [122, 0, 360], + [124, 0, 360], + [125, 0, 360], + [127, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_two_third.param.yaml b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_two_third.param.yaml new file mode 100644 index 00000000..015d8cfc --- /dev/null +++ b/aip_x2_gen2_launch/config/point_filters/right_upper/point_filters_two_third.param.yaml @@ -0,0 +1,165 @@ +/**: + ros__parameters: + point_filters: | + { + "ring_section_filter": [ + [0, 0, 15], + [1, 0, 15], + [2, 0, 15], + [3, 0, 15], + [17, 0, 10], + [18, 0, 10], + [19, 0, 10], + [20, 0, 10], + [21, 0, 10], + [22, 0, 10], + [23, 0, 10], + [24, 0, 10], + [25, 0, 10], + [26, 0, 10], + [27, 0, 10], + [28, 0, 10], + [29, 0, 10], + [30, 0, 15], + [31, 0, 15], + [32, 0, 15], + [33, 0, 10], + [34, 0, 10], + [35, 0, 10], + [36, 0, 10], + [37, 0, 10], + [38, 0, 10], + [39, 0, 10], + [40, 0, 10], + [41, 0, 10], + [42, 0, 10], + [43, 0, 10], + [44, 0, 10], + [45, 0, 25], + [46, 0, 25], + [47, 0, 25], + [48, 0, 26], + [49, 0, 26], + [50, 0, 27], + [51, 0, 40], + [52, 0, 40], + [53, 0, 40], + [54, 0, 40], + [55, 0, 40], + [56, 0, 40], + [57, 0, 45], + [58, 0, 45], + [59, 0, 45], + [60, 0, 45], + [61, 0, 40], + [62, 0, 35], + [63, 0, 35], + [64, 0, 35], + [65, 0, 35], + [66, 0, 40], + [67, 0, 40],[67, 269, 360], + [68, 0, 42],[68, 269, 360], + [69, 0, 42],[69, 269, 360], + [70, 0, 42],[70, 269, 360], + [71, 0, 44],[71, 269, 360], + [72, 0, 44],[72, 269, 360], + [73, 0, 44],[73, 269, 360], + [74, 0, 44],[74, 269, 360], + [75, 0, 44],[75, 269, 360], + [76, 0, 45],[76, 269, 360], + [77, 0, 45],[77, 269, 360], + [78, 0, 45],[78, 269, 360], + [79, 0, 45],[79, 269, 360], + [80, 0, 45],[80, 269, 360], + [81, 0, 45],[81, 269, 360], + [82, 0, 45],[82, 269, 360], + [83, 0, 45],[83, 269, 360], + [84, 0, 45],[84, 269, 360], + [85, 0, 45],[85, 269, 360], + [86, 0, 45],[86, 269, 360], + [87, 0, 45],[87, 269, 360], + [88, 0, 45],[88, 269, 360], + [89, 0, 45],[89, 269, 360], + [90, 0, 45],[90, 269, 360], + [91, 0, 45],[91, 269, 360], + [92, 0, 45],[92, 269, 360], + [93, 0, 44],[93, 269, 360], + [94, 0, 44],[94, 269, 360], + [95, 0, 44],[95, 269, 360], + [96, 0, 44],[96, 269, 360], + [97, 0, 44],[97, 269, 360], + [98, 0, 44],[98, 269, 360], + [99, 0, 44],[99, 269, 360], + [100, 0, 46],[100, 269, 360], + [101, 0, 46],[101, 269, 360], + [102, 0, 46],[102, 269, 360], + [103, 0, 44],[103, 269, 360], + [104, 0, 44],[104, 269, 360], + [105, 0, 44],[105, 269, 360], + [106, 0, 44],[106, 269, 360], + [107, 0, 44],[107, 269, 360], + [108, 0, 46],[108, 269, 360], + [109, 0, 47],[109, 269, 360], + [110, 0, 48],[110, 87.78, 89.97],[110, 269, 360], + [111, 0, 49],[111, 269, 360], + [112, 0, 50],[112, 269, 360], + [113, 0, 52],[113, 269, 360], + [114, 0, 53],[114, 269, 360], + [115, 0, 54],[115, 269, 360], + [116, 0, 55],[116, 269, 360], + [117, 0, 56],[117, 269, 360], + [118, 0, 57],[118, 269, 360], + [119, 0, 58],[119, 269, 360], + [120, 0, 59],[120, 269, 360], + [121, 0, 61],[121, 269, 360], + [122, 0, 63],[122, 268, 360], + [123, 0, 66],[123, 268, 360], + [124, 0, 69],[124, 268, 360], + [125, 0, 69],[125, 267, 360], + [126, 0, 71],[126, 267, 360], + [127, 0, 72],[127, 267, 360], + [0, 0, 360], + [3, 0, 360], + [6, 0, 360], + [9, 0, 360], + [12, 0, 360], + [15, 0, 360], + [18, 0, 360], + [21, 0, 360], + [24, 0, 360], + [27, 0, 360], + [30, 0, 360], + [33, 0, 360], + [36, 0, 360], + [39, 0, 360], + [42, 0, 360], + [45, 0, 360], + [48, 0, 360], + [51, 0, 360], + [54, 0, 360], + [57, 0, 360], + [60, 0, 360], + [63, 0, 360], + [66, 0, 360], + [69, 0, 360], + [72, 0, 360], + [75, 0, 360], + [78, 0, 360], + [81, 0, 360], + [84, 0, 360], + [87, 0, 360], + [90, 0, 360], + [93, 0, 360], + [96, 0, 360], + [99, 0, 360], + [102, 0, 360], + [105, 0, 360], + [108, 0, 360], + [111, 0, 360], + [114, 0, 360], + [117, 0, 360], + [120, 0, 360], + [123, 0, 360], + [126, 0, 360] + ] + } diff --git a/aip_x2_gen2_launch/config/simple_object_merger.param.yaml b/aip_x2_gen2_launch/config/simple_object_merger.param.yaml new file mode 100644 index 00000000..b85c9ce6 --- /dev/null +++ b/aip_x2_gen2_launch/config/simple_object_merger.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + update_rate_hz: 20.0 + new_frame_id: "base_link" + timeout_threshold: 1.0 + input_topics: ["/sensing/radar/front_center/detected_objects", "/sensing/radar/rear_center/detected_objects"] diff --git a/aip_x2_gen2_launch/data/traffic_light_camera.yaml b/aip_x2_gen2_launch/data/traffic_light_camera.yaml new file mode 100644 index 00000000..458ad17c --- /dev/null +++ b/aip_x2_gen2_launch/data/traffic_light_camera.yaml @@ -0,0 +1,20 @@ +image_width: 1920 +image_height: 1080 +camera_name: traffic_light/camera +camera_matrix: + rows: 3 + cols: 3 + data: [2410.755261, 0.000000, 922.621401, 0.000000, 2403.573140, 534.752500, 0.000000, 0.000000, 1.000000] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.126600, 0.152594, 0.002432, -0.001244, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] +projection_matrix: + rows: 3 + cols: 4 + data: [2370.254883, 0.000000, 920.136018, 0.000000, 0.000000, 2388.885254, 535.599668, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/aip_x2_gen2_launch/launch/ars548.launch.xml b/aip_x2_gen2_launch/launch/ars548.launch.xml new file mode 100644 index 00000000..29c03ecb --- /dev/null +++ b/aip_x2_gen2_launch/launch/ars548.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/gnss.launch.xml b/aip_x2_gen2_launch/launch/gnss.launch.xml new file mode 100644 index 00000000..7f9e5d3f --- /dev/null +++ b/aip_x2_gen2_launch/launch/gnss.launch.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml new file mode 100644 index 00000000..4adc9311 --- /dev/null +++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml new file mode 100644 index 00000000..443294ba --- /dev/null +++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/imu.launch.xml b/aip_x2_gen2_launch/launch/imu.launch.xml new file mode 100644 index 00000000..fd07632c --- /dev/null +++ b/aip_x2_gen2_launch/launch/imu.launch.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml new file mode 100644 index 00000000..bddb9a2a --- /dev/null +++ b/aip_x2_gen2_launch/launch/lidar.launch.xml @@ -0,0 +1,287 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py new file mode 100644 index 00000000..6f17007b --- /dev/null +++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py @@ -0,0 +1,333 @@ +# Copyright 2023 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration + +# from launch.conditions import LaunchConfigurationNotEquals +from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def get_lidar_make(sensor_name): + if sensor_name[:6].lower() == "pandar": + return "Hesai", ".csv" + elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: + return "Velodyne", ".yaml" + return "unrecognized_sensor_model" + + +def get_vehicle_info(context): + # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support + # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py + gp = context.launch_configurations.get("ros_params", {}) + if not gp: + gp = dict(context.launch_configurations.get("global_params", {})) + p = {} + p["vehicle_length"] = gp["front_overhang"] + gp["wheel_base"] + gp["rear_overhang"] + p["vehicle_width"] = gp["wheel_tread"] + gp["left_overhang"] + gp["right_overhang"] + p["min_longitudinal_offset"] = -gp["rear_overhang"] + p["max_longitudinal_offset"] = gp["front_overhang"] + gp["wheel_base"] + p["min_lateral_offset"] = -(gp["wheel_tread"] / 2.0 + gp["right_overhang"]) + p["max_lateral_offset"] = gp["wheel_tread"] / 2.0 + gp["left_overhang"] + p["min_height_offset"] = 0.0 + p["max_height_offset"] = gp["vehicle_height"] + return p + + +def launch_setup(context, *args, **kwargs): + def load_composable_node_param(param_path): + with open(LaunchConfiguration(param_path).perform(context), "r") as f: + return yaml.safe_load(f)["/**"]["ros__parameters"] + + def create_parameter_dict(*args): + result = {} + for x in args: + result[x] = LaunchConfiguration(x) + return result + + def str2vector(string): + return [float(x) for x in string.strip("[]").split(",")] + + # Model and make + sensor_model = LaunchConfiguration("sensor_model").perform(context) + sensor_make, sensor_extension = get_lidar_make(sensor_model) + + glog_component = ComposableNode( + package="autoware_glog_component", + plugin="GlogComponent", + name="glog_component", + ) + + nebula_component = ComposableNode( + package="nebula_ros", + plugin=sensor_make + "RosWrapper", + name=sensor_make.lower() + "_ros_wrapper_node", + parameters=[ + { + "sensor_model": sensor_model, + **create_parameter_dict( + "host_ip", + "sensor_ip", + "multicast_ip", + "data_port", + "return_mode", + "min_range", + "max_range", + "frame_id", + "sync_angle", + "cut_angle", + "dual_return_distance_threshold", + "rotation_speed", + "cloud_min_angle", + "cloud_max_angle", + "gnss_port", + "packet_mtu_size", + "setup_sensor", + "ptp_profile", + "ptp_transport_type", + "ptp_switch_type", + "ptp_domain", + "diag_span", + "calibration_file", + "launch_hw", + ), + "retry_hw": True, + }, + ] + + [load_composable_node_param("point_filters_param_file")], + remappings=[ + # ("aw_points", "pointcloud_raw"), + ("pandar_points", "pointcloud_raw_ex"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + cropbox_parameters = create_parameter_dict("input_frame", "output_frame") + cropbox_parameters["negative"] = True + + vehicle_info = get_vehicle_info(context) + cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] + cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] + cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"] + cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"] + cropbox_parameters["min_z"] = vehicle_info["min_height_offset"] + cropbox_parameters["max_z"] = vehicle_info["max_height_offset"] + + self_crop_component = ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent", + name="crop_box_filter_self", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "self_cropped/pointcloud_ex"), + ], + parameters=[cropbox_parameters], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + undistort_component = ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent", + name="distortion_corrector_node", + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("~/input/imu", "/sensing/imu/imu_data"), + ("~/input/pointcloud", "self_cropped/pointcloud_ex"), + ("~/output/pointcloud", "rectified/pointcloud_ex"), + ], + parameters=[load_composable_node_param("distortion_corrector_node_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + ring_outlier_filter_component = ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent", + name="ring_outlier_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + dual_return_filter_component = ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::DualReturnOutlierFilterComponent", + name="dual_return_filter", + remappings=[ + ("input", "rectified/pointcloud_ex"), + ("output", "pointcloud"), + ], + parameters=[ + { + "vertical_bins": LaunchConfiguration("vertical_bins"), + "min_azimuth_deg": LaunchConfiguration("min_azimuth_deg"), + "max_azimuth_deg": LaunchConfiguration("max_azimuth_deg"), + } + ] + + [load_composable_node_param("dual_return_filter_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range"))) + blockage_diag_component = ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::BlockageDiagComponent", + name="blockage_return_diag", + remappings=[ + ("input", "pointcloud_raw_ex"), + ("output", "blockage_diag/pointcloud"), + ], + parameters=[ + { + "angle_range": LaunchConfiguration("blockage_range"), + "horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"), + "vertical_bins": LaunchConfiguration("vertical_bins"), + "is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"), + "max_distance_range": distance_range[1], + "horizontal_resolution": LaunchConfiguration("horizontal_resolution"), + } + ] + + [load_composable_node_param("blockage_diagnostics_param_file")], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + container = ComposableNodeContainer( + name="nebula_node_container", + namespace="autoware_pointcloud_preprocessor", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + glog_component, + nebula_component, + self_crop_component, + undistort_component, + ], + ) + + ring_outlier_filter_loader = LoadComposableNodes( + composable_node_descriptions=[ring_outlier_filter_component], + target_container=container, + condition=LaunchConfigurationNotEquals("return_mode", "Dual"), + ) + + dual_return_filter_loader = LoadComposableNodes( + composable_node_descriptions=[dual_return_filter_component], + target_container=container, + condition=LaunchConfigurationEquals("return_mode", "Dual"), + ) + + blockage_diag_loader = LoadComposableNodes( + composable_node_descriptions=[blockage_diag_component], + target_container=container, + condition=launch.conditions.IfCondition(LaunchConfiguration("enable_blockage_diag")), + ) + + return [ + container, + ring_outlier_filter_loader, + dual_return_filter_loader, + blockage_diag_loader, + ] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + # a default_value of None is equivalent to not passing that kwarg at all + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + add_launch_arg("sensor_model", description="sensor model name") + add_launch_arg("config_file", "", description="sensor configuration file") + add_launch_arg("launch_hw", "True", "do launch driver") + add_launch_arg("setup_sensor", "True", "configure sensor") + add_launch_arg("sensor_ip", "192.168.1.201", "device ip address") + add_launch_arg( + "multicast_ip", + "", + "the multicast group the sensor shall broadcast to. leave empty to disable multicast", + ) + add_launch_arg("host_ip", "255.255.255.255", "host ip address") + add_launch_arg("sync_angle", "0") + add_launch_arg("cut_angle", "0.0") + # add_launch_arg("point_filters", "{}", "point filter definitions in JSON format") + add_launch_arg("base_frame", "base_link", "base frame id") + add_launch_arg("min_range", "0.3", "minimum view range for Velodyne sensors") + add_launch_arg("max_range", "300.0", "maximum view range for Velodyne sensors") + add_launch_arg("cloud_min_angle", "0", "minimum view angle setting on device") + add_launch_arg("cloud_max_angle", "360", "maximum view angle setting on device") + add_launch_arg("data_port", "2368", "device data port number") + add_launch_arg("gnss_port", "2380", "device gnss port number") + add_launch_arg("packet_mtu_size", "1500", "packet mtu size") + add_launch_arg("rotation_speed", "600", "rotational frequency") + add_launch_arg("dual_return_distance_threshold", "0.1", "dual return distance threshold") + add_launch_arg("frame_id", "lidar", "frame id") + add_launch_arg("input_frame", LaunchConfiguration("base_frame"), "use for cropbox") + add_launch_arg("output_frame", LaunchConfiguration("base_frame"), "use for cropbox") + add_launch_arg("diag_span", "1000") + add_launch_arg("use_multithread", "False", "use multithread") + add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") + add_launch_arg("container_name", "nebula_node_container") + + add_launch_arg("dual_return_filter_param_file") + add_launch_arg( + "blockage_diagnostics_param_file", + [FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics.param.yaml"], + ) + add_launch_arg( + "distortion_corrector_node_param_file", + [FindPackageShare("common_sensor_launch"), "/config/distortion_corrector_node.param.yaml"], + ) + add_launch_arg("vertical_bins", "128") + add_launch_arg("horizontal_ring_id", "12") + add_launch_arg("blockage_range", "[270.0, 90.0]") + + add_launch_arg("min_azimuth_deg", "135.0") + add_launch_arg("max_azimuth_deg", "225.0") + add_launch_arg("enable_blockage_diag", "true") + add_launch_arg("point_filters_param_file") + + add_launch_arg("calibration_file", "") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py new file mode 100644 index 00000000..0775e3a2 --- /dev/null +++ b/aip_x2_gen2_launch/launch/pointcloud_preprocessor.launch.py @@ -0,0 +1,93 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def launch_setup(context, *args, **kwargs): + # set concat filter as a component + concat_component = ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="concatenate_data", + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "concatenated/pointcloud"), + ], + parameters=[ + { + "input_topics": [ + "/sensing/lidar/front_upper/pointcloud", + "/sensing/lidar/front_lower/pointcloud", + "/sensing/lidar/left_upper/pointcloud", + "/sensing/lidar/left_lower/pointcloud", + "/sensing/lidar/right_upper/pointcloud", + "/sensing/lidar/right_lower/pointcloud", + "/sensing/lidar/rear_upper/pointcloud", + "/sensing/lidar/rear_lower/pointcloud", + ], + "input_offset": [0.005, 0.025, 0.050, 0.005, 0.050, 0.005, 0.005, 0.025], + "timeout_sec": 0.075, + "output_frame": LaunchConfiguration("base_frame"), + "input_twist_topic_type": "twist", + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + concat_loader = LoadComposableNodes( + composable_node_descriptions=[concat_component], + target_container=LaunchConfiguration("pointcloud_container_name"), + ) + + return [concat_loader] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None): + launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) + + add_launch_arg("base_frame", "base_link") + add_launch_arg("use_multithread", "True") + add_launch_arg("use_intra_process", "True") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/aip_x2_gen2_launch/launch/radar.launch.xml b/aip_x2_gen2_launch/launch/radar.launch.xml new file mode 100644 index 00000000..d793d8de --- /dev/null +++ b/aip_x2_gen2_launch/launch/radar.launch.xml @@ -0,0 +1,203 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/sensing.launch.xml b/aip_x2_gen2_launch/launch/sensing.launch.xml new file mode 100644 index 00000000..8000211f --- /dev/null +++ b/aip_x2_gen2_launch/launch/sensing.launch.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py b/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py new file mode 100644 index 00000000..0eafa9e0 --- /dev/null +++ b/aip_x2_gen2_launch/launch/topic_state_monitor.launch.py @@ -0,0 +1,195 @@ +# Copyright 2024 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + # GNSS topic monitor + gnss_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_gnss_pose", + parameters=[ + { + "topic": "/sensing/gnss/pose", + "topic_type": "geometry_msgs/msg/PoseStamped", + "best_effort": True, + "diag_name": "gnss_topic_status", + "warn_rate": 2.5, + "error_rate": 0.5, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # IMU topic monitor + imu_topic_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_imu_data", + parameters=[ + { + "topic": "/sensing/imu/imu_data", + "topic_type": "sensor_msgs/msg/Imu", + "best_effort": True, + "diag_name": "imu_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # Radar topic monitors + radar_front_center_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_front_center", + parameters=[ + { + "topic": "/sensing/radar/front_center/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_front_center_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_front_left_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_front_left", + parameters=[ + { + "topic": "/sensing/radar/front_left/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_front_left_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_front_right_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_front_right", + parameters=[ + { + "topic": "/sensing/radar/front_right/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_front_right_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_rear_center_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_rear_center", + parameters=[ + { + "topic": "/sensing/radar/rear_center/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_rear_center_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_rear_left_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_rear_left", + parameters=[ + { + "topic": "/sensing/radar/rear_left/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_rear_left_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + radar_rear_right_monitor = ComposableNode( + package="topic_state_monitor", + plugin="topic_state_monitor::TopicStateMonitorNode", + name="topic_state_monitor_radar_rear_right", + parameters=[ + { + "topic": "/sensing/radar/rear_right/objects_raw", + "topic_type": "radar_msgs/msg/RadarTracks", + "best_effort": True, + "diag_name": "radar_rear_right_topic_status", + "warn_rate": 5.0, + "error_rate": 1.0, + "timeout": 5.0, + "window_size": 10, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + + # ComposableNodeContainer to run all ComposableNodes + container = ComposableNodeContainer( + name="topic_state_monitor_container", + namespace="topic_state_monitor", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + gnss_topic_monitor, + imu_topic_monitor, + radar_front_center_monitor, + radar_front_left_monitor, + radar_front_right_monitor, + radar_rear_center_monitor, + radar_rear_left_monitor, + radar_rear_right_monitor, + ], + output="screen", + ) + + return LaunchDescription([container]) diff --git a/aip_x2_gen2_launch/package.xml b/aip_x2_gen2_launch/package.xml new file mode 100644 index 00000000..685c1424 --- /dev/null +++ b/aip_x2_gen2_launch/package.xml @@ -0,0 +1,34 @@ + + + + aip_x2_gen2_launch + 0.1.0 + The aip_x2_gen2_launch package + + Tomohito Ando + Apache License 2.0 + + ament_cmake_auto + + autoware_glog_component + autoware_gnss_poser + autoware_imu_corrector + autoware_pointcloud_preprocessor + autoware_radar_tracks_msgs_converter + autoware_simple_object_merger + autoware_vehicle_velocity_converter + common_sensor_launch + dummy_diag_publisher + septentrio_gnss_driver + tamagawa_imu_driver + topic_tools + ublox_gps + usb_cam + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/aip_xx1_description/CMakeLists.txt b/aip_xx1_description/CMakeLists.txt index 50723262..362fb6da 100644 --- a/aip_xx1_description/CMakeLists.txt +++ b/aip_xx1_description/CMakeLists.txt @@ -2,9 +2,12 @@ cmake_minimum_required(VERSION 3.5) project(aip_xx1_description) find_package(ament_cmake_auto REQUIRED) +find_package(aip_urdf_compiler REQUIRED) ament_auto_find_build_dependencies() +aip_cmake_urdf_compile() + ament_auto_package(INSTALL_TO_SHARE urdf config diff --git a/aip_xx1_description/config/sensor_kit_calibration.yaml b/aip_xx1_description/config/sensor_kit_calibration.yaml index ab417b52..bc589de3 100644 --- a/aip_xx1_description/config/sensor_kit_calibration.yaml +++ b/aip_xx1_description/config/sensor_kit_calibration.yaml @@ -6,6 +6,7 @@ sensor_kit_base_link: roll: -0.025 pitch: 0.315 yaw: 1.035 + type: monocular_camera camera1/camera_link: x: -0.10731 y: -0.56343 @@ -13,6 +14,7 @@ sensor_kit_base_link: roll: -0.025 pitch: 0.32 yaw: -2.12 + type: monocular_camera camera2/camera_link: x: 0.10731 y: -0.56343 @@ -20,6 +22,7 @@ sensor_kit_base_link: roll: -0.00 pitch: 0.335 yaw: -1.04 + type: monocular_camera camera3/camera_link: x: -0.10731 y: 0.56343 @@ -27,6 +30,7 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.325 yaw: 2.0943951 + type: monocular_camera camera4/camera_link: x: 0.07356 y: 0.0 @@ -34,6 +38,7 @@ sensor_kit_base_link: roll: 0.0 pitch: -0.03 yaw: -0.005 + type: monocular_camera camera5/camera_link: x: -0.07356 y: 0.0 @@ -41,6 +46,7 @@ sensor_kit_base_link: roll: 0.0 pitch: -0.01 yaw: 3.125 + type: monocular_camera camera6/camera_link: x: 0.05 y: 0.0175 @@ -48,6 +54,7 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.0 yaw: 0.0 + type: monocular_camera camera7/camera_link: x: 0.05 y: -0.0175 @@ -55,6 +62,7 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.0 yaw: 0.0 + type: monocular_camera velodyne_top_base_link: x: 0.0 y: 0.0 @@ -62,6 +70,7 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.0 yaw: 1.575 + type: velodyne_128 velodyne_left_base_link: x: 0.0 y: 0.56362 @@ -69,6 +78,7 @@ sensor_kit_base_link: roll: -0.02 pitch: 0.71 yaw: 1.575 + type: velodyne_16 velodyne_right_base_link: x: 0.0 y: -0.56362 @@ -76,6 +86,7 @@ sensor_kit_base_link: roll: -0.01 pitch: 0.71 yaw: -1.580 + type: velodyne_16 gnss_link: x: -0.1 y: 0.0 @@ -83,6 +94,8 @@ sensor_kit_base_link: roll: 0.0 pitch: 0.0 yaw: 0.0 + type: gnss + frame_id: gnss tamagawa/imu_link: x: 0.0 y: 0.0 @@ -90,3 +103,5 @@ sensor_kit_base_link: roll: 3.14159265359 pitch: 0.0 yaw: 3.14159265359 + type: imu + frame_id: tamagawa/imu diff --git a/aip_xx1_description/config/sensors_calibration.yaml b/aip_xx1_description/config/sensors_calibration.yaml index e8c4b75e..bf8dce3a 100644 --- a/aip_xx1_description/config/sensors_calibration.yaml +++ b/aip_xx1_description/config/sensors_calibration.yaml @@ -6,6 +6,7 @@ base_link: roll: 0.0 pitch: 0.0 yaw: 0.0 + type: radar sensor_kit_base_link: x: 0.9 y: 0.0 @@ -13,6 +14,7 @@ base_link: roll: -0.001 pitch: 0.015 yaw: -0.0364 + type: units livox_front_right_base_link: x: 3.290 y: -0.65485 @@ -20,6 +22,7 @@ base_link: roll: 0.0 pitch: 0.0 yaw: -0.872664444 + type: livox_horizon livox_front_left_base_link: x: 3.290 y: 0.65485 @@ -27,6 +30,7 @@ base_link: roll: -0.021 pitch: 0.05 yaw: 0.872664444 + type: livox_horizon velodyne_rear_base_link: x: -0.358 y: 0.0 @@ -34,3 +38,4 @@ base_link: roll: -0.02 pitch: 0.7281317 yaw: 3.141592 + type: velodyne_16 diff --git a/aip_xx1_description/package.xml b/aip_xx1_description/package.xml index b35b48e0..73954095 100644 --- a/aip_xx1_description/package.xml +++ b/aip_xx1_description/package.xml @@ -7,8 +7,10 @@ Yukihiro Saito Apache 2 + ament_cmake ament_cmake_auto + aip_urdf_compiler velodyne_description diff --git a/aip_xx1_description/urdf/.gitignore b/aip_xx1_description/urdf/.gitignore new file mode 100644 index 00000000..e1e98315 --- /dev/null +++ b/aip_xx1_description/urdf/.gitignore @@ -0,0 +1 @@ +*.xacro diff --git a/aip_xx1_description/urdf/sensor_kit.xacro b/aip_xx1_description/urdf/sensor_kit.xacro deleted file mode 100644 index ef36763f..00000000 --- a/aip_xx1_description/urdf/sensor_kit.xacro +++ /dev/null @@ -1,228 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_xx1_description/urdf/sensors.xacro b/aip_xx1_description/urdf/sensors.xacro deleted file mode 100644 index 0484bdc3..00000000 --- a/aip_xx1_description/urdf/sensors.xacro +++ /dev/null @@ -1,72 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_xx1_gen2_description/CMakeLists.txt b/aip_xx1_gen2_description/CMakeLists.txt index 549de0f8..782df039 100644 --- a/aip_xx1_gen2_description/CMakeLists.txt +++ b/aip_xx1_gen2_description/CMakeLists.txt @@ -2,9 +2,12 @@ cmake_minimum_required(VERSION 3.5) project(aip_xx1_gen2_description) find_package(ament_cmake_auto REQUIRED) +find_package(aip_urdf_compiler REQUIRED) ament_auto_find_build_dependencies() +aip_cmake_urdf_compile() + ament_auto_package(INSTALL_TO_SHARE urdf config diff --git a/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml b/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml index 88288533..218231a4 100644 --- a/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml +++ b/aip_xx1_gen2_description/config/sensor_kit_calibration.yaml @@ -6,6 +6,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value + type: monocular_camera camera1/camera_link: x: 0.372 # Design Value y: 0.045 # Design Value @@ -13,6 +14,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value + type: monocular_camera camera2/camera_link: x: 0.372 # Design Value y: -0.045 # Design Value @@ -20,6 +22,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value + type: monocular_camera camera3/camera_link: x: 0.133 # Design Value y: 0.498 # Design Value @@ -27,6 +30,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.872665 # Design Value + type: monocular_camera camera4/camera_link: x: 0.133 # Design Value y: -0.498 # Design Value @@ -34,6 +38,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: -0.872665 # Design Value + type: monocular_camera camera5/camera_link: x: 0.095 # Design Value y: 0.524 # Design Value @@ -41,6 +46,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 1.0472 # Design Value + type: monocular_camera camera6/camera_link: x: 0.095 # Design Value y: -0.524 # Design Value @@ -48,6 +54,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: -1.0472 # Design Value + type: monocular_camera camera7/camera_link: x: -0.345 # Design Value y: 0.244 # Design Value @@ -55,6 +62,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 2.70526 # Design Value + type: monocular_camera camera8/camera_link: x: -0.345 # Design Value y: -0.244 # Design Value @@ -62,6 +70,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: -2.70526 # Design Value + type: monocular_camera camera9/camera_link: x: -0.362 # Design Value y: 0.202 # Design Value @@ -69,6 +78,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 2.79253 # Design Value + type: monocular_camera camera10/camera_link: x: -0.362 # Design Value y: -0.202 # Design Value @@ -76,6 +86,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: -2.79253 # Design Value + type: monocular_camera hesai_top_base_link: x: 0.0 # Design Value y: 0.0 # Design Value @@ -83,6 +94,7 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 4.36332298038 # Design Value + type: pandar_ot128 hesai_left_base_link: x: 0.0 # Design Value y: 0.564 # Design Value @@ -90,6 +102,8 @@ sensor_kit_base_link: roll: 0.872665 # Design Value pitch: 0.0 # Design Value yaw: 3.14159265359 # Design Value + type: pandar_xt32 + frame_id: hesai_side_left hesai_right_base_link: x: 0.0 # Design Value y: -0.564 # Design Value @@ -97,6 +111,8 @@ sensor_kit_base_link: roll: 0.69813132679 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value + type: pandar_xt32 + frame_id: hesai_side_right gnss_link: x: -0.279 # Design Value y: 0.0 # Design Value @@ -104,6 +120,8 @@ sensor_kit_base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value + type: gnss + frame_id: gnss tamagawa/imu_link: x: -0.129 # Design Value y: 0.0 # Design Value @@ -111,3 +129,5 @@ sensor_kit_base_link: roll: 3.14159265359 pitch: 0.0 # Design Value yaw: 3.14159265359 # Design Value + type: imu + frame_id: tamagawa/imu diff --git a/aip_xx1_gen2_description/config/sensors_calibration.yaml b/aip_xx1_gen2_description/config/sensors_calibration.yaml index a57d3ea9..4fb70f70 100644 --- a/aip_xx1_gen2_description/config/sensors_calibration.yaml +++ b/aip_xx1_gen2_description/config/sensors_calibration.yaml @@ -6,6 +6,7 @@ base_link: roll: 0.0 pitch: 0.0 yaw: 0.0 + type: units hesai_front_left_base_link: x: 3.373 # Design Value y: 0.740 # Design Value @@ -13,6 +14,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 2.44346132679 # Design Value + type: pandar_xt32 hesai_front_right_base_link: x: 3.373 # Design Value y: -0.740 # Design Value @@ -20,6 +22,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.69813132679 # Design Value + type: pandar_xt32 # velodyne_rear_base_link: #unused # x: -0.358 # y: 0.0 @@ -34,6 +37,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 0.0 # Design Value + type: radar front_right/radar_link: x: 3.384 # Design Value y: -0.7775 # Design Value @@ -41,6 +45,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: -1.22173 # Design Value + type: radar front_left/radar_link: x: 3.384 # Design Value y: 0.7775 # Design Value @@ -48,6 +53,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 1.22173 # Design Value + type: radar rear_center/radar_link: x: -0.858 # Design Value y: 0.0 # Design Value @@ -55,6 +61,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 3.141592 # Design Value + type: radar rear_right/radar_link: x: -0.782 # Design Value y: -0.761 # Design Value @@ -62,6 +69,7 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: -2.0944 # Design Value + type: radar rear_left/radar_link: x: -0.782 # Design Value y: 0.761 # Design Value @@ -69,3 +77,4 @@ base_link: roll: 0.0 # Design Value pitch: 0.0 # Design Value yaw: 2.0944 # Design Value + type: radar diff --git a/aip_xx1_gen2_description/package.xml b/aip_xx1_gen2_description/package.xml index 9b010d72..e1f2774f 100644 --- a/aip_xx1_gen2_description/package.xml +++ b/aip_xx1_gen2_description/package.xml @@ -9,6 +9,7 @@ ament_cmake_auto + aip_urdf_compiler velodyne_description diff --git a/aip_xx1_gen2_description/urdf/.gitignore b/aip_xx1_gen2_description/urdf/.gitignore new file mode 100644 index 00000000..e1e98315 --- /dev/null +++ b/aip_xx1_gen2_description/urdf/.gitignore @@ -0,0 +1 @@ +*.xacro diff --git a/aip_xx1_gen2_description/urdf/sensor_kit.xacro b/aip_xx1_gen2_description/urdf/sensor_kit.xacro deleted file mode 100644 index 137b3589..00000000 --- a/aip_xx1_gen2_description/urdf/sensor_kit.xacro +++ /dev/null @@ -1,253 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/aip_xx1_gen2_launch/CMakeLists.txt b/aip_xx1_gen2_launch/CMakeLists.txt index 7f8d9f60..779ac533 100644 --- a/aip_xx1_gen2_launch/CMakeLists.txt +++ b/aip_xx1_gen2_launch/CMakeLists.txt @@ -14,3 +14,8 @@ ament_auto_package(INSTALL_TO_SHARE data config ) + +install(PROGRAMS + scripts/septentrio_heading_converter.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/aip_xx1_gen2_launch/config/lidar_gen2.yaml b/aip_xx1_gen2_launch/config/lidar_gen2.yaml index 8480292f..c081a7fa 100644 --- a/aip_xx1_gen2_launch/config/lidar_gen2.yaml +++ b/aip_xx1_gen2_launch/config/lidar_gen2.yaml @@ -7,23 +7,20 @@ launches: sensor_ip: 192.168.1.201 data_port: 2368 scan_phase: 160.0 - vertical_bins: 128 - sensor_type: hesai_XT32 namespace: front_left parameters: - max_range: 300.0 + max_range: 80.0 sensor_frame: hesai_front_left sensor_ip: 192.168.1.21 data_port: 2369 scan_phase: 50.0 cloud_min_angle: 50 cloud_max_angle: 320 - vertical_bins: 16 - horizontal_ring_id: 0 - sensor_type: hesai_XT32 namespace: front_right parameters: - max_range: 300.0 + max_range: 80.0 sensor_frame: hesai_front_right sensor_ip: 192.168.1.22 data_port: 2370 diff --git a/aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml b/aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml index 05cac6ee..6f0ce8d0 100644 --- a/aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml +++ b/aip_xx1_gen2_launch/config/mosaic_x5_rover.param.yaml @@ -27,7 +27,7 @@ datum: Default att_offset: - heading: 0.0 + heading: -90.0 pitch: 0.0 ant_type: Unknown @@ -83,7 +83,7 @@ poscovcartesian: false poscovgeodetic: true velcovgeodetic: false - atteuler: false + atteuler: true attcoveuler: false pose: false twist: false diff --git a/aip_xx1_gen2_launch/launch/gnss.launch.xml b/aip_xx1_gen2_launch/launch/gnss.launch.xml index 775bf214..c2ed6dc1 100644 --- a/aip_xx1_gen2_launch/launch/gnss.launch.xml +++ b/aip_xx1_gen2_launch/launch/gnss.launch.xml @@ -1,45 +1,31 @@ - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + - - + + - - diff --git a/aip_xx1_gen2_launch/launch/lidar.launch.py b/aip_xx1_gen2_launch/launch/lidar.launch.py index 81a791b7..a2a6d6e1 100644 --- a/aip_xx1_gen2_launch/launch/lidar.launch.py +++ b/aip_xx1_gen2_launch/launch/lidar.launch.py @@ -105,6 +105,7 @@ def load_yaml(yaml_file_path): base_parameters["enable_blockage_diag"] = LaunchConfiguration("enable_blockage_diag").perform( context ) + base_parameters["return_mode"] = LaunchConfiguration("return_mode").perform(context) sub_launch_actions = [] for launch in config["launches"]: @@ -181,6 +182,7 @@ def add_launch_arg(name: str, default_value=None, **kwargs): add_launch_arg("use_pointcloud_container", "false", description="launch pointcloud container") add_launch_arg("pointcloud_container_name", "pointcloud_container") add_launch_arg("enable_blockage_diag", "false") + add_launch_arg("return_mode", "Dual") # Create launch description with the config_file argument ld = LaunchDescription(launch_arguments) diff --git a/aip_xx1_gen2_launch/launch/radar.launch.xml b/aip_xx1_gen2_launch/launch/radar.launch.xml index c917c9a6..9ba41052 100644 --- a/aip_xx1_gen2_launch/launch/radar.launch.xml +++ b/aip_xx1_gen2_launch/launch/radar.launch.xml @@ -25,7 +25,8 @@ - + + @@ -36,9 +37,10 @@ + - + @@ -54,23 +56,27 @@ - - - - - - - - - - + + + + + + + + + + + + + + - + @@ -86,23 +92,27 @@ - - - - - - - - - - + + + + + + + + + + + + + + - + @@ -118,23 +128,27 @@ - - - - - - - - - - + + + + + + + + + + + + + + - + @@ -150,23 +164,27 @@ - - - - - - - - - - + + + + + + + + + + + + + + - + @@ -182,23 +200,27 @@ - - - - - - - - - - + + + + + + + + + + + + + + - + @@ -214,15 +236,18 @@ - - - - - - - - - + + + + + + + + + + + + diff --git a/aip_xx1_gen2_launch/launch/sensing.launch.xml b/aip_xx1_gen2_launch/launch/sensing.launch.xml index 1f389007..e23e668b 100644 --- a/aip_xx1_gen2_launch/launch/sensing.launch.xml +++ b/aip_xx1_gen2_launch/launch/sensing.launch.xml @@ -29,7 +29,6 @@ - diff --git a/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py b/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py new file mode 100755 index 00000000..aae22217 --- /dev/null +++ b/aip_xx1_gen2_launch/scripts/septentrio_heading_converter.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python3 +# Copyright 2024 TIER IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# cspell: ignore atteuler + +from autoware_sensing_msgs.msg import GnssInsOrientationStamped +from geometry_msgs.msg import Quaternion +import numpy as np +import rclpy +from rclpy.node import Node +from septentrio_gnss_driver.msg import AttEuler + + +class OrientationConverter(Node): + def __init__(self): + super().__init__("septentrio_orientation_converter") + self.publisher = self.create_publisher( + GnssInsOrientationStamped, "/sensing/gnss/septentrio/orientation", 10 + ) + self.subscription = self.create_subscription( + AttEuler, "/sensing/gnss/septentrio/atteuler", self.attitude_callback, 10 + ) + self.subscription # prevent unused variable warning + + def heading_to_quaternion(self, heading: float) -> Quaternion: + # The input heading is in a North-East coordinate system and measured in degrees. + # Heading values range from [0, 360). + # Examples: + # - Heading is 0[deg] when the vehicle faces North. + # - Heading is 90[deg] when the vehicle faces East. + # - Heading is 180[deg] when the vehicle faces South. + # - Heading is 270[deg] when the vehicle faces West. + + # The output quaternion represents orientation in an East-North-Up (ENU) coordinate system. + # The quaternion is of the form [x, y, z, w], where: + # - Facing East: [x, y, z, w] = [0, 0, 0, 1] = [0, 0, sin( 0[deg]), cos( 0[deg])] + # - Facing North: [x, y, z, w] = [0, 0, 0.7, 0.7] = [0, 0, sin( 45[deg]), cos( 45[deg])] + # - Facing West: [x, y, z, w] = [0, 0, 1, 0] = [0, 0, sin( 90[deg]), cos( 90[deg])] + # - Facing South: [x, y, z, w] = [0, 0, -0.7, 0.7] = [0, 0, sin(135[deg]), cos(135[deg])] + q = Quaternion() + q.x = 0.0 + q.y = 0.0 + q.z = np.sin(np.deg2rad(90 - heading) / 2.0) + q.w = np.cos(np.deg2rad(90 - heading) / 2.0) + return q + + def attitude_callback(self, attitude_msg: AttEuler) -> None: + # When septentrio driver cannot measure the heading, it will publish -20000000000.0 + if attitude_msg.heading < 0: + return + + orientation_msg = GnssInsOrientationStamped() + orientation_msg.header = attitude_msg.header + + # Even if using dual antenna, the roll is not estimated by the septentrio driver. + # Therefore, this assume the roll & pitch are 0 and convert the heading to quaternion. + orientation_msg.orientation.orientation = self.heading_to_quaternion(attitude_msg.heading) + + # Septentrio driver does not provide the covariance of the heading. + # Therefore, this assumes the covariance of the heading is 1.0. + orientation_msg.orientation.rmse_rotation_x = 1.0 + orientation_msg.orientation.rmse_rotation_y = 1.0 + orientation_msg.orientation.rmse_rotation_z = 1.0 + + self.publisher.publish(orientation_msg) + + +def main(args=None) -> None: + rclpy.init(args=args) + + converter = OrientationConverter() + + rclpy.spin(converter) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + converter.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main()