Merge remote-tracking branch 'origin' into feat/z_fitting_by_vectormap

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Author: SakodaShintaro

launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml

@@ -108,7 +108,7 @@
     <!-- pointcloud_downsampling -->
     <let name="override_input_pointcloud" value="$(var input_pointcloud)"/>
     <let name="override_input_pointcloud" value="$(var input_pointcloud)/relay" if="$(var multi_localizer_mode)"/>
-    <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py" if="$(var use_ndt_pose)">
+    <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.xml" if="$(var use_ndt_pose)">
       <arg name="lidar_container_name" value="$(var lidar_container_name)"/>
       <arg name="input_pointcloud" value="$(var override_input_pointcloud)"/>
     </include>

launch/tier4_localization_launch/launch/util/util.launch.py

@@ -0,0 +1,33 @@
+<launch>
+  <!-- topic -->
+  <arg name="output/pointcloud" default="downsample/pointcloud" description="final output topic name"/>
+
+  <!-- container -->
+  <arg name="lidar_container_name" default="/sensing/lidar/top/pointcloud_preprocessor/pointcloud_container" description="container name of main lidar used for localization"/>
+
+  <!-- whether use intra-process -->
+  <arg name="use_intra_process" default="true" description="use ROS 2 component container communication"/>
+
+  <load_composable_node target="$(var lidar_container_name)">
+    <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::CropBoxFilterComponent" name="crop_box_filter_measurement_range">
+      <param from="$(var crop_box_filter_measurement_range_param_path)"/>
+      <remap from="input" to="$(var input_pointcloud)"/>
+      <remap from="output" to="measurement_range/pointcloud"/>
+      <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
+    </composable_node>
+
+    <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent" name="voxel_grid_downsample_filter">
+      <param from="$(var voxel_grid_downsample_filter_param_path)"/>
+      <remap from="input" to="measurement_range/pointcloud"/>
+      <remap from="output" to="voxel_grid_downsample/pointcloud"/>
+      <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
+    </composable_node>
+
+    <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::RandomDownsampleFilterComponent" name="random_downsample_filter">
+      <param from="$(var random_downsample_filter_param_path)"/>
+      <remap from="input" to="voxel_grid_downsample/pointcloud"/>
+      <remap from="output" to="$(var output/pointcloud)"/>
+      <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
+    </composable_node>
+  </load_composable_node>
+</launch>

launch/tier4_localization_launch/launch/util/util.launch.xml

@@ -15,4 +15,7 @@ ament_auto_add_executable(node
   src/node.cpp
 )
 
-ament_auto_package(INSTALL_TO_SHARE launch)
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config)

map/map_height_fitter/CMakeLists.txt

@@ -4,8 +4,18 @@ This library fits the given point with the ground of the point cloud map.
 The map loading operation is switched by the parameter `enable_partial_load` of the node specified by `map_loader_name`.
 The node using this library must use multi thread executor.
 
-| Interface    | Local Name         | Description                              |
-| ------------ | ------------------ | ---------------------------------------- |
-| Parameter    | map_loader_name    | The point cloud map loader name.         |
-| Subscription | ~/pointcloud_map   | The topic name to load the whole map     |
-| Client       | ~/partial_map_load | The service name to load the partial map |
+## Parameters
+
+{{ json_to_markdown("map/map_height_fitter/schema/map_height_fitter.schema.json") }}
+
+## Topic subscription
+
+| Topic Name       | Description                                                                                  |
+| ---------------- | -------------------------------------------------------------------------------------------- |
+| ~/pointcloud_map | The topic containing the whole pointcloud map (only used when `enable_partial_load = false`) |
+
+## Service client
+
+| Service Name       | Description                         |
+| ------------------ | ----------------------------------- |
+| ~/partial_map_load | The service to load the partial map |

map/map_height_fitter/README.md

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    map_loader_name: "/map/pointcloud_map_loader"
+    fit_target: "pointcloud_map"

map/map_height_fitter/config/map_height_fitter.param.yaml

@@ -1,9 +1,10 @@
 <launch>
+  <arg name="param_path" default="$(find-pkg-share map_height_fitter)/config/map_height_fitter.param.yaml"/>
+
   <group>
     <push-ros-namespace namespace="map"/>
     <node pkg="map_height_fitter" exec="node" name="map_height_fitter">
-      <param name="map_loader_name" value="/map/pointcloud_map_loader"/>
-      <param name="fit_target" value="pointcloud_map"/>
+      <param from="$(var param_path)"/>
       <remap from="~/pointcloud_map" to="/map/pointcloud_map"/>
       <remap from="~/partial_map_load" to="/map/get_partial_pointcloud_map"/>
       <remap from="~/vector_map" to="/map/vector_map"/>

map/map_height_fitter/launch/map_height_fitter.launch.xml

@@ -0,0 +1,33 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for map_height_fitter",
+  "type": "object",
+  "definitions": {
+    "map_height_fitter": {
+      "type": "object",
+      "properties": {
+        "map_loader_name": {
+          "type": "string",
+          "description": "Node name of the map loader from which this map_height_fitter will retrieve its parameters",
+          "default": "/map/pointcloud_map_loader"
+        }
+      },
+      "required": ["map_loader_name"],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/map_height_fitter"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

map/map_height_fitter/schema/map_height_fitter.schema.json

@@ -31,10 +31,10 @@
 
     # debug
     debug_mode: false
-    filter_scope_minx: -100
-    filter_scope_maxx: 100
-    filter_scope_miny: -100
-    filter_scope_maxy: 100
-    filter_scope_minz: -100
-    filter_scope_maxz: 100
+    filter_scope_min_x: -100
+    filter_scope_max_x: 100
+    filter_scope_min_y: -100
+    filter_scope_max_y: 100
+    filter_scope_min_z: -100
+    filter_scope_max_z: 100
     image_buffer_size: 15

perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml

@@ -44,8 +44,6 @@
 #include <utility>
 #include <vector>
 
-// cspell: ignore minx, maxx, miny, maxy, minz, maxz
-
 namespace image_projection_based_fusion
 {
 using autoware_auto_perception_msgs::msg::DetectedObject;
@@ -131,13 +129,12 @@ class FusionNode : public rclcpp::Node
   // debugger
   std::shared_ptr<Debugger> debugger_;
   virtual bool out_of_scope(const ObjType & obj) = 0;
-  // cspell: ignore minx, maxx, miny, maxy, minz, maxz
-  float filter_scope_minx_;
-  float filter_scope_maxx_;
-  float filter_scope_miny_;
-  float filter_scope_maxy_;
-  float filter_scope_minz_;
-  float filter_scope_maxz_;
+  float filter_scope_min_x_;
+  float filter_scope_max_x_;
+  float filter_scope_min_y_;
+  float filter_scope_max_y_;
+  float filter_scope_min_z_;
+  float filter_scope_max_z_;
 
   /** \brief processing time publisher. **/
   std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;

perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp

@@ -22,7 +22,6 @@
   <arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
 
   <!-- debug -->
-  <!-- cspell: ignore minx, maxx, miny, maxy, minz, maxz  -->
   <arg name="input/image0" default="/image_raw0"/>
   <arg name="input/image1" default="/image_raw1"/>
   <arg name="input/image2" default="/image_raw2"/>

perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml

@@ -137,14 +137,12 @@ FusionNode<Msg, ObjType, Msg2D>::FusionNode(
     stop_watch_ptr_->tic("processing_time");
   }
 
-  // cspell: ignore minx, maxx, miny, maxy, minz, maxz
-  // FIXME: use min_x instead of minx
-  filter_scope_minx_ = declare_parameter<double>("filter_scope_min_x");
-  filter_scope_maxx_ = declare_parameter<double>("filter_scope_max_x");
-  filter_scope_miny_ = declare_parameter<double>("filter_scope_min_y");
-  filter_scope_maxy_ = declare_parameter<double>("filter_scope_max_y");
-  filter_scope_minz_ = declare_parameter<double>("filter_scope_min_z");
-  filter_scope_maxz_ = declare_parameter<double>("filter_scope_max_z");
+  filter_scope_min_x_ = declare_parameter<double>("filter_scope_min_x");
+  filter_scope_max_x_ = declare_parameter<double>("filter_scope_max_x");
+  filter_scope_min_y_ = declare_parameter<double>("filter_scope_min_y");
+  filter_scope_max_y_ = declare_parameter<double>("filter_scope_max_y");
+  filter_scope_min_z_ = declare_parameter<double>("filter_scope_min_z");
+  filter_scope_max_z_ = declare_parameter<double>("filter_scope_max_z");
 }
 
 template <class Msg, class Obj, class Msg2D>

perception/image_projection_based_fusion/src/fusion_node.cpp

@@ -28,8 +28,6 @@
 #include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
 #endif
 
-// cspell: ignore minx, maxx, miny, maxy, minz, maxz
-
 namespace image_projection_based_fusion
 {
 
@@ -253,17 +251,17 @@ bool RoiClusterFusionNode::out_of_scope(const DetectedObjectWithFeature & obj)
   for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(cluster, "x"), iter_y(cluster, "y"),
        iter_z(cluster, "z");
        iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
-    if (!valid_point(*iter_x, filter_scope_minx_, filter_scope_maxx_)) {
+    if (!valid_point(*iter_x, filter_scope_min_x_, filter_scope_max_x_)) {
       is_out = true;
       break;
     }
 
-    if (!valid_point(*iter_y, filter_scope_miny_, filter_scope_maxy_)) {
+    if (!valid_point(*iter_y, filter_scope_min_y_, filter_scope_max_y_)) {
       is_out = true;
       break;
     }
 
-    if (!valid_point(*iter_z, filter_scope_minz_, filter_scope_maxz_)) {
+    if (!valid_point(*iter_z, filter_scope_min_z_, filter_scope_max_z_)) {
       is_out = true;
       break;
     }

perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp

@@ -19,8 +19,6 @@
 #include <image_projection_based_fusion/utils/geometry.hpp>
 #include <image_projection_based_fusion/utils/utils.hpp>
 
-// cspell: ignore minx, maxx, miny, maxy, minz, maxz
-
 namespace image_projection_based_fusion
 {
 
@@ -268,13 +266,13 @@ bool RoiDetectedObjectFusionNode::out_of_scope(const DetectedObject & obj)
     return (p > min_num) && (p < max_num);
   };
 
-  if (!valid_point(pose.position.x, filter_scope_minx_, filter_scope_maxx_)) {
+  if (!valid_point(pose.position.x, filter_scope_min_x_, filter_scope_max_x_)) {
     return is_out;
   }
-  if (!valid_point(pose.position.y, filter_scope_miny_, filter_scope_maxy_)) {
+  if (!valid_point(pose.position.y, filter_scope_min_y_, filter_scope_max_y_)) {
     return is_out;
   }
-  if (!valid_point(pose.position.z, filter_scope_minz_, filter_scope_maxz_)) {
+  if (!valid_point(pose.position.z, filter_scope_min_z_, filter_scope_max_z_)) {
     return is_out;
   }