From 39f09fdb42e85c34e196bf9aaa9bddcd8fa175d5 Mon Sep 17 00:00:00 2001 From: N-Eiki Date: Mon, 10 Jun 2024 18:09:55 +0900 Subject: [PATCH] feat : git cherry-pick -m1 673f588 # feat(gen2): change lidar frame_id in occupancy_grid_map #802 fix : change msg type to autoware*.msg from autoware_auto*.msg Signed-off-by: N-Eiki --- ...tcloud_based_occupancy_grid_map.param.yaml | 6 +- ...ntcloud_based_occupancy_grid_map.launch.py | 94 ++----------------- .../docs/vector-map-filter.md | 2 +- .../docs/vector-map-inside-area-filter.md | 2 +- .../lanelet2_map_filter_nodelet.hpp | 6 +- .../vector_map_inside_area_filter.hpp | 4 +- sensing/pointcloud_preprocessor/package.xml | 2 + .../lanelet2_map_filter_nodelet.cpp | 4 +- .../vector_map_inside_area_filter.cpp | 4 +- 9 files changed, 22 insertions(+), 102 deletions(-) diff --git a/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml index 878bea4cd8ced..f9a1c4f930001 100644 --- a/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml +++ b/perception/probabilistic_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/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py index b112dd0a84b83..3c6dd495060ee 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_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,23 +111,8 @@ 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 @@ -186,22 +120,14 @@ def launch_setup(context, *args, **kwargs): 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], @@ -209,12 +135,6 @@ def launch_setup(context, *args, **kwargs): ) 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 index fedd3c081cdeb..8bbb966c50e47 100644 --- 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 index 4bafac7872e78..7f6712183b102 100644 --- 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/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp index ed0d753a68ecc..56f2464f47a75 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp @@ -33,6 +33,8 @@ #include #endif +#include + #include #include @@ -59,7 +61,7 @@ class Lanelet2MapFilterComponent : public rclcpp::Node std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; - rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr pointcloud_sub_; rclcpp::Publisher::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/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index 74209c1a22e4c..894768d385438 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/pointcloud_preprocessor/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::SharedPtr map_sub_; + rclcpp::Subscription::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 index 9a1eae786c379..68738f97d25f3 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -22,6 +22,7 @@ ament_cmake_auto autoware_cmake + autoware_map_msgs autoware_auto_geometry autoware_point_types cgal @@ -49,6 +50,7 @@ tier4_debug_msgs tier4_pcl_extensions + ament_lint_auto autoware_lint_common ros_testing 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 index 502c85d1746e8..8f7cb2abec6a4 100644 --- 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( + map_sub_ = this->create_subscription( "input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&Lanelet2MapFilterComponent::mapCallback, this, _1)); pointcloud_sub_ = this->create_subscription( @@ -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::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 index 5b8755714b375..8fcf15326829f 100644 --- 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( + map_sub_ = this->create_subscription( "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;