Ros2 v0.8.0 localization launch (autowarefoundation#54)

* restore file name for v0.8.0 update in localization launch

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add random sample (autowarefoundation#84)

Signed-off-by: Yamato Ando <yamato.ando@gmail.com>

* Add ndt_scan_matcher.yaml (autowarefoundation#162)

Signed-off-by: Yuma Nihei <yuma.nihei@tier4.jp>

* ]Revert "restore file name for v0.8.0 update in localization launch"

This reverts commit f589733f7dad05989bde323baeb5c43a62cd26e1.

* fix param type

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix exec name

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Co-authored-by: YamatoAndo <yamato.ando@gmail.com>
Co-authored-by: Yuma Nihei <yuma.nihei@tier4.jp>

launch/localization_launch/CMakeLists.txt

@@ -18,4 +18,5 @@ endif()
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )

launch/localization_launch/config/ndt_scan_matcher.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 whetherto 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/launch/pose_estimator/pose_estimator.launch.xml

@@ -10,6 +10,7 @@
     <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.yaml" />
   </include>
 
 </launch>

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

@@ -4,6 +4,7 @@
   <!-- Topics -->
   <arg name="input_sensor_points_topic" default="/sensing/lidar/pointcloud" doc="Sensor points topic"/>
   <arg name="output_mesurement_range_sensor_points_topic" default="mesurement_range/pointcloud" doc="Sensor points topic"/>
+  <arg name="output_voxel_grid_downsample_sensor_points_topic" default="voxel_grid_downsample/pointcloud" doc="Sensor points topic"/>
   <arg name="output_downsample_sensor_points_topic" default="downsample/pointcloud" doc="Sensor points topic"/>
 
   <!-- frame_ids -->
@@ -30,12 +31,18 @@
 
 
   <!-- Run a VoxelGrid filter to clean NaNs and downsample the data -->
-  <node pkg="pointcloud_preprocessor" exec="downsample_filter_node" name="voxel_grid_filter" output="log">
+  <node pkg="pointcloud_preprocessor" exec="voxel_grid_downsample_filter_node" name="voxel_grid_filter" output="log">
     <remap from="input" to="$(var output_mesurement_range_sensor_points_topic)" />
-    <remap from="output" to="$(var output_downsample_sensor_points_topic)" />
+    <remap from="output" to="$(var output_voxel_grid_downsample_sensor_points_topic)" />
     <param name="voxel_size_x" value="3.0" />
     <param name="voxel_size_y" value="3.0" />
     <param name="voxel_size_z" value="3.0" />
   </node>
 
+  <node pkg="pointcloud_preprocessor" exec="random_downsample_filter_node" name="random_filter" output="log">
+    <remap from="input" to="$(var output_voxel_grid_downsample_sensor_points_topic)" />
+    <remap from="output" to="$(var output_downsample_sensor_points_topic)" />
+    <param name="sample_num" value="1500" />
+  </node>
+
 </launch>