feat: add localization_launch package

launch/localization_launch/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.5)
+project(localization_launch)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter)
+endif()
+
+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
+  config
+)

launch/localization_launch/README.md

@@ -0,0 +1,26 @@
+# localization_launch
+
+## Structure
+
+![localization_launch](./localization_launch.drawio.svg)
+
+## Package Dependencies
+
+Please see `<exec_depend>` in `package.xml`.
+
+## Usage
+
+You can include as follows in `*.launch.xml` to use `localization.launch.xml`.
+
+```xml
+  <include file="$(find-pkg-share localization_launch)/launch/localization.launch.xml">
+  </include>
+```
+
+## Notes
+
+There are some `param.yaml` files in `config` directory.
+
+```bash
+ndt_scan_matcher.param.yaml
+```

launch/localization_launch/config/crop_box_filter_measurement_range.param.yaml

@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    input_frame: "base_link"
+    output_frame: "base_link"
+    min_x: -60.0
+    max_x: 60.0
+    min_y: -60.0
+    max_y: 60.0
+    min_z: -30.0
+    max_z: 50.0
+    negative: False

launch/localization_launch/config/ndt_scan_matcher.param.yaml

@@ -0,0 +1,34 @@
+/**:
+  ros__parameters:
+    # Vehicle reference frame
+    base_frame: "base_link"
+
+    # Subscriber queue size
+    input_sensor_points_queue_size: 1
+
+    # NDT implementation type
+    # 0=PCL_GENERIC, 1=PCL_MODIFIED, 2=OMP
+    ndt_implement_type: 2
+
+    # The maximum difference between two consecutive
+    # transformations in order to consider convergence
+    trans_epsilon: 0.01
+
+    # The newton line search maximum step length
+    step_size: 0.1
+
+    # The ND voxel grid resolution
+    resolution: 2.0
+
+    # The number of iterations required to calculate alignment
+    max_iterations: 30
+
+    # Threshold for deciding whether to trust the estimation result
+    converged_param_transform_probability: 3.0
+
+    # neighborhood search method in OMP
+    # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1
+    omp_neighborhood_search_method: 0
+
+    # Number of threads used for parallel computing
+    omp_num_threads: 4

launch/localization_launch/config/random_downsample_filter.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    sample_num: 1500

launch/localization_launch/config/voxel_grid_filter.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    voxel_size_x: 3.0
+    voxel_size_y: 3.0
+    voxel_size_z: 3.0

launch/localization_launch/launch/localization.launch.xml

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- localization module -->
+  <group>
+    <push-ros-namespace namespace="localization"/>
+    <!-- util module -->
+    <group>
+      <push-ros-namespace namespace="util"/>
+      <include file="$(find-pkg-share localization_launch)/launch/util/util.launch.xml">
+      </include>
+    </group>
+    <!-- pose_estimator module -->
+    <group>
+      <push-ros-namespace namespace="pose_estimator"/>
+      <include file="$(find-pkg-share localization_launch)/launch/pose_estimator/pose_estimator.launch.xml">
+      </include>
+    </group>
+    <!-- twist_estimator module -->
+    <group>
+      <push-ros-namespace namespace="twist_estimator"/>
+      <include file="$(find-pkg-share localization_launch)/launch/twist_estimator/twist_estimator.launch.xml">
+      </include>
+    </group>
+    <!-- pose_twist_fusion_filter module -->
+    <group>
+      <push-ros-namespace namespace="pose_twist_fusion_filter"/>
+      <include file="$(find-pkg-share localization_launch)/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml">
+      </include>
+    </group>
+    <!-- localization_error_monitor_module -->
+    <include file="$(find-pkg-share localization_launch)/launch/localization_error_monitor/localization_error_monitor.launch.xml">
+    </include>
+  </group>
+</launch>

launch/localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+    <include file="$(find-pkg-share localization_error_monitor)/launch/localization_error_monitor.launch.xml">
+      <arg name="input/pose_with_cov" value="/localization/pose_with_covariance" />
+      <arg name="scale" value="3.0" />
+      <arg name="error_ellipse_size" value="1.0" />
+      <arg name="warn_ellipse_size" value="0.8" />
+      <arg name="error_ellipse_size_lateral_direction" value="1.0" />
+      <arg name="warn_ellipse_size_lateral_direction" value="0.8" />
+    </include>
+</launch>

launch/localization_launch/launch/pose_estimator/pose_estimator.launch.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<launch>
+
+  <include file="$(find-pkg-share ndt_scan_matcher)/launch/ndt_scan_matcher.launch.xml">
+    <arg name="input_map_points_topic" value="/map/pointcloud_map"/>
+    <arg name="input_sensor_points_topic" value="/localization/util/downsample/pointcloud"/>
+    <arg name="input_initial_pose_topic" value="/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias"/>
+
+
+    <arg name="output_pose_topic" value="/localization/pose_estimator/pose"/>
+    <arg name="output_pose_with_covariance_topic" value="/localization/pose_estimator/pose_with_covariance"/>
+    <arg name="output_diagnostics_topic" value="/localization/diagnostics"/>
+
+    <arg name="param_file" value="$(find-pkg-share localization_launch)/config/ndt_scan_matcher.param.yaml" />
+  </include>
+
+</launch>

launch/localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<launch>
+  <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
+    <arg name="enable_yaw_bias_estimation" value="False"/>
+    <arg name="tf_rate" value="50.0"/>
+    <arg name="twist_rate" value="25.0"/>
+    <arg name="input_initial_pose_name" value="/initialpose3d"/>
+
+    <arg name="input_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
+    <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
+    <arg name="output_odom_name" value="kinematic_state"/>
+    <arg name="output_pose_name" value="pose"/>
+    <arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>
+    <arg name="output_pose_without_yawbias_name" value="pose_without_yawbias"/>
+    <arg name="output_pose_with_covariance_without_yawbias_name" value="pose_with_covariance_without_yawbias"/>
+    <arg name="output_twist_name" value="twist"/>
+    <arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
+
+    <arg name="twist_stddev_wz" value="0.003 "/>
+
+    <arg name="proc_stddev_vx_c" value="10.0"/>
+    <arg name="proc_stddev_wz_c" value="5.0"/>
+  </include>
+  <include file="$(find-pkg-share stop_filter)/launch/stop_filter.launch.xml">
+    <arg name="use_twist_with_covariance" value="True"/>
+    <arg name="input_odom_name" value="/localization/pose_twist_fusion_filter/kinematic_state"/>
+    <arg name="input_twist_with_covariance_name" value="/localization/pose_twist_fusion_filter/twist_with_covariance"/>
+    <arg name="output_odom_name" value="/localization/kinematic_state"/>
+  </include>
+</launch>

launch/localization_launch/launch/twist_estimator/twist_estimator.launch.xml

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+  <include file="$(find-pkg-share vehicle_velocity_converter)/launch/vehicle_velocity_converter.launch.xml">
+    <arg name="input_vehicle_velocity_topic" value="/vehicle/status/velocity_status"/>
+    <arg name="output_twist_with_covariance" value="/localization/twist_estimator/vehicle_velocity_converter/twist_with_covariance"/>
+  </include>
+  <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">
+    <arg name="input_vehicle_twist_with_covariance_topic" value="/localization/twist_estimator/vehicle_velocity_converter/twist_with_covariance"/>
+    <arg name="output_twist_with_covariance_topic" value="/localization/twist_estimator/twist_with_covariance" />
+  </include>
+</launch>

launch/localization_launch/launch/util/util.launch.py

@@ -0,0 +1,135 @@
+# 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.conditions import LaunchConfigurationNotEquals
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.descriptions import ComposableNode
+from launch_ros.substitutions import FindPackageShare
+import yaml
+
+
+def launch_setup(context, *args, **kwargs):
+    # https://github.com/ros2/launch_ros/issues/156
+    def load_composable_node_param(param_path):
+        with open(LaunchConfiguration(param_path).perform(context), "r") as f:
+            return yaml.safe_load(f)["/**"]["ros__parameters"]
+
+    crop_box_component = ComposableNode(
+        package="pointcloud_preprocessor",
+        plugin="pointcloud_preprocessor::CropBoxFilterComponent",
+        name="crop_box_filter_measurement_range",
+        remappings=[
+            ("input", LaunchConfiguration("input_sensor_points_topic")),
+            ("output", LaunchConfiguration("output_measurement_range_sensor_points_topic")),
+        ],
+        parameters=[
+            load_composable_node_param("crop_box_filter_measurement_range_param_path"),
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+    voxel_grid_downsample_component = ComposableNode(
+        package="pointcloud_preprocessor",
+        plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
+        name="voxel_grid_downsample_filter",
+        remappings=[
+            ("input", LaunchConfiguration("output_measurement_range_sensor_points_topic")),
+            ("output", LaunchConfiguration("output_voxel_grid_downsample_sensor_points_topic")),
+        ],
+        parameters=[load_composable_node_param("voxel_grid_downsample_filter_param_path")],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+    random_downsample_component = ComposableNode(
+        package="pointcloud_preprocessor",
+        plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
+        name="random_downsample_filter",
+        remappings=[
+            ("input", LaunchConfiguration("output_voxel_grid_downsample_sensor_points_topic")),
+            ("output", LaunchConfiguration("output_downsample_sensor_points_topic")),
+        ],
+        parameters=[load_composable_node_param("random_downsample_filter_param_path")],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
+    composable_nodes = [
+        crop_box_component,
+        voxel_grid_downsample_component,
+        random_downsample_component,
+    ]
+
+    load_composable_nodes = LoadComposableNodes(
+        condition=LaunchConfigurationNotEquals("container", ""),
+        composable_node_descriptions=composable_nodes,
+        target_container=LaunchConfiguration("container"),
+    )
+
+    return [load_composable_nodes]
+
+
+def generate_launch_description():
+    launch_arguments = []
+
+    def add_launch_arg(name: str, default_value=None, description=None):
+        arg = DeclareLaunchArgument(name, default_value=default_value, description=description)
+        launch_arguments.append(arg)
+
+    add_launch_arg(
+        "crop_box_filter_measurement_range_param_path",
+        [
+            FindPackageShare("localization_launch"),
+            "/config/crop_box_filter_measurement_range.param.yaml",
+        ],
+        "path to the parameter file of crop_box_filter_measurement_range",
+    )
+    add_launch_arg(
+        "voxel_grid_downsample_filter_param_path",
+        [FindPackageShare("localization_launch"), "/config/voxel_grid_filter.param.yaml"],
+        "path to the parameter file of voxel_grid_downsample_filter",
+    )
+    add_launch_arg(
+        "random_downsample_filter_param_path",
+        [FindPackageShare("localization_launch"), "/config/random_downsample_filter.param.yaml"],
+        "path to the parameter file of random_downsample_filter",
+    )
+    add_launch_arg("use_intra_process", "true", "use ROS2 component container communication")
+    add_launch_arg(
+        "container",
+        "/sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container",
+        "container name",
+    )
+    add_launch_arg(
+        "input_sensor_points_topic",
+        "/sensing/lidar/top/rectified/pointcloud",
+        "input topic name for raw pointcloud",
+    )
+    add_launch_arg(
+        "output_measurement_range_sensor_points_topic",
+        "measurement_range/pointcloud",
+        "output topic name for crop box filter",
+    )
+    add_launch_arg(
+        "output_voxel_grid_downsample_sensor_points_topic",
+        "voxel_grid_downsample/pointcloud",
+        "output topic name for voxel grid downsample filter",
+    )
+    add_launch_arg(
+        "output_downsample_sensor_points_topic",
+        "downsample/pointcloud",
+        "output topic name for downsample filter. this is final output",
+    )
+
+    return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)])

launch/localization_launch/launch/util/util.launch.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- Topics -->
+  <arg name="input_sensor_points_topic" default="/sensing/lidar/top/rectified/pointcloud" description="input topic name for raw pointcloud"/>
+  <arg name="output_measurement_range_sensor_points_topic" default="measurement_range/pointcloud" description="output topic name for crop box filter"/>
+  <arg name="output_voxel_grid_downsample_sensor_points_topic" default="voxel_grid_downsample/pointcloud" description="output topic name for voxel grid downsample filter"/>
+  <arg name="output_downsample_sensor_points_topic" default="downsample/pointcloud" description="output topic name for downsample filter. this is final output"/>
+
+  <!-- container -->
+  <arg name="container" default="/sensing/lidar/top/pointcloud_preprocessor/velodyne_node_container"  description="container name"/>
+
+  <!-- option -->
+  <arg name="use_intra_process" default="true" description="use ROS2 component container communication"/>
+
+  <!-- pose_initializer -->
+  <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml" />
+
+  <!-- util -->
+  <include file="$(find-pkg-share localization_launch)/launch/util/util.launch.py" />
+
+</launch>