feat : git cherry-pick -m1 673f588 # feat(gen2): change lidar frame_i…

…d in occupancy_grid_map #802 fix : change msg type to autoware*.msg from autoware_auto*.msg Signed-off-by: N-Eiki <[email protected]>
tier4 · Jun 10, 2024 · 39f09fd · 39f09fd
1 parent a732236
commit 39f09fd
Show file tree

Hide file tree

Showing 9 changed files with 22 additions and 102 deletions.
diff --git a/...stic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml b/...stic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml
@@ -12,10 +12,6 @@
         map_length_x: 150.0 # [m]
         map_length_y: 150.0 # [m]
 
-      # downsample input pointcloud
-      downsample_input_pointcloud: true
-      downsample_voxel_size: 0.25 # [m]
-
       # each grid map parameters
       ogm_creation_config:
         height_filter:
@@ -24,7 +20,7 @@
           max_height: 2.0
         enable_single_frame_mode: true
         # use sensor pointcloud to filter obstacle pointcloud
-        filter_obstacle_pointcloud_by_raw_pointcloud: false
+        filter_obstacle_pointcloud_by_raw_pointcloud: true
 
         grid_map_type: "OccupancyGridMapFixedBlindSpot"
         OccupancyGridMapFixedBlindSpot:

diff --git a/...istic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py b/...istic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py
@@ -92,57 +92,6 @@ def get_ogm_creation_config(total_config: dict, list_iter: int) -> dict:
     return ogm_creation_config
 
 
-# generate downsample node
-def get_downsample_filter_node(setting: dict) -> ComposableNode:
-    plugin_str = setting["plugin"]
-    voxel_size = setting["voxel_size"]
-    node_name = setting["node_name"]
-    return ComposableNode(
-        package="pointcloud_preprocessor",
-        plugin=plugin_str,
-        name=node_name,
-        remappings=[
-            ("input", setting["input_topic"]),
-            ("output", setting["output_topic"]),
-        ],
-        parameters=[
-            {
-                "voxel_size_x": voxel_size,
-                "voxel_size_y": voxel_size,
-                "voxel_size_z": voxel_size,
-            }
-        ],
-        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-    )
-
-
-def get_downsample_preprocess_nodes(voxel_size: float, raw_pointcloud_topics: list) -> list:
-    nodes = []
-    for i in range(len(raw_pointcloud_topics)):
-        raw_pointcloud_topic: str = raw_pointcloud_topics[i]
-        frame_name: str = raw_pointcloud_topic.split("/")[
-            -2
-        ]  # `/sensing/lidar/top/pointcloud` -> `top
-        processed_pointcloud_topic: str = frame_name + "/raw/downsample/pointcloud"
-        raw_settings = {
-            "plugin": "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent",
-            "node_name": "raw_pc_downsample_filter_" + frame_name,
-            "input_topic": raw_pointcloud_topic,
-            "output_topic": processed_pointcloud_topic,
-            "voxel_size": voxel_size,
-        }
-        nodes.append(get_downsample_filter_node(raw_settings))
-    obstacle_settings = {
-        "plugin": "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent",
-        "node_name": "obstacle_pc_downsample_filter",
-        "input_topic": LaunchConfiguration("input/obstacle_pointcloud"),
-        "output_topic": "/perception/occupancy_grid_map/obstacle/downsample/pointcloud",
-        "voxel_size": voxel_size,
-    }
-    nodes.append(get_downsample_filter_node(obstacle_settings))
-    return nodes
-
-
 def launch_setup(context, *args, **kwargs):
     """Launch fusion based occupancy grid map creation nodes.
 
@@ -162,59 +111,30 @@ def launch_setup(context, *args, **kwargs):
     gridmap_generation_composable_nodes = []
 
     number_of_nodes = len(fusion_config["raw_pointcloud_topics"])
-    print(
-        "launching multi_lidar_pointcloud_based occupancy grid map",
-        number_of_nodes,
-        "nodes in the container named",
-        LaunchConfiguration("pointcloud_container_name").perform(context),
-    )
 
-    # Down sample settings
-    downsample_input_pointcloud: bool = total_config["downsample_input_pointcloud"]
-    downsample_voxel_size: float = total_config["downsample_voxel_size"]
-
-    # get obstacle pointcloud
-    obstacle_pointcloud_topic: str = (
-        "/perception/occupancy_grid_map/obstacle/downsample/pointcloud"
-        if downsample_input_pointcloud
-        else LaunchConfiguration("input/obstacle_pointcloud").perform(context)
-    )
+    print(number_of_nodes)
 
     for i in range(number_of_nodes):
         # load parameter file
         ogm_creation_config = get_ogm_creation_config(total_config, i)
         ogm_creation_config["updater_type"] = LaunchConfiguration("updater_type").perform(context)
         ogm_creation_config.update(updater_config)
 
-        raw_pointcloud_topic: str = fusion_config["raw_pointcloud_topics"][i]
-        frame_name: str = raw_pointcloud_topic.split("/")[
-            -2
-        ]  # assume `/sensing/lidar/top/pointcloud` -> `top
-        if downsample_input_pointcloud:
-            raw_pointcloud_topic = "raw/downsample/pointcloud"
-
         # generate composable node
         node = ComposableNode(
             package="probabilistic_occupancy_grid_map",
             plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode",
-            name="occupancy_grid_map_node",
-            namespace=frame_name,
+            name="occupancy_grid_map_node_in_" + str(i),
             remappings=[
-                ("~/input/obstacle_pointcloud", obstacle_pointcloud_topic),
-                ("~/input/raw_pointcloud", raw_pointcloud_topic),
+                ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")),
+                ("~/input/raw_pointcloud", fusion_config["raw_pointcloud_topics"][i]),
                 ("~/output/occupancy_grid_map", fusion_config["fusion_input_ogm_topics"][i]),
             ],
             parameters=[ogm_creation_config],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         )
         gridmap_generation_composable_nodes.append(node)
 
-    if downsample_input_pointcloud:
-        downsample_nodes = get_downsample_preprocess_nodes(
-            downsample_voxel_size, fusion_config["raw_pointcloud_topics"]
-        )
-        gridmap_generation_composable_nodes.extend(downsample_nodes)
-
     # 2. launch occupancy grid map fusion node
     gridmap_fusion_node = [
         ComposableNode(
@@ -231,7 +151,7 @@ def launch_setup(context, *args, **kwargs):
 
     # 3. launch setting
     occupancy_grid_map_container = ComposableNodeContainer(
-        name=LaunchConfiguration("pointcloud_container_name"),
+        name=LaunchConfiguration("container_name"),
         namespace="",
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),
@@ -242,7 +162,7 @@ def launch_setup(context, *args, **kwargs):
 
     load_composable_nodes = LoadComposableNodes(
         composable_node_descriptions=gridmap_generation_composable_nodes + gridmap_fusion_node,
-        target_container=LaunchConfiguration("pointcloud_container_name"),
+        target_container=LaunchConfiguration("container_name"),
         condition=IfCondition(LaunchConfiguration("use_pointcloud_container")),
     )
 
@@ -270,7 +190,7 @@ def add_launch_arg(name: str, default_value=None):
             add_launch_arg("use_multithread", "false"),
             add_launch_arg("use_intra_process", "true"),
             add_launch_arg("use_pointcloud_container", "false"),
-            add_launch_arg("pointcloud_container_name", "occupancy_grid_map_container"),
+            add_launch_arg("container_name", "occupancy_grid_map_container"),
             add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"),
             add_launch_arg("output", "occupancy_grid"),
             add_launch_arg(

diff --git a/sensing/pointcloud_preprocessor/docs/vector-map-filter.md b/sensing/pointcloud_preprocessor/docs/vector-map-filter.md
@@ -13,7 +13,7 @@ The `vector_map_filter` is a node that removes points on the outside of lane by
 | Name                 | Type                                         | Description      |
 | -------------------- | -------------------------------------------- | ---------------- |
 | `~/input/points`     | `sensor_msgs::msg::PointCloud2`              | reference points |
-| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map       |
+| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map       |
 
 ### Output
 

diff --git a/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md b/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md
@@ -22,7 +22,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, so please
 | Name                 | Type                                         | Description                          |
 | -------------------- | -------------------------------------------- | ------------------------------------ |
 | `~/input`            | `sensor_msgs::msg::PointCloud2`              | input points                         |
-| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map used for filtering points |
+| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map used for filtering points |
 
 ### Output
 

diff --git a/...ocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/...ocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp
@@ -33,6 +33,8 @@
 #include <tf2_eigen/tf2_eigen.hpp>
 #endif
 
+#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
+
 #include <tf2_ros/transform_listener.h>
 
 #include <memory>
@@ -59,7 +61,7 @@ class Lanelet2MapFilterComponent : public rclcpp::Node
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
-  rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_sub_;
+  rclcpp::Subscription<autoware_map_msgs::msg::LaneletMapBin>::SharedPtr map_sub_;
   rclcpp::Subscription<PointCloud2>::SharedPtr pointcloud_sub_;
   rclcpp::Publisher<PointCloud2>::SharedPtr filtered_pointcloud_pub_;
 
@@ -71,7 +73,7 @@ class Lanelet2MapFilterComponent : public rclcpp::Node
 
   void pointcloudCallback(const PointCloud2ConstPtr msg);
 
-  void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
+  void mapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg);
 
   bool transformPointCloud(
     const std::string & in_target_frame, const PointCloud2ConstPtr & in_cloud_ptr,

diff --git a/...essor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/...essor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp
@@ -36,10 +36,10 @@ class VectorMapInsideAreaFilterComponent : public pointcloud_preprocessor::Filte
   void filter(
     const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;
 
-  rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_sub_;
+  rclcpp::Subscription<autoware_map_msgs::msg::LaneletMapBin>::SharedPtr map_sub_;
   lanelet::ConstPolygons3d polygon_lanelets_;
 
-  void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
+  void mapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg);
 
   // parameter
   std::string polygon_type_;

diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml
@@ -22,6 +22,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_map_msgs</depend>
   <depend>autoware_auto_geometry</depend>
   <depend>autoware_point_types</depend>
   <depend>cgal</depend>
@@ -49,6 +50,7 @@
   <depend>tier4_debug_msgs</depend>
   <depend>tier4_pcl_extensions</depend>
 
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
   <test_depend>ros_testing</test_depend>

diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp
@@ -51,7 +51,7 @@ Lanelet2MapFilterComponent::Lanelet2MapFilterComponent(const rclcpp::NodeOptions
 
   // Set subscriber
   {
-    map_sub_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
+    map_sub_ = this->create_subscription<autoware_map_msgs::msg::LaneletMapBin>(
       "input/vector_map", rclcpp::QoS{1}.transient_local(),
       std::bind(&Lanelet2MapFilterComponent::mapCallback, this, _1));
     pointcloud_sub_ = this->create_subscription<PointCloud2>(
@@ -259,7 +259,7 @@ void Lanelet2MapFilterComponent::pointcloudCallback(const PointCloud2ConstPtr cl
 }
 
 void Lanelet2MapFilterComponent::mapCallback(
-  const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr map_msg)
+  const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr map_msg)
 {
   lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
   lanelet::utils::conversion::fromBinMsg(*map_msg, lanelet_map_ptr_);

diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp
@@ -71,7 +71,7 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent(
 
   using std::placeholders::_1;
   // Set subscriber
-  map_sub_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
+  map_sub_ = this->create_subscription<autoware_map_msgs::msg::LaneletMapBin>(
     "input/vector_map", rclcpp::QoS{1}.transient_local(),
     std::bind(&VectorMapInsideAreaFilterComponent::mapCallback, this, _1));
 }
@@ -107,7 +107,7 @@ void VectorMapInsideAreaFilterComponent::filter(
 }
 
 void VectorMapInsideAreaFilterComponent::mapCallback(
-  const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr map_msg)
+  const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr map_msg)
 {
   tf_input_frame_ = map_msg->header.frame_id;