From ca46285b39734eaab64938bc533c05615545f086 Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 22 Sep 2022 18:48:08 +0900 Subject: [PATCH 01/48] first commit Signed-off-by: kminoda --- launch/tier4_map_launch/CMakeLists.txt | 1 + .../config/pointcloud_map_loader.param.yaml | 8 + launch/tier4_map_launch/launch/map.launch.py | 23 ++- launch/tier4_map_launch/launch/map.launch.xml | 3 +- map/autoware_map_msgs/CMakeLists.txt | 32 ++++ map/autoware_map_msgs/msg/AreaInfo.msg | 3 + map/autoware_map_msgs/msg/PCDMapWithID.msg | 2 + map/autoware_map_msgs/package.xml | 23 +++ .../srv/LoadPCDMapsGeneral.srv | 7 + map/map_loader/CMakeLists.txt | 2 + .../config/pointcloud_map_loader.param.yaml | 8 + map/map_loader/package.xml | 6 +- .../lanelet2_map_visualization_node.cpp | 17 +- .../differential_map_loading_module.cpp | 165 ++++++++++++++++++ .../differential_map_loading_module.hpp | 68 ++++++++ .../pointcloud_map_loader_node.cpp | 67 +++---- .../pointcloud_map_loader_node.hpp | 26 ++- .../pointcloud_map_publisher_module.cpp | 92 ++++++++++ .../pointcloud_map_publisher_module.hpp | 37 ++++ 19 files changed, 526 insertions(+), 64 deletions(-) create mode 100644 launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml create mode 100755 map/autoware_map_msgs/CMakeLists.txt create mode 100755 map/autoware_map_msgs/msg/AreaInfo.msg create mode 100755 map/autoware_map_msgs/msg/PCDMapWithID.msg create mode 100755 map/autoware_map_msgs/package.xml create mode 100755 map/autoware_map_msgs/srv/LoadPCDMapsGeneral.srv create mode 100644 map/map_loader/config/pointcloud_map_loader.param.yaml create mode 100644 map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.cpp create mode 100644 map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.hpp rename map/map_loader/{include/map_loader => src/pointcloud_map_loader}/pointcloud_map_loader_node.hpp (64%) create mode 100644 map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.cpp create mode 100644 map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.hpp diff --git a/launch/tier4_map_launch/CMakeLists.txt b/launch/tier4_map_launch/CMakeLists.txt index 98f12b64f3ca4..b0e770d7c7619 100644 --- a/launch/tier4_map_launch/CMakeLists.txt +++ b/launch/tier4_map_launch/CMakeLists.txt @@ -7,4 +7,5 @@ autoware_package() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml new file mode 100644 index 0000000000000..fa94afbf12a0b --- /dev/null +++ b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + enable_whole_load: true + enable_downsampled_whole_load: false + enable_partial_load: false + + # only used when downsample_whole_load enabled + leaf_size: 3.0 # downsample leaf size [m] diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index 6d5265f5945e9..3678c8c780052 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -32,9 +32,14 @@ def launch_setup(context, *args, **kwargs): lanelet2_map_loader_param_path = LaunchConfiguration("lanelet2_map_loader_param_path").perform( context ) + pointcloud_map_loader_param_path = LaunchConfiguration("pointcloud_map_loader_param_path").perform( + context + ) with open(lanelet2_map_loader_param_path, "r") as f: lanelet2_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(pointcloud_map_loader_param_path, "r") as f: + pointcloud_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] map_hash_generator = Node( package="map_loader", @@ -77,9 +82,15 @@ def launch_setup(context, *args, **kwargs): package="map_loader", plugin="PointCloudMapLoaderNode", name="pointcloud_map_loader", - remappings=[("output/pointcloud_map", "pointcloud_map")], + remappings=[ + ("output/pointcloud_map", "pointcloud_map"), + ("output/debug/downsampled_pointcloud_map", "debug/downsampled_pointcloud_map"), + ], parameters=[ - {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]} + { + "pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"] + }, + pointcloud_map_loader_param, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -149,6 +160,14 @@ def add_launch_arg(name: str, default_value=None, description=None): ], "path to lanelet2_map_loader param file", ), + add_launch_arg( + "pointcloud_map_loader_param_path", + [ + FindPackageShare("tier4_map_launch"), + "/config/pointcloud_map_loader.param.yaml", + ], + "path to pointcloud_map_loader param file", + ), add_launch_arg("use_intra_process", "false", "use ROS2 component container communication"), add_launch_arg("use_multithread", "false", "use multithread"), diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 7dbdd216f9625..f0654a580fe95 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -2,7 +2,7 @@ - + @@ -12,6 +12,7 @@ + diff --git a/map/autoware_map_msgs/CMakeLists.txt b/map/autoware_map_msgs/CMakeLists.txt new file mode 100755 index 0000000000000..d4c2fd83ed70e --- /dev/null +++ b/map/autoware_map_msgs/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_map_msgs) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/AreaInfo.msg" + "msg/PCDMapWithID.msg" + "srv/LoadPCDMapsGeneral.srv" + DEPENDENCIES + std_msgs + geometry_msgs + sensor_msgs +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() +ament_auto_package() diff --git a/map/autoware_map_msgs/msg/AreaInfo.msg b/map/autoware_map_msgs/msg/AreaInfo.msg new file mode 100755 index 0000000000000..9a1b9823f6862 --- /dev/null +++ b/map/autoware_map_msgs/msg/AreaInfo.msg @@ -0,0 +1,3 @@ +# this only supports spherical area message for now +geometry_msgs/Point center +float32 radius diff --git a/map/autoware_map_msgs/msg/PCDMapWithID.msg b/map/autoware_map_msgs/msg/PCDMapWithID.msg new file mode 100755 index 0000000000000..70908af46be19 --- /dev/null +++ b/map/autoware_map_msgs/msg/PCDMapWithID.msg @@ -0,0 +1,2 @@ +string id +sensor_msgs/PointCloud2 pcd diff --git a/map/autoware_map_msgs/package.xml b/map/autoware_map_msgs/package.xml new file mode 100755 index 0000000000000..c8c5f0abc5fc3 --- /dev/null +++ b/map/autoware_map_msgs/package.xml @@ -0,0 +1,23 @@ + + + autoware_map_msgs + 0.0.0 + The autoware_map_msgs package + RyuYamamoto + MIT + + ament_cmake_auto + geometry_msgs + rclcpp + sensor_msgs + std_msgs + tier4_map_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/map/autoware_map_msgs/srv/LoadPCDMapsGeneral.srv b/map/autoware_map_msgs/srv/LoadPCDMapsGeneral.srv new file mode 100755 index 0000000000000..8ad7a0aa10c0f --- /dev/null +++ b/map/autoware_map_msgs/srv/LoadPCDMapsGeneral.srv @@ -0,0 +1,7 @@ +int32 mode # 0: Area load, 1: Differential area load +AreaInfo area +string[] already_loaded_ids # only used when mode = 1 + +--- +autoware_map_msgs/PCDMapWithID[] loaded_pcds +string[] already_loaded_ids # only used when mode = 1 diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 49d294ff1bb79..6c0542eec9d20 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -8,6 +8,8 @@ find_package(PCL REQUIRED COMPONENTS io) ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/pointcloud_map_loader_node.cpp + src/pointcloud_map_loader/differential_map_loading_module.cpp + src/pointcloud_map_loader/pointcloud_map_publisher_module.cpp ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml new file mode 100644 index 0000000000000..bace033ee79ec --- /dev/null +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + enable_whole_load: false + enable_downsampled_whole_load: true + enable_partial_load: true + + # only used when downsample_whole_load enabled + leaf_size: 3.0 # downsample leaf size [m] diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 1f1f258b1fec5..5554e9c2c10db 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -13,16 +13,20 @@ autoware_cmake autoware_auto_mapping_msgs + autoware_map_msgs geometry_msgs lanelet2_extension libpcl-all-dev pcl_conversions + pcl_ros rclcpp rclcpp_components std_msgs + tf2 tf2_geometry_msgs tf2_ros tier4_autoware_utils + tier4_debug_msgs visualization_msgs ament_lint_auto @@ -32,4 +36,4 @@ ament_cmake - + \ No newline at end of file diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 472b28264f617..55b16080c94fa 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -114,16 +114,10 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map); lanelet::ConstPolygons3d obstacle_polygons = lanelet::utils::query::getAllObstaclePolygons(viz_lanelet_map); - lanelet::ConstPolygons3d no_obstacle_segmentation_area = - lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "no_obstacle_segmentation_area"); - lanelet::ConstPolygons3d no_obstacle_segmentation_area_for_run_out = - lanelet::utils::query::getAllPolygonsByType( - viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out"); std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, - cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas, - cl_no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area_for_run_out; + cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas; setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); @@ -139,8 +133,6 @@ void Lanelet2MapVisualizationNode::onMapBin( setColor(&cl_parking_lots, 0.5, 0.5, 0.0, 0.3); setColor(&cl_parking_spaces, 1.0, 0.647, 0.0, 0.6); setColor(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5); - setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); visualization_msgs::msg::MarkerArray map_marker_array; @@ -204,13 +196,6 @@ void Lanelet2MapVisualizationNode::onMapBin( insertMarkerArray( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road)); - insertMarkerArray( - &map_marker_array, lanelet::visualization::noObstacleSegmentationAreaAsMarkerArray( - no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray( - no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out)); pub_marker_->publish(map_marker_array); } diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.cpp new file mode 100644 index 0000000000000..53ad41a8ba9ce --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.cpp @@ -0,0 +1,165 @@ +// Copyright 2022 The Autoware Contributors +// +// 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. + +#include "differential_map_loading_module.hpp" + +bool sphereAndBoxOverlapExists( + const geometry_msgs::msg::Point position, + const double radius, + const pcl::PointXYZ position_min, + const pcl::PointXYZ position_max) +{ + if ( + (position_min.x - radius <= position.x && position.x <= position_max.x + radius && + position_min.y <= position.y && position.y <= position_max.y && position_min.z <= position.z && + position.z <= position_max.z) || + (position_min.x <= position.x && position.x <= position_max.x && + position_min.y - radius <= position.y && position.y <= position_max.y + radius && + position_min.z <= position.z && position.z <= position_max.z) || + (position_min.x <= position.x && position.x <= position_max.x && position_min.y <= position.y && + position.y <= position_max.y && position_min.z - radius <= position.z && + position.z <= position_max.z + radius)) { + return true; + } + double r2 = std::pow(radius, 2.0); + double minx2 = std::pow(position.x - position_min.x, 2.0); + double maxx2 = std::pow(position.x - position_max.x, 2.0); + double miny2 = std::pow(position.y - position_min.y, 2.0); + double maxy2 = std::pow(position.y - position_max.y, 2.0); + double minz2 = std::pow(position.z - position_min.z, 2.0); + double maxz2 = std::pow(position.z - position_max.z, 2.0); + if ( + minx2 + miny2 + minz2 <= r2 || minx2 + miny2 + maxz2 <= r2 || minx2 + maxy2 + minz2 <= r2 || + minx2 + maxy2 + maxz2 <= r2 || maxx2 + miny2 + minz2 <= r2 || maxx2 + miny2 + maxz2 <= r2 || + maxx2 + maxy2 + minz2 <= r2 || maxx2 + maxy2 + maxz2 <= r2) { + return true; + } + return false; +} + +bool isGridWithinQueriedArea( + const autoware_map_msgs::msg::AreaInfo area, + const PCDFileMetadata metadata) +{ + // Currently, the area load only supports spherical area + geometry_msgs::msg::Point position = area.center; + double radius = area.radius; + bool res = sphereAndBoxOverlapExists(position, radius, metadata.min, metadata.max); + return res; +} + +DifferentialMapLoadingModule::DifferentialMapLoadingModule( + rclcpp::Node * node, const std::vector pcd_paths) +: logger_(node->get_logger()) +{ + all_pcd_file_metadata_dict_ = generatePCDMetadata(pcd_paths); + load_pcd_maps_general_service_ = node->create_service( + "load_pcd_maps_general", std::bind( + &DifferentialMapLoadingModule::loadPCDMapsGeneralCallback, this, + std::placeholders::_1, std::placeholders::_2)); +} + +void DifferentialMapLoadingModule::differentialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area, + const std::vector already_loaded_ids, + autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const +{ + // iterate over all the available pcd map grids + for (const auto & ele : all_pcd_file_metadata_dict_) { + std::string path = ele.first; + PCDFileMetadata metadata = ele.second; + + // assume that the map ID = map path (for now) + std::string map_id = path; + + // skip if the pcd file is not within the queried area + if (!isGridWithinQueriedArea(area, metadata)) continue; + + bool is_already_loaded = std::find(already_loaded_ids.begin(), already_loaded_ids.end(), map_id) != already_loaded_ids.end(); + if (is_already_loaded) { + response->already_loaded_ids.push_back(map_id); + } else { + autoware_map_msgs::msg::PCDMapWithID pcd_map_with_id; + loadPCDMapWithID(path, map_id, pcd_map_with_id); + response->loaded_pcds.push_back(pcd_map_with_id); + } + } + RCLCPP_INFO_STREAM(logger_, "Finished diff area loading"); +} + +void DifferentialMapLoadingModule::partialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area, + autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const +{ + // iterate over all the available pcd map grids + + for (const auto & ele : all_pcd_file_metadata_dict_) { + std::string path = ele.first; + PCDFileMetadata metadata = ele.second; + + // assume that the map ID = map path (for now) + std::string map_id = path; + + // skip if the pcd file is not within the queried area + if (!isGridWithinQueriedArea(area, metadata)) continue; + + autoware_map_msgs::msg::PCDMapWithID pcd_map_with_id; + loadPCDMapWithID(path, map_id, pcd_map_with_id); + response->loaded_pcds.push_back(pcd_map_with_id); + } +} + +bool DifferentialMapLoadingModule::loadPCDMapsGeneralCallback( + autoware_map_msgs::srv::LoadPCDMapsGeneral::Request::SharedPtr req, + autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr res) +{ + int mode = req->mode; + if (mode == 0) { + auto area = req->area; + partialAreaLoad(area, res); + } else if (mode == 1) { + auto area = req->area; + std::vector already_loaded_ids = req->already_loaded_ids; + differentialAreaLoad(area, already_loaded_ids, res); + } + return true; +} + +void DifferentialMapLoadingModule::loadPCDMapWithID( + const std::string path, const std::string map_id, + autoware_map_msgs::msg::PCDMapWithID & pcd_map_with_id) const +{ + sensor_msgs::msg::PointCloud2 pcd; + if (pcl::io::loadPCDFile(path, pcd) == -1) { + RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); + } + pcd_map_with_id.pcd = pcd; + pcd_map_with_id.id = map_id; +} + +std::map DifferentialMapLoadingModule::generatePCDMetadata( + const std::vector & pcd_paths) const +{ + pcl::PointCloud partial_pcd; + std::map all_pcd_file_metadata_dict; + for (const auto & path : pcd_paths) { + if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { + RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); + } + PCDFileMetadata metadata = {}; + pcl::getMinMax3D(partial_pcd, metadata.min, metadata.max); + all_pcd_file_metadata_dict[path] = metadata; + } + return all_pcd_file_metadata_dict; +} diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.hpp new file mode 100644 index 0000000000000..995ee0e2f4d12 --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.hpp @@ -0,0 +1,68 @@ +// Copyright 2022 The Autoware Contributors +// +// 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. + +#ifndef DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ +#define DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" + +#include +#include +#include + +struct PCDFileMetadata +{ + pcl::PointXYZ min; + pcl::PointXYZ max; +}; + +class DifferentialMapLoadingModule +{ + +public: + explicit DifferentialMapLoadingModule( + rclcpp::Node * node, const std::vector pcd_paths); + +private: + rclcpp::Logger logger_; + + std::map all_pcd_file_metadata_dict_; + rclcpp::Service::SharedPtr + load_pcd_maps_general_service_; + + bool loadPCDMapsGeneralCallback( + autoware_map_msgs::srv::LoadPCDMapsGeneral::Request::SharedPtr req, + autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr res); + void partialAreaLoad(const autoware_map_msgs::msg::AreaInfo area, + autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const; + void differentialAreaLoad(const autoware_map_msgs::msg::AreaInfo area_info, + const std::vector already_loaded_ids, + autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const; + void loadPCDMapWithID( + const std::string path, const std::string map_id, + autoware_map_msgs::msg::PCDMapWithID & pcd_map_with_id) const; + std::map generatePCDMetadata( + const std::vector & pcd_paths) const; +}; + +#endif // DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ \ No newline at end of file diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 99f46ab58fa71..014565d41a605 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -28,9 +28,10 @@ * limitations under the License. */ -#include "map_loader/pointcloud_map_loader_node.hpp" +#include "pointcloud_map_loader_node.hpp" #include +#include #include #include @@ -61,17 +62,32 @@ bool isPcdFile(const std::string & p) PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & options) : Node("pointcloud_map_loader", options) { - rclcpp::QoS durable_qos{1}; - durable_qos.transient_local(); - pub_pointcloud_map_ = - this->create_publisher("output/pointcloud_map", durable_qos); + leaf_size_ = declare_parameter("leaf_size", 3.0); - const auto pcd_paths_or_directory = - declare_parameter("pcd_paths_or_directory", std::vector({})); + const auto pcd_paths = getPcdPaths( + declare_parameter("pcd_paths_or_directory", std::vector({}))); - std::vector pcd_paths{}; + if (declare_parameter("enable_whole_load", false)) { + std::string publisher_name = "output/pointcloud_map"; + pcd_map_publisher_ = std::make_unique(this, pcd_paths, publisher_name, boost::none); + } + + if (declare_parameter("enable_downsampled_whole_load", true)) { + std::string publisher_name = "output/debug/downsampled_pointcloud_map"; + downsampled_pcd_map_publisher_ = std::make_unique(this, pcd_paths, publisher_name, leaf_size_); + } + + if (declare_parameter("enable_partial_load", true)) { + differential_map_loading_ = std::make_unique(this, pcd_paths); + } +} +std::vector PointCloudMapLoaderNode::getPcdPaths( + const std::vector & pcd_paths_or_directory) const +{ + std::vector pcd_paths; for (const auto & p : pcd_paths_or_directory) { + (void)p; if (!fs::exists(p)) { RCLCPP_ERROR_STREAM(get_logger(), "invalid path: " << p); } @@ -89,42 +105,9 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt } } } - - const auto pcd = loadPCDFiles(pcd_paths); - - if (pcd.width == 0) { - RCLCPP_ERROR(get_logger(), "No PCD was loaded: pcd_paths.size() = %zu", pcd_paths.size()); - return; - } - - pub_pointcloud_map_->publish(pcd); + return pcd_paths; } -sensor_msgs::msg::PointCloud2 PointCloudMapLoaderNode::loadPCDFiles( - const std::vector & pcd_paths) -{ - sensor_msgs::msg::PointCloud2 whole_pcd{}; - - sensor_msgs::msg::PointCloud2 partial_pcd; - for (const auto & path : pcd_paths) { - if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { - RCLCPP_ERROR_STREAM(get_logger(), "PCD load failed: " << path); - } - - if (whole_pcd.width == 0) { - whole_pcd = partial_pcd; - } else { - whole_pcd.width += partial_pcd.width; - whole_pcd.row_step += partial_pcd.row_step; - whole_pcd.data.reserve(whole_pcd.data.size() + partial_pcd.data.size()); - whole_pcd.data.insert(whole_pcd.data.end(), partial_pcd.data.begin(), partial_pcd.data.end()); - } - } - - whole_pcd.header.frame_id = "map"; - - return whole_pcd; -} #include RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudMapLoaderNode) diff --git a/map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp similarity index 64% rename from map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp rename to map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index 6bc55ef642706..f9223ec0f8a7f 100644 --- a/map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -31,10 +31,26 @@ #ifndef MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ #define MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +// #include "autoware_map_msgs/srv/load_pcd_partially.hpp" +// #include "autoware_map_msgs/srv/load_pcd_partially_for_publish.hpp" +#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" +#include "autoware_map_msgs/msg/area_info.hpp" +#include "autoware_map_msgs/msg/pcd_map_with_id.hpp" + +#include "differential_map_loading_module.hpp" +#include "pointcloud_map_publisher_module.hpp" + #include +#include #include +#include +#include +#include + +#include +#include #include #include @@ -44,9 +60,15 @@ class PointCloudMapLoaderNode : public rclcpp::Node explicit PointCloudMapLoaderNode(const rclcpp::NodeOptions & options); private: - rclcpp::Publisher::SharedPtr pub_pointcloud_map_; + // ros param + float leaf_size_; + + std::unique_ptr differential_map_loading_; + std::unique_ptr pcd_map_publisher_; + std::unique_ptr downsampled_pcd_map_publisher_; - sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector & pcd_paths); + std::vector getPcdPaths( + const std::vector & pcd_paths_or_directory) const; }; #endif // MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.cpp new file mode 100644 index 0000000000000..5bbc86d38b69c --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.cpp @@ -0,0 +1,92 @@ +// Copyright 2022 The Autoware Contributors +// +// 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. + +#include "pointcloud_map_publisher_module.hpp" + +#include +#include +#include + +sensor_msgs::msg::PointCloud2 downsample(const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) +{ + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::fromROSMsg(msg_input, *pcl_input); + pcl::VoxelGrid filter; + filter.setInputCloud(pcl_input); + filter.setLeafSize(leaf_size, leaf_size, leaf_size); + filter.filter(*pcl_output); + + sensor_msgs::msg::PointCloud2 msg_output; + pcl::toROSMsg(*pcl_output, msg_output); + msg_output.header = msg_input.header; + return msg_output; +} + + +PointcloudMapPublisherModule::PointcloudMapPublisherModule( + rclcpp::Node * node, const std::vector & pcd_paths, + const std::string publisher_name, const boost::optional leaf_size) +: logger_(node->get_logger()) +{ + rclcpp::QoS durable_qos{1}; + durable_qos.transient_local(); + pub_pointcloud_map_ = node->create_publisher( + publisher_name, durable_qos); + sensor_msgs::msg::PointCloud2 pcd = loadPCDFiles(pcd_paths, leaf_size); + + if (pcd.width == 0) { + RCLCPP_ERROR(logger_, "No PCD was loaded: pcd_paths.size() = %zu", pcd_paths.size()); + return; + } + + pcd.header.frame_id = "map"; + pub_pointcloud_map_->publish(pcd); +} + +sensor_msgs::msg::PointCloud2 PointcloudMapPublisherModule::loadPCDFiles( + const std::vector & pcd_paths, const boost::optional leaf_size) const +{ + sensor_msgs::msg::PointCloud2 whole_pcd; + sensor_msgs::msg::PointCloud2 partial_pcd; + + for (int i = 0; i < int(pcd_paths.size()); ++i) { + auto & path = pcd_paths[i]; + if (i % 50 == 0) { + RCLCPP_INFO_STREAM( + logger_, + "Load " << path << " (" << i + 1 << " out of " << int(pcd_paths.size()) << ")"); + } + + if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { + RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); + } + + if (leaf_size) { + partial_pcd = downsample(partial_pcd, leaf_size.get()); + } + + if (whole_pcd.width == 0) { + whole_pcd = partial_pcd; + } else { + whole_pcd.width += partial_pcd.width; + whole_pcd.row_step += partial_pcd.row_step; + whole_pcd.data.insert(whole_pcd.data.end(), partial_pcd.data.begin(), partial_pcd.data.end()); + } + } + + whole_pcd.header.frame_id = "map"; + + return whole_pcd; +} diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.hpp new file mode 100644 index 0000000000000..18dc05cd7c63f --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.hpp @@ -0,0 +1,37 @@ +// Copyright 2022 The Autoware Contributors +// +// 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. + +#ifndef POINTCLOUD_MAP_PUBLISHER_MODULE_MODULE_HPP_ +#define POINTCLOUD_MAP_PUBLISHER_MODULE_MODULE_HPP_ + +#include + +#include +#include + +class PointcloudMapPublisherModule +{ +public: + explicit PointcloudMapPublisherModule( + rclcpp::Node * node, const std::vector & pcd_paths, + const std::string publisher_name, const boost::optional leaf_size); + +private: + rclcpp::Logger logger_; + rclcpp::Publisher::SharedPtr pub_pointcloud_map_; + + sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector & pcd_paths, const boost::optional leaf_size) const; +}; + +#endif // POINTCLOUD_MAP_PUBLISHER_MODULE_MODULE_HPP_ \ No newline at end of file From adbc19c9bc9dfbbddc113459d062941ee110cc9f Mon Sep 17 00:00:00 2001 From: kminoda Date: Thu, 22 Sep 2022 18:55:02 +0900 Subject: [PATCH 02/48] reverted unnecessary modification Signed-off-by: kminoda --- .../lanelet2_map_visualization_node.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 55b16080c94fa..472b28264f617 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -114,10 +114,16 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map); lanelet::ConstPolygons3d obstacle_polygons = lanelet::utils::query::getAllObstaclePolygons(viz_lanelet_map); + lanelet::ConstPolygons3d no_obstacle_segmentation_area = + lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "no_obstacle_segmentation_area"); + lanelet::ConstPolygons3d no_obstacle_segmentation_area_for_run_out = + lanelet::utils::query::getAllPolygonsByType( + viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out"); std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, - cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas; + cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas, + cl_no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area_for_run_out; setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); @@ -133,6 +139,8 @@ void Lanelet2MapVisualizationNode::onMapBin( setColor(&cl_parking_lots, 0.5, 0.5, 0.0, 0.3); setColor(&cl_parking_spaces, 1.0, 0.647, 0.0, 0.6); setColor(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); + setColor(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5); + setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); visualization_msgs::msg::MarkerArray map_marker_array; @@ -196,6 +204,13 @@ void Lanelet2MapVisualizationNode::onMapBin( insertMarkerArray( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::noObstacleSegmentationAreaAsMarkerArray( + no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray( + no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out)); pub_marker_->publish(map_marker_array); } From f5eecaeefe9bf1bff27cb8c8b01e5f9f5397f03a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 22 Sep 2022 09:58:51 +0000 Subject: [PATCH 03/48] ci(pre-commit): autofix --- launch/tier4_map_launch/launch/map.launch.py | 10 +++----- map/map_loader/package.xml | 2 +- .../differential_map_loading_module.cpp | 18 ++++++------- .../differential_map_loading_module.hpp | 25 ++++++++++--------- .../pointcloud_map_loader_node.cpp | 11 ++++---- .../pointcloud_map_loader_node.hpp | 15 ++++++----- .../pointcloud_map_publisher_module.cpp | 15 ++++++----- .../pointcloud_map_publisher_module.hpp | 10 +++++--- 8 files changed, 52 insertions(+), 54 deletions(-) diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index 3678c8c780052..3f73195ec98c7 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -32,9 +32,9 @@ def launch_setup(context, *args, **kwargs): lanelet2_map_loader_param_path = LaunchConfiguration("lanelet2_map_loader_param_path").perform( context ) - pointcloud_map_loader_param_path = LaunchConfiguration("pointcloud_map_loader_param_path").perform( - context - ) + pointcloud_map_loader_param_path = LaunchConfiguration( + "pointcloud_map_loader_param_path" + ).perform(context) with open(lanelet2_map_loader_param_path, "r") as f: lanelet2_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] @@ -87,9 +87,7 @@ def launch_setup(context, *args, **kwargs): ("output/debug/downsampled_pointcloud_map", "debug/downsampled_pointcloud_map"), ], parameters=[ - { - "pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"] - }, + {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]}, pointcloud_map_loader_param, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 5554e9c2c10db..9fac58f667e86 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -36,4 +36,4 @@ ament_cmake - \ No newline at end of file + diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.cpp index 53ad41a8ba9ce..07b9dd9224b58 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.cpp @@ -15,9 +15,7 @@ #include "differential_map_loading_module.hpp" bool sphereAndBoxOverlapExists( - const geometry_msgs::msg::Point position, - const double radius, - const pcl::PointXYZ position_min, + const geometry_msgs::msg::Point position, const double radius, const pcl::PointXYZ position_min, const pcl::PointXYZ position_max) { if ( @@ -49,8 +47,7 @@ bool sphereAndBoxOverlapExists( } bool isGridWithinQueriedArea( - const autoware_map_msgs::msg::AreaInfo area, - const PCDFileMetadata metadata) + const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata) { // Currently, the area load only supports spherical area geometry_msgs::msg::Point position = area.center; @@ -66,13 +63,12 @@ DifferentialMapLoadingModule::DifferentialMapLoadingModule( all_pcd_file_metadata_dict_ = generatePCDMetadata(pcd_paths); load_pcd_maps_general_service_ = node->create_service( "load_pcd_maps_general", std::bind( - &DifferentialMapLoadingModule::loadPCDMapsGeneralCallback, this, - std::placeholders::_1, std::placeholders::_2)); + &DifferentialMapLoadingModule::loadPCDMapsGeneralCallback, this, + std::placeholders::_1, std::placeholders::_2)); } void DifferentialMapLoadingModule::differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, - const std::vector already_loaded_ids, + const autoware_map_msgs::msg::AreaInfo area, const std::vector already_loaded_ids, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const { // iterate over all the available pcd map grids @@ -86,7 +82,9 @@ void DifferentialMapLoadingModule::differentialAreaLoad( // skip if the pcd file is not within the queried area if (!isGridWithinQueriedArea(area, metadata)) continue; - bool is_already_loaded = std::find(already_loaded_ids.begin(), already_loaded_ids.end(), map_id) != already_loaded_ids.end(); + bool is_already_loaded = + std::find(already_loaded_ids.begin(), already_loaded_ids.end(), map_id) != + already_loaded_ids.end(); if (is_already_loaded) { response->already_loaded_ids.push_back(map_id); } else { diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.hpp index 995ee0e2f4d12..ca1004210e74e 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.hpp @@ -12,23 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ -#define DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ +#ifndef POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ +#define POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ #include +#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" + #include +#include +#include #include #include -#include -#include #include -#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" - +#include #include #include -#include struct PCDFileMetadata { @@ -38,11 +38,10 @@ struct PCDFileMetadata class DifferentialMapLoadingModule { - public: explicit DifferentialMapLoadingModule( rclcpp::Node * node, const std::vector pcd_paths); - + private: rclcpp::Logger logger_; @@ -53,9 +52,11 @@ class DifferentialMapLoadingModule bool loadPCDMapsGeneralCallback( autoware_map_msgs::srv::LoadPCDMapsGeneral::Request::SharedPtr req, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr res); - void partialAreaLoad(const autoware_map_msgs::msg::AreaInfo area, + void partialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const; - void differentialAreaLoad(const autoware_map_msgs::msg::AreaInfo area_info, + void differentialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area_info, const std::vector already_loaded_ids, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const; void loadPCDMapWithID( @@ -65,4 +66,4 @@ class DifferentialMapLoadingModule const std::vector & pcd_paths) const; }; -#endif // DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ \ No newline at end of file +#endif // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 014565d41a605..0c4d720cd17ed 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -64,17 +64,19 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt { leaf_size_ = declare_parameter("leaf_size", 3.0); - const auto pcd_paths = getPcdPaths( - declare_parameter("pcd_paths_or_directory", std::vector({}))); + const auto pcd_paths = + getPcdPaths(declare_parameter("pcd_paths_or_directory", std::vector({}))); if (declare_parameter("enable_whole_load", false)) { std::string publisher_name = "output/pointcloud_map"; - pcd_map_publisher_ = std::make_unique(this, pcd_paths, publisher_name, boost::none); + pcd_map_publisher_ = + std::make_unique(this, pcd_paths, publisher_name, boost::none); } if (declare_parameter("enable_downsampled_whole_load", true)) { std::string publisher_name = "output/debug/downsampled_pointcloud_map"; - downsampled_pcd_map_publisher_ = std::make_unique(this, pcd_paths, publisher_name, leaf_size_); + downsampled_pcd_map_publisher_ = + std::make_unique(this, pcd_paths, publisher_name, leaf_size_); } if (declare_parameter("enable_partial_load", true)) { @@ -108,6 +110,5 @@ std::vector PointCloudMapLoaderNode::getPcdPaths( return pcd_paths; } - #include RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudMapLoaderNode) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index f9223ec0f8a7f..8e447fa7dfbb0 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -28,22 +28,21 @@ * limitations under the License. */ -#ifndef MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ -#define MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ // #include "autoware_map_msgs/srv/load_pcd_partially.hpp" // #include "autoware_map_msgs/srv/load_pcd_partially_for_publish.hpp" -#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" -#include "autoware_map_msgs/msg/area_info.hpp" -#include "autoware_map_msgs/msg/pcd_map_with_id.hpp" - #include "differential_map_loading_module.hpp" #include "pointcloud_map_publisher_module.hpp" #include -#include +#include "autoware_map_msgs/msg/area_info.hpp" +#include "autoware_map_msgs/msg/pcd_map_with_id.hpp" +#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" #include +#include #include #include @@ -71,4 +70,4 @@ class PointCloudMapLoaderNode : public rclcpp::Node const std::vector & pcd_paths_or_directory) const; }; -#endif // MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#endif // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.cpp index 5bbc86d38b69c..51f0a3321036e 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.cpp @@ -18,7 +18,8 @@ #include #include -sensor_msgs::msg::PointCloud2 downsample(const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) +sensor_msgs::msg::PointCloud2 downsample( + const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) { pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); @@ -34,16 +35,15 @@ sensor_msgs::msg::PointCloud2 downsample(const sensor_msgs::msg::PointCloud2 & m return msg_output; } - PointcloudMapPublisherModule::PointcloudMapPublisherModule( - rclcpp::Node * node, const std::vector & pcd_paths, - const std::string publisher_name, const boost::optional leaf_size) + rclcpp::Node * node, const std::vector & pcd_paths, const std::string publisher_name, + const boost::optional leaf_size) : logger_(node->get_logger()) { rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); - pub_pointcloud_map_ = node->create_publisher( - publisher_name, durable_qos); + pub_pointcloud_map_ = + node->create_publisher(publisher_name, durable_qos); sensor_msgs::msg::PointCloud2 pcd = loadPCDFiles(pcd_paths, leaf_size); if (pcd.width == 0) { @@ -65,8 +65,7 @@ sensor_msgs::msg::PointCloud2 PointcloudMapPublisherModule::loadPCDFiles( auto & path = pcd_paths[i]; if (i % 50 == 0) { RCLCPP_INFO_STREAM( - logger_, - "Load " << path << " (" << i + 1 << " out of " << int(pcd_paths.size()) << ")"); + logger_, "Load " << path << " (" << i + 1 << " out of " << int(pcd_paths.size()) << ")"); } if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.hpp index 18dc05cd7c63f..83478e363e928 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_MAP_PUBLISHER_MODULE_MODULE_HPP_ -#define POINTCLOUD_MAP_PUBLISHER_MODULE_MODULE_HPP_ +#ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_PUBLISHER_MODULE_HPP_ +#define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_PUBLISHER_MODULE_HPP_ #include #include + #include class PointcloudMapPublisherModule @@ -31,7 +32,8 @@ class PointcloudMapPublisherModule rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_pointcloud_map_; - sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector & pcd_paths, const boost::optional leaf_size) const; + sensor_msgs::msg::PointCloud2 loadPCDFiles( + const std::vector & pcd_paths, const boost::optional leaf_size) const; }; -#endif // POINTCLOUD_MAP_PUBLISHER_MODULE_MODULE_HPP_ \ No newline at end of file +#endif // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_PUBLISHER_MODULE_HPP_ From 7ec56903b39c8d0b67e2771076169254100d99e8 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 26 Sep 2022 09:54:59 +0900 Subject: [PATCH 04/48] renamed some classes Signed-off-by: kminoda --- map/map_loader/CMakeLists.txt | 4 +-- ...cpp => differential_map_loader_module.cpp} | 32 ++++++++++--------- ...hpp => differential_map_loader_module.hpp} | 29 ++++++++--------- ...e.cpp => pointcloud_map_loader_module.cpp} | 21 ++++++------ ...e.hpp => pointcloud_map_loader_module.hpp} | 14 ++++---- .../pointcloud_map_loader_node.cpp | 13 ++++---- .../pointcloud_map_loader_node.hpp | 25 ++++++++------- 7 files changed, 69 insertions(+), 69 deletions(-) rename map/map_loader/src/pointcloud_map_loader/{differential_map_loading_module.cpp => differential_map_loader_module.cpp} (84%) rename map/map_loader/src/pointcloud_map_loader/{differential_map_loading_module.hpp => differential_map_loader_module.hpp} (81%) rename map/map_loader/src/pointcloud_map_loader/{pointcloud_map_publisher_module.cpp => pointcloud_map_loader_module.cpp} (81%) rename map/map_loader/src/pointcloud_map_loader/{pointcloud_map_publisher_module.hpp => pointcloud_map_loader_module.hpp} (69%) diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 6c0542eec9d20..31b0884a5eb86 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -8,8 +8,8 @@ find_package(PCL REQUIRED COMPONENTS io) ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/pointcloud_map_loader_node.cpp - src/pointcloud_map_loader/differential_map_loading_module.cpp - src/pointcloud_map_loader/pointcloud_map_publisher_module.cpp + src/pointcloud_map_loader/differential_map_loader_module.cpp + src/pointcloud_map_loader/pointcloud_map_loader_module.cpp ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp similarity index 84% rename from map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.cpp rename to map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 07b9dd9224b58..475c637d748a5 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "differential_map_loading_module.hpp" +#include "differential_map_loader_module.hpp" bool sphereAndBoxOverlapExists( - const geometry_msgs::msg::Point position, const double radius, const pcl::PointXYZ position_min, + const geometry_msgs::msg::Point position, + const double radius, + const pcl::PointXYZ position_min, const pcl::PointXYZ position_max) { if ( @@ -47,7 +49,8 @@ bool sphereAndBoxOverlapExists( } bool isGridWithinQueriedArea( - const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata) + const autoware_map_msgs::msg::AreaInfo area, + const PCDFileMetadata metadata) { // Currently, the area load only supports spherical area geometry_msgs::msg::Point position = area.center; @@ -56,19 +59,20 @@ bool isGridWithinQueriedArea( return res; } -DifferentialMapLoadingModule::DifferentialMapLoadingModule( +DifferentialMapLoaderModule::DifferentialMapLoaderModule( rclcpp::Node * node, const std::vector pcd_paths) : logger_(node->get_logger()) { all_pcd_file_metadata_dict_ = generatePCDMetadata(pcd_paths); load_pcd_maps_general_service_ = node->create_service( "load_pcd_maps_general", std::bind( - &DifferentialMapLoadingModule::loadPCDMapsGeneralCallback, this, - std::placeholders::_1, std::placeholders::_2)); + &DifferentialMapLoaderModule::loadPCDMapsGeneralCallback, this, + std::placeholders::_1, std::placeholders::_2)); } -void DifferentialMapLoadingModule::differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, const std::vector already_loaded_ids, +void DifferentialMapLoaderModule::differentialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area, + const std::vector already_loaded_ids, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const { // iterate over all the available pcd map grids @@ -82,9 +86,7 @@ void DifferentialMapLoadingModule::differentialAreaLoad( // skip if the pcd file is not within the queried area if (!isGridWithinQueriedArea(area, metadata)) continue; - bool is_already_loaded = - std::find(already_loaded_ids.begin(), already_loaded_ids.end(), map_id) != - already_loaded_ids.end(); + bool is_already_loaded = std::find(already_loaded_ids.begin(), already_loaded_ids.end(), map_id) != already_loaded_ids.end(); if (is_already_loaded) { response->already_loaded_ids.push_back(map_id); } else { @@ -96,7 +98,7 @@ void DifferentialMapLoadingModule::differentialAreaLoad( RCLCPP_INFO_STREAM(logger_, "Finished diff area loading"); } -void DifferentialMapLoadingModule::partialAreaLoad( +void DifferentialMapLoaderModule::partialAreaLoad( const autoware_map_msgs::msg::AreaInfo area, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const { @@ -118,7 +120,7 @@ void DifferentialMapLoadingModule::partialAreaLoad( } } -bool DifferentialMapLoadingModule::loadPCDMapsGeneralCallback( +bool DifferentialMapLoaderModule::loadPCDMapsGeneralCallback( autoware_map_msgs::srv::LoadPCDMapsGeneral::Request::SharedPtr req, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr res) { @@ -134,7 +136,7 @@ bool DifferentialMapLoadingModule::loadPCDMapsGeneralCallback( return true; } -void DifferentialMapLoadingModule::loadPCDMapWithID( +void DifferentialMapLoaderModule::loadPCDMapWithID( const std::string path, const std::string map_id, autoware_map_msgs::msg::PCDMapWithID & pcd_map_with_id) const { @@ -146,7 +148,7 @@ void DifferentialMapLoadingModule::loadPCDMapWithID( pcd_map_with_id.id = map_id; } -std::map DifferentialMapLoadingModule::generatePCDMetadata( +std::map DifferentialMapLoaderModule::generatePCDMetadata( const std::vector & pcd_paths) const { pcl::PointCloud partial_pcd; diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp similarity index 81% rename from map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.hpp rename to map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index ca1004210e74e..9f97b15d3106a 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loading_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -12,23 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ -#define POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ +#ifndef DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ +#define DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ #include -#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" - #include -#include -#include #include #include +#include +#include #include -#include +#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" + #include #include +#include struct PCDFileMetadata { @@ -36,12 +36,13 @@ struct PCDFileMetadata pcl::PointXYZ max; }; -class DifferentialMapLoadingModule +class DifferentialMapLoaderModule { + public: - explicit DifferentialMapLoadingModule( + explicit DifferentialMapLoaderModule( rclcpp::Node * node, const std::vector pcd_paths); - + private: rclcpp::Logger logger_; @@ -52,11 +53,9 @@ class DifferentialMapLoadingModule bool loadPCDMapsGeneralCallback( autoware_map_msgs::srv::LoadPCDMapsGeneral::Request::SharedPtr req, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr res); - void partialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, + void partialAreaLoad(const autoware_map_msgs::msg::AreaInfo area, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const; - void differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area_info, + void differentialAreaLoad(const autoware_map_msgs::msg::AreaInfo area_info, const std::vector already_loaded_ids, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const; void loadPCDMapWithID( @@ -66,4 +65,4 @@ class DifferentialMapLoadingModule const std::vector & pcd_paths) const; }; -#endif // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ +#endif // DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ \ No newline at end of file diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp similarity index 81% rename from map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.cpp rename to map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index 51f0a3321036e..4764e2118c155 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pointcloud_map_publisher_module.hpp" +#include "pointcloud_map_loader_module.hpp" #include #include #include -sensor_msgs::msg::PointCloud2 downsample( - const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) +sensor_msgs::msg::PointCloud2 downsample(const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) { pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); @@ -35,15 +34,16 @@ sensor_msgs::msg::PointCloud2 downsample( return msg_output; } -PointcloudMapPublisherModule::PointcloudMapPublisherModule( - rclcpp::Node * node, const std::vector & pcd_paths, const std::string publisher_name, - const boost::optional leaf_size) + +PointcloudMapLoaderModule::PointcloudMapLoaderModule( + rclcpp::Node * node, const std::vector & pcd_paths, + const std::string publisher_name, const boost::optional leaf_size) : logger_(node->get_logger()) { rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); - pub_pointcloud_map_ = - node->create_publisher(publisher_name, durable_qos); + pub_pointcloud_map_ = node->create_publisher( + publisher_name, durable_qos); sensor_msgs::msg::PointCloud2 pcd = loadPCDFiles(pcd_paths, leaf_size); if (pcd.width == 0) { @@ -55,7 +55,7 @@ PointcloudMapPublisherModule::PointcloudMapPublisherModule( pub_pointcloud_map_->publish(pcd); } -sensor_msgs::msg::PointCloud2 PointcloudMapPublisherModule::loadPCDFiles( +sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( const std::vector & pcd_paths, const boost::optional leaf_size) const { sensor_msgs::msg::PointCloud2 whole_pcd; @@ -65,7 +65,8 @@ sensor_msgs::msg::PointCloud2 PointcloudMapPublisherModule::loadPCDFiles( auto & path = pcd_paths[i]; if (i % 50 == 0) { RCLCPP_INFO_STREAM( - logger_, "Load " << path << " (" << i + 1 << " out of " << int(pcd_paths.size()) << ")"); + logger_, + "Load " << path << " (" << i + 1 << " out of " << int(pcd_paths.size()) << ")"); } if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp similarity index 69% rename from map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.hpp rename to map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index 83478e363e928..619ab99acd1bb 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_publisher_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -12,19 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_PUBLISHER_MODULE_HPP_ -#define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_PUBLISHER_MODULE_HPP_ +#ifndef POINTCLOUD_MAP_LOADER_MODULE_MODULE_HPP_ +#define POINTCLOUD_MAP_LOADER_MODULE_MODULE_HPP_ #include #include - #include -class PointcloudMapPublisherModule +class PointcloudMapLoaderModule { public: - explicit PointcloudMapPublisherModule( + explicit PointcloudMapLoaderModule( rclcpp::Node * node, const std::vector & pcd_paths, const std::string publisher_name, const boost::optional leaf_size); @@ -32,8 +31,7 @@ class PointcloudMapPublisherModule rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_pointcloud_map_; - sensor_msgs::msg::PointCloud2 loadPCDFiles( - const std::vector & pcd_paths, const boost::optional leaf_size) const; + sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector & pcd_paths, const boost::optional leaf_size) const; }; -#endif // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_PUBLISHER_MODULE_HPP_ +#endif // POINTCLOUD_MAP_LOADER_MODULE_MODULE_HPP_ \ No newline at end of file diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 0c4d720cd17ed..b0b9ce7151844 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -64,23 +64,21 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt { leaf_size_ = declare_parameter("leaf_size", 3.0); - const auto pcd_paths = - getPcdPaths(declare_parameter("pcd_paths_or_directory", std::vector({}))); + const auto pcd_paths = getPcdPaths( + declare_parameter("pcd_paths_or_directory", std::vector({}))); if (declare_parameter("enable_whole_load", false)) { std::string publisher_name = "output/pointcloud_map"; - pcd_map_publisher_ = - std::make_unique(this, pcd_paths, publisher_name, boost::none); + pcd_map_loader_ = std::make_unique(this, pcd_paths, publisher_name, boost::none); } if (declare_parameter("enable_downsampled_whole_load", true)) { std::string publisher_name = "output/debug/downsampled_pointcloud_map"; - downsampled_pcd_map_publisher_ = - std::make_unique(this, pcd_paths, publisher_name, leaf_size_); + downsampled_pcd_map_loader_ = std::make_unique(this, pcd_paths, publisher_name, leaf_size_); } if (declare_parameter("enable_partial_load", true)) { - differential_map_loading_ = std::make_unique(this, pcd_paths); + differential_map_loader_ = std::make_unique(this, pcd_paths); } } @@ -110,5 +108,6 @@ std::vector PointCloudMapLoaderNode::getPcdPaths( return pcd_paths; } + #include RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudMapLoaderNode) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index 8e447fa7dfbb0..cea40f935dca4 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -28,21 +28,22 @@ * limitations under the License. */ -#ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ -#define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#ifndef MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#define MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ // #include "autoware_map_msgs/srv/load_pcd_partially.hpp" // #include "autoware_map_msgs/srv/load_pcd_partially_for_publish.hpp" -#include "differential_map_loading_module.hpp" -#include "pointcloud_map_publisher_module.hpp" +#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" +#include "autoware_map_msgs/msg/area_info.hpp" +#include "autoware_map_msgs/msg/pcd_map_with_id.hpp" + +#include "differential_map_loader_module.hpp" +#include "pointcloud_map_loader_module.hpp" #include -#include "autoware_map_msgs/msg/area_info.hpp" -#include "autoware_map_msgs/msg/pcd_map_with_id.hpp" -#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" -#include #include +#include #include #include @@ -62,12 +63,12 @@ class PointCloudMapLoaderNode : public rclcpp::Node // ros param float leaf_size_; - std::unique_ptr differential_map_loading_; - std::unique_ptr pcd_map_publisher_; - std::unique_ptr downsampled_pcd_map_publisher_; + std::unique_ptr differential_map_loader_; + std::unique_ptr pcd_map_loader_; + std::unique_ptr downsampled_pcd_map_loader_; std::vector getPcdPaths( const std::vector & pcd_paths_or_directory) const; }; -#endif // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#endif // MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ From c4d20e58945e998bb122fb3ce0a9a1bdc2790bd9 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 26 Sep 2022 00:56:30 +0000 Subject: [PATCH 05/48] ci(pre-commit): autofix --- .../differential_map_loader_module.cpp | 18 ++++++------- .../differential_map_loader_module.hpp | 25 ++++++++++--------- .../pointcloud_map_loader_module.cpp | 15 ++++++----- .../pointcloud_map_loader_module.hpp | 10 +++++--- .../pointcloud_map_loader_node.cpp | 11 ++++---- .../pointcloud_map_loader_node.hpp | 15 ++++++----- 6 files changed, 47 insertions(+), 47 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 475c637d748a5..cc3d7ca9aa2e4 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -15,9 +15,7 @@ #include "differential_map_loader_module.hpp" bool sphereAndBoxOverlapExists( - const geometry_msgs::msg::Point position, - const double radius, - const pcl::PointXYZ position_min, + const geometry_msgs::msg::Point position, const double radius, const pcl::PointXYZ position_min, const pcl::PointXYZ position_max) { if ( @@ -49,8 +47,7 @@ bool sphereAndBoxOverlapExists( } bool isGridWithinQueriedArea( - const autoware_map_msgs::msg::AreaInfo area, - const PCDFileMetadata metadata) + const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata) { // Currently, the area load only supports spherical area geometry_msgs::msg::Point position = area.center; @@ -66,13 +63,12 @@ DifferentialMapLoaderModule::DifferentialMapLoaderModule( all_pcd_file_metadata_dict_ = generatePCDMetadata(pcd_paths); load_pcd_maps_general_service_ = node->create_service( "load_pcd_maps_general", std::bind( - &DifferentialMapLoaderModule::loadPCDMapsGeneralCallback, this, - std::placeholders::_1, std::placeholders::_2)); + &DifferentialMapLoaderModule::loadPCDMapsGeneralCallback, this, + std::placeholders::_1, std::placeholders::_2)); } void DifferentialMapLoaderModule::differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, - const std::vector already_loaded_ids, + const autoware_map_msgs::msg::AreaInfo area, const std::vector already_loaded_ids, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const { // iterate over all the available pcd map grids @@ -86,7 +82,9 @@ void DifferentialMapLoaderModule::differentialAreaLoad( // skip if the pcd file is not within the queried area if (!isGridWithinQueriedArea(area, metadata)) continue; - bool is_already_loaded = std::find(already_loaded_ids.begin(), already_loaded_ids.end(), map_id) != already_loaded_ids.end(); + bool is_already_loaded = + std::find(already_loaded_ids.begin(), already_loaded_ids.end(), map_id) != + already_loaded_ids.end(); if (is_already_loaded) { response->already_loaded_ids.push_back(map_id); } else { diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index 9f97b15d3106a..1a78502bb4b84 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -12,23 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ -#define DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ +#ifndef POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ +#define POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ #include +#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" + #include +#include +#include #include #include -#include -#include #include -#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" - +#include #include #include -#include struct PCDFileMetadata { @@ -38,11 +38,10 @@ struct PCDFileMetadata class DifferentialMapLoaderModule { - public: explicit DifferentialMapLoaderModule( rclcpp::Node * node, const std::vector pcd_paths); - + private: rclcpp::Logger logger_; @@ -53,9 +52,11 @@ class DifferentialMapLoaderModule bool loadPCDMapsGeneralCallback( autoware_map_msgs::srv::LoadPCDMapsGeneral::Request::SharedPtr req, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr res); - void partialAreaLoad(const autoware_map_msgs::msg::AreaInfo area, + void partialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const; - void differentialAreaLoad(const autoware_map_msgs::msg::AreaInfo area_info, + void differentialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area_info, const std::vector already_loaded_ids, autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const; void loadPCDMapWithID( @@ -65,4 +66,4 @@ class DifferentialMapLoaderModule const std::vector & pcd_paths) const; }; -#endif // DIFFERENTIAL_MAP_LOADING_MODULE_HPP_ \ No newline at end of file +#endif // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index 4764e2118c155..7c3c8c1430480 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -18,7 +18,8 @@ #include #include -sensor_msgs::msg::PointCloud2 downsample(const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) +sensor_msgs::msg::PointCloud2 downsample( + const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) { pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); @@ -34,16 +35,15 @@ sensor_msgs::msg::PointCloud2 downsample(const sensor_msgs::msg::PointCloud2 & m return msg_output; } - PointcloudMapLoaderModule::PointcloudMapLoaderModule( - rclcpp::Node * node, const std::vector & pcd_paths, - const std::string publisher_name, const boost::optional leaf_size) + rclcpp::Node * node, const std::vector & pcd_paths, const std::string publisher_name, + const boost::optional leaf_size) : logger_(node->get_logger()) { rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); - pub_pointcloud_map_ = node->create_publisher( - publisher_name, durable_qos); + pub_pointcloud_map_ = + node->create_publisher(publisher_name, durable_qos); sensor_msgs::msg::PointCloud2 pcd = loadPCDFiles(pcd_paths, leaf_size); if (pcd.width == 0) { @@ -65,8 +65,7 @@ sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( auto & path = pcd_paths[i]; if (i % 50 == 0) { RCLCPP_INFO_STREAM( - logger_, - "Load " << path << " (" << i + 1 << " out of " << int(pcd_paths.size()) << ")"); + logger_, "Load " << path << " (" << i + 1 << " out of " << int(pcd_paths.size()) << ")"); } if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index 619ab99acd1bb..92743f2384f47 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_MAP_LOADER_MODULE_MODULE_HPP_ -#define POINTCLOUD_MAP_LOADER_MODULE_MODULE_HPP_ +#ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_MODULE_HPP_ +#define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_MODULE_HPP_ #include #include + #include class PointcloudMapLoaderModule @@ -31,7 +32,8 @@ class PointcloudMapLoaderModule rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_pointcloud_map_; - sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector & pcd_paths, const boost::optional leaf_size) const; + sensor_msgs::msg::PointCloud2 loadPCDFiles( + const std::vector & pcd_paths, const boost::optional leaf_size) const; }; -#endif // POINTCLOUD_MAP_LOADER_MODULE_MODULE_HPP_ \ No newline at end of file +#endif // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index b0b9ce7151844..2907312e481c4 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -64,17 +64,19 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt { leaf_size_ = declare_parameter("leaf_size", 3.0); - const auto pcd_paths = getPcdPaths( - declare_parameter("pcd_paths_or_directory", std::vector({}))); + const auto pcd_paths = + getPcdPaths(declare_parameter("pcd_paths_or_directory", std::vector({}))); if (declare_parameter("enable_whole_load", false)) { std::string publisher_name = "output/pointcloud_map"; - pcd_map_loader_ = std::make_unique(this, pcd_paths, publisher_name, boost::none); + pcd_map_loader_ = + std::make_unique(this, pcd_paths, publisher_name, boost::none); } if (declare_parameter("enable_downsampled_whole_load", true)) { std::string publisher_name = "output/debug/downsampled_pointcloud_map"; - downsampled_pcd_map_loader_ = std::make_unique(this, pcd_paths, publisher_name, leaf_size_); + downsampled_pcd_map_loader_ = + std::make_unique(this, pcd_paths, publisher_name, leaf_size_); } if (declare_parameter("enable_partial_load", true)) { @@ -108,6 +110,5 @@ std::vector PointCloudMapLoaderNode::getPcdPaths( return pcd_paths; } - #include RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudMapLoaderNode) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index cea40f935dca4..4531a456a00bd 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -28,22 +28,21 @@ * limitations under the License. */ -#ifndef MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ -#define MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ // #include "autoware_map_msgs/srv/load_pcd_partially.hpp" // #include "autoware_map_msgs/srv/load_pcd_partially_for_publish.hpp" -#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" -#include "autoware_map_msgs/msg/area_info.hpp" -#include "autoware_map_msgs/msg/pcd_map_with_id.hpp" - #include "differential_map_loader_module.hpp" #include "pointcloud_map_loader_module.hpp" #include -#include +#include "autoware_map_msgs/msg/area_info.hpp" +#include "autoware_map_msgs/msg/pcd_map_with_id.hpp" +#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" #include +#include #include #include @@ -71,4 +70,4 @@ class PointCloudMapLoaderNode : public rclcpp::Node const std::vector & pcd_paths_or_directory) const; }; -#endif // MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#endif // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ From 4b97e86b97f4699efb8367fd0c4ef26f37dc4366 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 28 Sep 2022 09:21:43 +0900 Subject: [PATCH 06/48] move autoware_map_msgs to autoware_msgs repos Signed-off-by: kminoda --- map/autoware_map_msgs/CMakeLists.txt | 32 ------------------- map/autoware_map_msgs/msg/AreaInfo.msg | 3 -- map/autoware_map_msgs/msg/PCDMapWithID.msg | 2 -- map/autoware_map_msgs/package.xml | 23 ------------- .../srv/LoadPCDMapsGeneral.srv | 7 ---- 5 files changed, 67 deletions(-) delete mode 100755 map/autoware_map_msgs/CMakeLists.txt delete mode 100755 map/autoware_map_msgs/msg/AreaInfo.msg delete mode 100755 map/autoware_map_msgs/msg/PCDMapWithID.msg delete mode 100755 map/autoware_map_msgs/package.xml delete mode 100755 map/autoware_map_msgs/srv/LoadPCDMapsGeneral.srv diff --git a/map/autoware_map_msgs/CMakeLists.txt b/map/autoware_map_msgs/CMakeLists.txt deleted file mode 100755 index d4c2fd83ed70e..0000000000000 --- a/map/autoware_map_msgs/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(autoware_map_msgs) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(ament_cmake_auto REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED) -ament_auto_find_build_dependencies() - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/AreaInfo.msg" - "msg/PCDMapWithID.msg" - "srv/LoadPCDMapsGeneral.srv" - DEPENDENCIES - std_msgs - geometry_msgs - sensor_msgs -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() -ament_auto_package() diff --git a/map/autoware_map_msgs/msg/AreaInfo.msg b/map/autoware_map_msgs/msg/AreaInfo.msg deleted file mode 100755 index 9a1b9823f6862..0000000000000 --- a/map/autoware_map_msgs/msg/AreaInfo.msg +++ /dev/null @@ -1,3 +0,0 @@ -# this only supports spherical area message for now -geometry_msgs/Point center -float32 radius diff --git a/map/autoware_map_msgs/msg/PCDMapWithID.msg b/map/autoware_map_msgs/msg/PCDMapWithID.msg deleted file mode 100755 index 70908af46be19..0000000000000 --- a/map/autoware_map_msgs/msg/PCDMapWithID.msg +++ /dev/null @@ -1,2 +0,0 @@ -string id -sensor_msgs/PointCloud2 pcd diff --git a/map/autoware_map_msgs/package.xml b/map/autoware_map_msgs/package.xml deleted file mode 100755 index c8c5f0abc5fc3..0000000000000 --- a/map/autoware_map_msgs/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - autoware_map_msgs - 0.0.0 - The autoware_map_msgs package - RyuYamamoto - MIT - - ament_cmake_auto - geometry_msgs - rclcpp - sensor_msgs - std_msgs - tier4_map_msgs - - rosidl_default_runtime - - rosidl_interface_packages - - - ament_cmake - - diff --git a/map/autoware_map_msgs/srv/LoadPCDMapsGeneral.srv b/map/autoware_map_msgs/srv/LoadPCDMapsGeneral.srv deleted file mode 100755 index 8ad7a0aa10c0f..0000000000000 --- a/map/autoware_map_msgs/srv/LoadPCDMapsGeneral.srv +++ /dev/null @@ -1,7 +0,0 @@ -int32 mode # 0: Area load, 1: Differential area load -AreaInfo area -string[] already_loaded_ids # only used when mode = 1 - ---- -autoware_map_msgs/PCDMapWithID[] loaded_pcds -string[] already_loaded_ids # only used when mode = 1 From 83509d39b207dbee14230d88f9c997f5ed5755f0 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 28 Sep 2022 10:36:04 +0900 Subject: [PATCH 07/48] catch up with the modification in autoware_map_msgs Signed-off-by: kminoda --- .../differential_map_loader_module.cpp | 19 +++++++++---------- .../differential_map_loader_module.hpp | 17 +++++++++-------- .../pointcloud_map_loader_node.hpp | 5 ----- 3 files changed, 18 insertions(+), 23 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index cc3d7ca9aa2e4..92f441bf978d3 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -61,15 +61,15 @@ DifferentialMapLoaderModule::DifferentialMapLoaderModule( : logger_(node->get_logger()) { all_pcd_file_metadata_dict_ = generatePCDMetadata(pcd_paths); - load_pcd_maps_general_service_ = node->create_service( + load_pcd_maps_service_ = node->create_service( "load_pcd_maps_general", std::bind( - &DifferentialMapLoaderModule::loadPCDMapsGeneralCallback, this, + &DifferentialMapLoaderModule::onServiceLoadPCDMaps, this, std::placeholders::_1, std::placeholders::_2)); } void DifferentialMapLoaderModule::differentialAreaLoad( const autoware_map_msgs::msg::AreaInfo area, const std::vector already_loaded_ids, - autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const + LoadPCDMaps::Response::SharedPtr & response) const { // iterate over all the available pcd map grids for (const auto & ele : all_pcd_file_metadata_dict_) { @@ -98,7 +98,7 @@ void DifferentialMapLoaderModule::differentialAreaLoad( void DifferentialMapLoaderModule::partialAreaLoad( const autoware_map_msgs::msg::AreaInfo area, - autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const + LoadPCDMaps::Response::SharedPtr & response) const { // iterate over all the available pcd map grids @@ -118,15 +118,14 @@ void DifferentialMapLoaderModule::partialAreaLoad( } } -bool DifferentialMapLoaderModule::loadPCDMapsGeneralCallback( - autoware_map_msgs::srv::LoadPCDMapsGeneral::Request::SharedPtr req, - autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr res) +bool DifferentialMapLoaderModule::onServiceLoadPCDMaps( + LoadPCDMaps::Request::SharedPtr req, + LoadPCDMaps::Response::SharedPtr res) { - int mode = req->mode; - if (mode == 0) { + if (req->mode == LoadPCDMaps::Request::MODE_PARTIAL_AREA_LOAD) { auto area = req->area; partialAreaLoad(area, res); - } else if (mode == 1) { + } else if (req->mode == LoadPCDMaps::Request::MODE_DIFFERENTIAL_AREA_LOAD) { auto area = req->area; std::vector already_loaded_ids = req->already_loaded_ids; differentialAreaLoad(area, already_loaded_ids, res); diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index 1a78502bb4b84..431e6f11933bd 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -17,7 +17,7 @@ #include -#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" +#include "autoware_map_msgs/srv/load_pcd_maps.hpp" #include #include @@ -38,6 +38,8 @@ struct PCDFileMetadata class DifferentialMapLoaderModule { + using LoadPCDMaps = autoware_map_msgs::srv::LoadPCDMaps; + public: explicit DifferentialMapLoaderModule( rclcpp::Node * node, const std::vector pcd_paths); @@ -46,19 +48,18 @@ class DifferentialMapLoaderModule rclcpp::Logger logger_; std::map all_pcd_file_metadata_dict_; - rclcpp::Service::SharedPtr - load_pcd_maps_general_service_; + rclcpp::Service::SharedPtr load_pcd_maps_service_; - bool loadPCDMapsGeneralCallback( - autoware_map_msgs::srv::LoadPCDMapsGeneral::Request::SharedPtr req, - autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr res); + bool onServiceLoadPCDMaps( + LoadPCDMaps::Request::SharedPtr req, + LoadPCDMaps::Response::SharedPtr res); void partialAreaLoad( const autoware_map_msgs::msg::AreaInfo area, - autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const; + LoadPCDMaps::Response::SharedPtr & response) const; void differentialAreaLoad( const autoware_map_msgs::msg::AreaInfo area_info, const std::vector already_loaded_ids, - autoware_map_msgs::srv::LoadPCDMapsGeneral::Response::SharedPtr & response) const; + LoadPCDMaps::Response::SharedPtr & response) const; void loadPCDMapWithID( const std::string path, const std::string map_id, autoware_map_msgs::msg::PCDMapWithID & pcd_map_with_id) const; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index 4531a456a00bd..faa4769f54c3a 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -31,16 +31,11 @@ #ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ #define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ -// #include "autoware_map_msgs/srv/load_pcd_partially.hpp" -// #include "autoware_map_msgs/srv/load_pcd_partially_for_publish.hpp" #include "differential_map_loader_module.hpp" #include "pointcloud_map_loader_module.hpp" #include -#include "autoware_map_msgs/msg/area_info.hpp" -#include "autoware_map_msgs/msg/pcd_map_with_id.hpp" -#include "autoware_map_msgs/srv/load_pcd_maps_general.hpp" #include #include From 571a66633b923ca27e4dd5011509a7655e480a99 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 28 Sep 2022 01:37:33 +0000 Subject: [PATCH 08/48] ci(pre-commit): autofix --- .../differential_map_loader_module.cpp | 6 ++---- .../differential_map_loader_module.hpp | 6 ++---- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 92f441bf978d3..e5069695cf9fb 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -97,8 +97,7 @@ void DifferentialMapLoaderModule::differentialAreaLoad( } void DifferentialMapLoaderModule::partialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, - LoadPCDMaps::Response::SharedPtr & response) const + const autoware_map_msgs::msg::AreaInfo area, LoadPCDMaps::Response::SharedPtr & response) const { // iterate over all the available pcd map grids @@ -119,8 +118,7 @@ void DifferentialMapLoaderModule::partialAreaLoad( } bool DifferentialMapLoaderModule::onServiceLoadPCDMaps( - LoadPCDMaps::Request::SharedPtr req, - LoadPCDMaps::Response::SharedPtr res) + LoadPCDMaps::Request::SharedPtr req, LoadPCDMaps::Response::SharedPtr res) { if (req->mode == LoadPCDMaps::Request::MODE_PARTIAL_AREA_LOAD) { auto area = req->area; diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index 431e6f11933bd..a9a30290352ec 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -51,11 +51,9 @@ class DifferentialMapLoaderModule rclcpp::Service::SharedPtr load_pcd_maps_service_; bool onServiceLoadPCDMaps( - LoadPCDMaps::Request::SharedPtr req, - LoadPCDMaps::Response::SharedPtr res); + LoadPCDMaps::Request::SharedPtr req, LoadPCDMaps::Response::SharedPtr res); void partialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, - LoadPCDMaps::Response::SharedPtr & response) const; + const autoware_map_msgs::msg::AreaInfo area, LoadPCDMaps::Response::SharedPtr & response) const; void differentialAreaLoad( const autoware_map_msgs::msg::AreaInfo area_info, const std::vector already_loaded_ids, From b05e2c5a6d180d4e60b0600a4352dfe31640cf06 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 3 Oct 2022 12:14:41 +0900 Subject: [PATCH 09/48] aligned with autoware_map_msgs change (differential/partial modules seperation) Signed-off-by: kminoda --- .../config/pointcloud_map_loader.param.yaml | 5 +- launch/tier4_map_launch/launch/map.launch.py | 4 +- launch/tier4_map_launch/launch/map.launch.xml | 7 +- map/map_loader/CMakeLists.txt | 3 +- .../config/pointcloud_map_loader.param.yaml | 3 +- .../launch/pointcloud_map_loader.launch.xml | 5 + .../differential_map_loader_module.cpp | 136 ++++-------------- .../differential_map_loader_module.hpp | 30 ++-- .../partial_map_loader_module.cpp | 67 +++++++++ .../partial_map_loader_module.hpp | 59 ++++++++ .../pointcloud_map_loader_module.cpp | 11 +- .../pointcloud_map_loader_module.hpp | 2 +- .../pointcloud_map_loader_node.cpp | 46 ++++-- .../pointcloud_map_loader_node.hpp | 8 +- .../src/pointcloud_map_loader/utils.cpp | 61 ++++++++ .../src/pointcloud_map_loader/utils.hpp | 36 +++++ 16 files changed, 334 insertions(+), 149 deletions(-) create mode 100644 map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp create mode 100644 map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp create mode 100644 map/map_loader/src/pointcloud_map_loader/utils.cpp create mode 100644 map/map_loader/src/pointcloud_map_loader/utils.hpp diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml index fa94afbf12a0b..a5632ffb903c3 100644 --- a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml +++ b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml @@ -1,8 +1,9 @@ /**: ros__parameters: enable_whole_load: true - enable_downsampled_whole_load: false - enable_partial_load: false + enable_downsampled_whole_load: true + enable_partial_load: true + enable_differential_load: true # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index 3f73195ec98c7..d4043c90c7edd 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -84,7 +84,9 @@ def launch_setup(context, *args, **kwargs): name="pointcloud_map_loader", remappings=[ ("output/pointcloud_map", "pointcloud_map"), - ("output/debug/downsampled_pointcloud_map", "debug/downsampled_pointcloud_map"), + ("output/debug/downsampled_pointcloud_map", "pointcloud_map_downsampled"), + ("service/load_partial_pcd_map", "/map/load_partial_pointcloud_map"), + ("service/load_differential_pcd_map", "/map/load_differential_pointcloud_map"), ], parameters=[ {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]}, diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index f0654a580fe95..7b1bedc9058ec 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -9,10 +9,9 @@ - - - - + + + diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 31b0884a5eb86..9a9931905d889 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -8,8 +8,9 @@ find_package(PCL REQUIRED COMPONENTS io) ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/pointcloud_map_loader_node.cpp - src/pointcloud_map_loader/differential_map_loader_module.cpp src/pointcloud_map_loader/pointcloud_map_loader_module.cpp + src/pointcloud_map_loader/partial_map_loader_module.cpp + src/pointcloud_map_loader/differential_map_loader_module.cpp ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index bace033ee79ec..a5632ffb903c3 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -1,8 +1,9 @@ /**: ros__parameters: - enable_whole_load: false + enable_whole_load: true enable_downsampled_whole_load: true enable_partial_load: true + enable_differential_load: true # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml index 43676570277b6..d3c800fc16a4f 100644 --- a/map/map_loader/launch/pointcloud_map_loader.launch.xml +++ b/map/map_loader/launch/pointcloud_map_loader.launch.xml @@ -1,8 +1,13 @@ + + + + + diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index e5069695cf9fb..5c790349372b3 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -14,64 +14,23 @@ #include "differential_map_loader_module.hpp" -bool sphereAndBoxOverlapExists( - const geometry_msgs::msg::Point position, const double radius, const pcl::PointXYZ position_min, - const pcl::PointXYZ position_max) -{ - if ( - (position_min.x - radius <= position.x && position.x <= position_max.x + radius && - position_min.y <= position.y && position.y <= position_max.y && position_min.z <= position.z && - position.z <= position_max.z) || - (position_min.x <= position.x && position.x <= position_max.x && - position_min.y - radius <= position.y && position.y <= position_max.y + radius && - position_min.z <= position.z && position.z <= position_max.z) || - (position_min.x <= position.x && position.x <= position_max.x && position_min.y <= position.y && - position.y <= position_max.y && position_min.z - radius <= position.z && - position.z <= position_max.z + radius)) { - return true; - } - double r2 = std::pow(radius, 2.0); - double minx2 = std::pow(position.x - position_min.x, 2.0); - double maxx2 = std::pow(position.x - position_max.x, 2.0); - double miny2 = std::pow(position.y - position_min.y, 2.0); - double maxy2 = std::pow(position.y - position_max.y, 2.0); - double minz2 = std::pow(position.z - position_min.z, 2.0); - double maxz2 = std::pow(position.z - position_max.z, 2.0); - if ( - minx2 + miny2 + minz2 <= r2 || minx2 + miny2 + maxz2 <= r2 || minx2 + maxy2 + minz2 <= r2 || - minx2 + maxy2 + maxz2 <= r2 || maxx2 + miny2 + minz2 <= r2 || maxx2 + miny2 + maxz2 <= r2 || - maxx2 + maxy2 + minz2 <= r2 || maxx2 + maxy2 + maxz2 <= r2) { - return true; - } - return false; -} - -bool isGridWithinQueriedArea( - const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata) -{ - // Currently, the area load only supports spherical area - geometry_msgs::msg::Point position = area.center; - double radius = area.radius; - bool res = sphereAndBoxOverlapExists(position, radius, metadata.min, metadata.max); - return res; -} - DifferentialMapLoaderModule::DifferentialMapLoaderModule( - rclcpp::Node * node, const std::vector pcd_paths) -: logger_(node->get_logger()) + rclcpp::Node * node, const std::map & pcd_file_metadata_dict) +: logger_(node->get_logger()), + all_pcd_file_metadata_dict_(pcd_file_metadata_dict) { - all_pcd_file_metadata_dict_ = generatePCDMetadata(pcd_paths); - load_pcd_maps_service_ = node->create_service( - "load_pcd_maps_general", std::bind( - &DifferentialMapLoaderModule::onServiceLoadPCDMaps, this, + load_differential_pcd_maps_service_ = node->create_service( + "service/load_differential_pcd_map", std::bind( + &DifferentialMapLoaderModule::onServiceLoadDifferentialPointCloudMap, this, std::placeholders::_1, std::placeholders::_2)); } void DifferentialMapLoaderModule::differentialAreaLoad( const autoware_map_msgs::msg::AreaInfo area, const std::vector already_loaded_ids, - LoadPCDMaps::Response::SharedPtr & response) const + LoadDifferentialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids + std::vector should_remove(int(already_loaded_ids.size()), true); for (const auto & ele : all_pcd_file_metadata_dict_) { std::string path = ele.first; PCDFileMetadata metadata = ele.second; @@ -82,79 +41,44 @@ void DifferentialMapLoaderModule::differentialAreaLoad( // skip if the pcd file is not within the queried area if (!isGridWithinQueriedArea(area, metadata)) continue; - bool is_already_loaded = - std::find(already_loaded_ids.begin(), already_loaded_ids.end(), map_id) != - already_loaded_ids.end(); - if (is_already_loaded) { - response->already_loaded_ids.push_back(map_id); + auto id_in_already_loaded_list = + std::find(already_loaded_ids.begin(), already_loaded_ids.end(), map_id); + if (id_in_already_loaded_list != already_loaded_ids.end()) { + int index = id_in_already_loaded_list - already_loaded_ids.begin(); + should_remove[index] = false; } else { - autoware_map_msgs::msg::PCDMapWithID pcd_map_with_id; - loadPCDMapWithID(path, map_id, pcd_map_with_id); + autoware_map_msgs::msg::PointCloudMapWithID pcd_map_with_id; + loadPointCloudMapWithID(path, map_id, pcd_map_with_id); response->loaded_pcds.push_back(pcd_map_with_id); } } - RCLCPP_INFO_STREAM(logger_, "Finished diff area loading"); -} - -void DifferentialMapLoaderModule::partialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, LoadPCDMaps::Response::SharedPtr & response) const -{ - // iterate over all the available pcd map grids - - for (const auto & ele : all_pcd_file_metadata_dict_) { - std::string path = ele.first; - PCDFileMetadata metadata = ele.second; - - // assume that the map ID = map path (for now) - std::string map_id = path; - - // skip if the pcd file is not within the queried area - if (!isGridWithinQueriedArea(area, metadata)) continue; - - autoware_map_msgs::msg::PCDMapWithID pcd_map_with_id; - loadPCDMapWithID(path, map_id, pcd_map_with_id); - response->loaded_pcds.push_back(pcd_map_with_id); + + for (int i = 0; i < int(already_loaded_ids.size()); ++i) { + if (should_remove[i]) { + response->ids_to_remove.push_back(already_loaded_ids[i]); + } } + + RCLCPP_INFO_STREAM(logger_, "Finished diff area loading"); } -bool DifferentialMapLoaderModule::onServiceLoadPCDMaps( - LoadPCDMaps::Request::SharedPtr req, LoadPCDMaps::Response::SharedPtr res) +bool DifferentialMapLoaderModule::onServiceLoadDifferentialPointCloudMap( + LoadDifferentialPointCloudMap::Request::SharedPtr req, LoadDifferentialPointCloudMap::Response::SharedPtr res) { - if (req->mode == LoadPCDMaps::Request::MODE_PARTIAL_AREA_LOAD) { - auto area = req->area; - partialAreaLoad(area, res); - } else if (req->mode == LoadPCDMaps::Request::MODE_DIFFERENTIAL_AREA_LOAD) { - auto area = req->area; - std::vector already_loaded_ids = req->already_loaded_ids; - differentialAreaLoad(area, already_loaded_ids, res); - } + auto area = req->area; + std::vector already_loaded_ids = req->already_loaded_ids; + differentialAreaLoad(area, already_loaded_ids, res); return true; } -void DifferentialMapLoaderModule::loadPCDMapWithID( +void DifferentialMapLoaderModule::loadPointCloudMapWithID( const std::string path, const std::string map_id, - autoware_map_msgs::msg::PCDMapWithID & pcd_map_with_id) const + autoware_map_msgs::msg::PointCloudMapWithID & pcd_map_with_id) const { sensor_msgs::msg::PointCloud2 pcd; if (pcl::io::loadPCDFile(path, pcd) == -1) { RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); } - pcd_map_with_id.pcd = pcd; + pcd_map_with_id.pointcloud = pcd; pcd_map_with_id.id = map_id; } - -std::map DifferentialMapLoaderModule::generatePCDMetadata( - const std::vector & pcd_paths) const -{ - pcl::PointCloud partial_pcd; - std::map all_pcd_file_metadata_dict; - for (const auto & path : pcd_paths) { - if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { - RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); - } - PCDFileMetadata metadata = {}; - pcl::getMinMax3D(partial_pcd, metadata.min, metadata.max); - all_pcd_file_metadata_dict[path] = metadata; - } - return all_pcd_file_metadata_dict; -} diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index a9a30290352ec..443a00d3d03db 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -15,9 +15,11 @@ #ifndef POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ #define POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ +#include "utils.hpp" + #include -#include "autoware_map_msgs/srv/load_pcd_maps.hpp" +#include "autoware_map_msgs/srv/load_differential_point_cloud_map.hpp" #include #include @@ -30,39 +32,29 @@ #include #include -struct PCDFileMetadata -{ - pcl::PointXYZ min; - pcl::PointXYZ max; -}; - class DifferentialMapLoaderModule { - using LoadPCDMaps = autoware_map_msgs::srv::LoadPCDMaps; + using LoadDifferentialPointCloudMap = autoware_map_msgs::srv::LoadDifferentialPointCloudMap; public: explicit DifferentialMapLoaderModule( - rclcpp::Node * node, const std::vector pcd_paths); + rclcpp::Node * node, const std::map & pcd_file_metadata_dict); private: rclcpp::Logger logger_; std::map all_pcd_file_metadata_dict_; - rclcpp::Service::SharedPtr load_pcd_maps_service_; + rclcpp::Service::SharedPtr load_differential_pcd_maps_service_; - bool onServiceLoadPCDMaps( - LoadPCDMaps::Request::SharedPtr req, LoadPCDMaps::Response::SharedPtr res); - void partialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, LoadPCDMaps::Response::SharedPtr & response) const; + bool onServiceLoadDifferentialPointCloudMap( + LoadDifferentialPointCloudMap::Request::SharedPtr req, LoadDifferentialPointCloudMap::Response::SharedPtr res); void differentialAreaLoad( const autoware_map_msgs::msg::AreaInfo area_info, const std::vector already_loaded_ids, - LoadPCDMaps::Response::SharedPtr & response) const; - void loadPCDMapWithID( + LoadDifferentialPointCloudMap::Response::SharedPtr & response) const; + void loadPointCloudMapWithID( const std::string path, const std::string map_id, - autoware_map_msgs::msg::PCDMapWithID & pcd_map_with_id) const; - std::map generatePCDMetadata( - const std::vector & pcd_paths) const; + autoware_map_msgs::msg::PointCloudMapWithID & pcd_map_with_id) const; }; #endif // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp new file mode 100644 index 0000000000000..93904e546ac05 --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -0,0 +1,67 @@ +// Copyright 2022 The Autoware Contributors +// +// 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. + +#include "partial_map_loader_module.hpp" + +PartialMapLoaderModule::PartialMapLoaderModule( + rclcpp::Node * node, const std::map & pcd_file_metadata_dict) +: logger_(node->get_logger()), + all_pcd_file_metadata_dict_(pcd_file_metadata_dict) +{ + load_partial_pcd_maps_service_ = node->create_service( + "service/load_partial_pcd_map", std::bind( + &PartialMapLoaderModule::onServiceLoadPartialPointCloudMap, this, + std::placeholders::_1, std::placeholders::_2)); +} + +void PartialMapLoaderModule::partialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area, LoadPartialPointCloudMap::Response::SharedPtr & response) const +{ + // iterate over all the available pcd map grids + + for (const auto & ele : all_pcd_file_metadata_dict_) { + std::string path = ele.first; + PCDFileMetadata metadata = ele.second; + + // assume that the map ID = map path (for now) + std::string map_id = path; + + // skip if the pcd file is not within the queried area + if (!isGridWithinQueriedArea(area, metadata)) continue; + + autoware_map_msgs::msg::PointCloudMapWithID pcd_map_with_id; + loadPointCloudMapWithID(path, map_id, pcd_map_with_id); + response->loaded_pcds.push_back(pcd_map_with_id); + } +} + +bool PartialMapLoaderModule::onServiceLoadPartialPointCloudMap( + LoadPartialPointCloudMap::Request::SharedPtr req, LoadPartialPointCloudMap::Response::SharedPtr res) +{ + auto area = req->area; + partialAreaLoad(area, res); + return true; +} + +void PartialMapLoaderModule::loadPointCloudMapWithID( + const std::string path, const std::string map_id, + autoware_map_msgs::msg::PointCloudMapWithID & pcd_map_with_id) const +{ + sensor_msgs::msg::PointCloud2 pcd; + if (pcl::io::loadPCDFile(path, pcd) == -1) { + RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); + } + pcd_map_with_id.pointcloud = pcd; + pcd_map_with_id.id = map_id; +} diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp new file mode 100644 index 0000000000000..a7af61c654ba7 --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp @@ -0,0 +1,59 @@ +// Copyright 2022 The Autoware Contributors +// +// 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. + +#ifndef POINTCLOUD_MAP_LOADER__PARTIAL_MAP_LOADER_MODULE_HPP_ +#define POINTCLOUD_MAP_LOADER__PARTIAL_MAP_LOADER_MODULE_HPP_ + +#include "utils.hpp" + +#include + +#include "autoware_map_msgs/srv/load_partial_point_cloud_map.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +class PartialMapLoaderModule +{ + using LoadPartialPointCloudMap = autoware_map_msgs::srv::LoadPartialPointCloudMap; + +public: + explicit PartialMapLoaderModule( + rclcpp::Node * node, const std::map & pcd_file_metadata_dict); + +private: + rclcpp::Logger logger_; + + std::map all_pcd_file_metadata_dict_; + rclcpp::Service::SharedPtr load_partial_pcd_maps_service_; + + bool onServiceLoadPartialPointCloudMap( + LoadPartialPointCloudMap::Request::SharedPtr req, LoadPartialPointCloudMap::Response::SharedPtr res); + void partialAreaLoad( + const autoware_map_msgs::msg::AreaInfo area, LoadPartialPointCloudMap::Response::SharedPtr & response) const; + void loadPointCloudMapWithID( + const std::string path, const std::string map_id, + autoware_map_msgs::msg::PointCloudMapWithID & pcd_map_with_id) const; +}; + +#endif // POINTCLOUD_MAP_LOADER__PARTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index 7c3c8c1430480..0fc1d4c6fd3dd 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -37,14 +37,21 @@ sensor_msgs::msg::PointCloud2 downsample( PointcloudMapLoaderModule::PointcloudMapLoaderModule( rclcpp::Node * node, const std::vector & pcd_paths, const std::string publisher_name, - const boost::optional leaf_size) + const bool use_downsample) : logger_(node->get_logger()) { rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); pub_pointcloud_map_ = node->create_publisher(publisher_name, durable_qos); - sensor_msgs::msg::PointCloud2 pcd = loadPCDFiles(pcd_paths, leaf_size); + + sensor_msgs::msg::PointCloud2 pcd; + if (use_downsample) { + float leaf_size = node->declare_parameter("leaf_size"); + pcd = loadPCDFiles(pcd_paths, leaf_size); + } else { + pcd = loadPCDFiles(pcd_paths, boost::none); + } if (pcd.width == 0) { RCLCPP_ERROR(logger_, "No PCD was loaded: pcd_paths.size() = %zu", pcd_paths.size()); diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index 92743f2384f47..42b463ee1411e 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -26,7 +26,7 @@ class PointcloudMapLoaderModule public: explicit PointcloudMapLoaderModule( rclcpp::Node * node, const std::vector & pcd_paths, - const std::string publisher_name, const boost::optional leaf_size); + const std::string publisher_name, const bool use_downsample); private: rclcpp::Logger logger_; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 2907312e481c4..7bb86b1a1ceb2 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -62,25 +62,35 @@ bool isPcdFile(const std::string & p) PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & options) : Node("pointcloud_map_loader", options) { - leaf_size_ = declare_parameter("leaf_size", 3.0); - const auto pcd_paths = - getPcdPaths(declare_parameter("pcd_paths_or_directory", std::vector({}))); + getPcdPaths(declare_parameter>("pcd_paths_or_directory")); + bool enable_whole_load = declare_parameter("enable_whole_load"); + bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); + bool enable_partial_load = declare_parameter("enable_partial_load"); + bool enable_differential_load = declare_parameter("enable_differential_load"); - if (declare_parameter("enable_whole_load", false)) { + if (enable_whole_load) { std::string publisher_name = "output/pointcloud_map"; pcd_map_loader_ = - std::make_unique(this, pcd_paths, publisher_name, boost::none); + std::make_unique(this, pcd_paths, publisher_name, false); } - if (declare_parameter("enable_downsampled_whole_load", true)) { + if (enable_downsample_whole_load) { std::string publisher_name = "output/debug/downsampled_pointcloud_map"; downsampled_pcd_map_loader_ = - std::make_unique(this, pcd_paths, publisher_name, leaf_size_); + std::make_unique(this, pcd_paths, publisher_name, true); + } + + if (enable_partial_load | enable_differential_load) { + pcd_metadata_dict_ = generatePCDMetadata(pcd_paths); } - if (declare_parameter("enable_partial_load", true)) { - differential_map_loader_ = std::make_unique(this, pcd_paths); + if (enable_partial_load) { + partial_map_loader_ = std::make_unique(this, pcd_metadata_dict_); + } + + if (enable_differential_load) { + differential_map_loader_ = std::make_unique(this, pcd_metadata_dict_); } } @@ -89,7 +99,6 @@ std::vector PointCloudMapLoaderNode::getPcdPaths( { std::vector pcd_paths; for (const auto & p : pcd_paths_or_directory) { - (void)p; if (!fs::exists(p)) { RCLCPP_ERROR_STREAM(get_logger(), "invalid path: " << p); } @@ -110,5 +119,22 @@ std::vector PointCloudMapLoaderNode::getPcdPaths( return pcd_paths; } +std::map PointCloudMapLoaderNode::generatePCDMetadata( + const std::vector & pcd_paths) const +{ + pcl::PointCloud partial_pcd; + std::map all_pcd_file_metadata_dict; + for (const auto & path : pcd_paths) { + if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { + RCLCPP_ERROR_STREAM(get_logger(), "PCD load failed: " << path); + } + PCDFileMetadata metadata = {}; + pcl::getMinMax3D(partial_pcd, metadata.min, metadata.max); + all_pcd_file_metadata_dict[path] = metadata; + } + return all_pcd_file_metadata_dict; +} + + #include RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudMapLoaderNode) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index faa4769f54c3a..dbd0c73811613 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -32,6 +32,7 @@ #define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ #include "differential_map_loader_module.hpp" +#include "partial_map_loader_module.hpp" #include "pointcloud_map_loader_module.hpp" #include @@ -55,14 +56,17 @@ class PointCloudMapLoaderNode : public rclcpp::Node private: // ros param - float leaf_size_; + std::map pcd_metadata_dict_; - std::unique_ptr differential_map_loader_; std::unique_ptr pcd_map_loader_; std::unique_ptr downsampled_pcd_map_loader_; + std::unique_ptr partial_map_loader_; + std::unique_ptr differential_map_loader_; std::vector getPcdPaths( const std::vector & pcd_paths_or_directory) const; + std::map generatePCDMetadata( + const std::vector & pcd_paths) const; }; #endif // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp new file mode 100644 index 0000000000000..2145c1564b8e2 --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -0,0 +1,61 @@ +// Copyright 2022 The Autoware Contributors +// +// 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. + +#include + +#include + +bool sphereAndBoxOverlapExists( + const geometry_msgs::msg::Point position, const double radius, const pcl::PointXYZ position_min, + const pcl::PointXYZ position_max) +{ + if ( + (position_min.x - radius <= position.x && position.x <= position_max.x + radius && + position_min.y <= position.y && position.y <= position_max.y && position_min.z <= position.z && + position.z <= position_max.z) || + (position_min.x <= position.x && position.x <= position_max.x && + position_min.y - radius <= position.y && position.y <= position_max.y + radius && + position_min.z <= position.z && position.z <= position_max.z) || + (position_min.x <= position.x && position.x <= position_max.x && position_min.y <= position.y && + position.y <= position_max.y && position_min.z - radius <= position.z && + position.z <= position_max.z + radius)) { + return true; + } + double r2 = std::pow(radius, 2.0); + double minx2 = std::pow(position.x - position_min.x, 2.0); + double maxx2 = std::pow(position.x - position_max.x, 2.0); + double miny2 = std::pow(position.y - position_min.y, 2.0); + double maxy2 = std::pow(position.y - position_max.y, 2.0); + double minz2 = std::pow(position.z - position_min.z, 2.0); + double maxz2 = std::pow(position.z - position_max.z, 2.0); + if ( + minx2 + miny2 + minz2 <= r2 || minx2 + miny2 + maxz2 <= r2 || minx2 + maxy2 + minz2 <= r2 || + minx2 + maxy2 + maxz2 <= r2 || maxx2 + miny2 + minz2 <= r2 || maxx2 + miny2 + maxz2 <= r2 || + maxx2 + maxy2 + minz2 <= r2 || maxx2 + maxy2 + maxz2 <= r2) { + return true; + } + return false; +} + +bool isGridWithinQueriedArea( + const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata) +{ + // Currently, the area load only supports spherical area + geometry_msgs::msg::Point position = area.center; + double radius = area.radius; + bool res = sphereAndBoxOverlapExists(position, radius, metadata.min, metadata.max); + return res; +} + +#endif // POINTCLOUD_MAP_LOADER__UTILS_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp new file mode 100644 index 0000000000000..2733894efabf8 --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -0,0 +1,36 @@ +// Copyright 2022 The Autoware Contributors +// +// 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. + +#ifndef POINTCLOUD_MAP_LOADER__UTILS_HPP_ +#define POINTCLOUD_MAP_LOADER__UTILS_HPP_ + +#include +#include +#include + +#include + +struct PCDFileMetadata +{ + pcl::PointXYZ min; + pcl::PointXYZ max; +}; + +bool sphereAndBoxOverlapExists( + const geometry_msgs::msg::Point position, const double radius, const pcl::PointXYZ position_min, + const pcl::PointXYZ position_max); +bool isGridWithinQueriedArea( + const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata); + +#endif // POINTCLOUD_MAP_LOADER__UTILS_HPP_ From 52fb97c7be75758d5d3d6ebbfad21de195232b48 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 3 Oct 2022 03:16:15 +0000 Subject: [PATCH 10/48] ci(pre-commit): autofix --- .../differential_map_loader_module.cpp | 15 ++++++++------- .../differential_map_loader_module.hpp | 3 ++- .../partial_map_loader_module.cpp | 13 +++++++------ .../partial_map_loader_module.hpp | 7 ++++--- .../pointcloud_map_loader_node.cpp | 4 ++-- .../src/pointcloud_map_loader/utils.hpp | 2 +- 6 files changed, 24 insertions(+), 20 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 5c790349372b3..727d5bfecbd38 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -16,13 +16,13 @@ DifferentialMapLoaderModule::DifferentialMapLoaderModule( rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), - all_pcd_file_metadata_dict_(pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) { load_differential_pcd_maps_service_ = node->create_service( - "service/load_differential_pcd_map", std::bind( - &DifferentialMapLoaderModule::onServiceLoadDifferentialPointCloudMap, this, - std::placeholders::_1, std::placeholders::_2)); + "service/load_differential_pcd_map", + std::bind( + &DifferentialMapLoaderModule::onServiceLoadDifferentialPointCloudMap, this, + std::placeholders::_1, std::placeholders::_2)); } void DifferentialMapLoaderModule::differentialAreaLoad( @@ -52,7 +52,7 @@ void DifferentialMapLoaderModule::differentialAreaLoad( response->loaded_pcds.push_back(pcd_map_with_id); } } - + for (int i = 0; i < int(already_loaded_ids.size()); ++i) { if (should_remove[i]) { response->ids_to_remove.push_back(already_loaded_ids[i]); @@ -63,7 +63,8 @@ void DifferentialMapLoaderModule::differentialAreaLoad( } bool DifferentialMapLoaderModule::onServiceLoadDifferentialPointCloudMap( - LoadDifferentialPointCloudMap::Request::SharedPtr req, LoadDifferentialPointCloudMap::Response::SharedPtr res) + LoadDifferentialPointCloudMap::Request::SharedPtr req, + LoadDifferentialPointCloudMap::Response::SharedPtr res) { auto area = req->area; std::vector already_loaded_ids = req->already_loaded_ids; diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index 443a00d3d03db..23f25b699df49 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -47,7 +47,8 @@ class DifferentialMapLoaderModule rclcpp::Service::SharedPtr load_differential_pcd_maps_service_; bool onServiceLoadDifferentialPointCloudMap( - LoadDifferentialPointCloudMap::Request::SharedPtr req, LoadDifferentialPointCloudMap::Response::SharedPtr res); + LoadDifferentialPointCloudMap::Request::SharedPtr req, + LoadDifferentialPointCloudMap::Response::SharedPtr res); void differentialAreaLoad( const autoware_map_msgs::msg::AreaInfo area_info, const std::vector already_loaded_ids, diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index 93904e546ac05..a69511e20d48c 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -16,17 +16,17 @@ PartialMapLoaderModule::PartialMapLoaderModule( rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), - all_pcd_file_metadata_dict_(pcd_file_metadata_dict) +: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) { load_partial_pcd_maps_service_ = node->create_service( "service/load_partial_pcd_map", std::bind( - &PartialMapLoaderModule::onServiceLoadPartialPointCloudMap, this, - std::placeholders::_1, std::placeholders::_2)); + &PartialMapLoaderModule::onServiceLoadPartialPointCloudMap, + this, std::placeholders::_1, std::placeholders::_2)); } void PartialMapLoaderModule::partialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, LoadPartialPointCloudMap::Response::SharedPtr & response) const + const autoware_map_msgs::msg::AreaInfo area, + LoadPartialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids @@ -47,7 +47,8 @@ void PartialMapLoaderModule::partialAreaLoad( } bool PartialMapLoaderModule::onServiceLoadPartialPointCloudMap( - LoadPartialPointCloudMap::Request::SharedPtr req, LoadPartialPointCloudMap::Response::SharedPtr res) + LoadPartialPointCloudMap::Request::SharedPtr req, + LoadPartialPointCloudMap::Response::SharedPtr res) { auto area = req->area; partialAreaLoad(area, res); diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp index a7af61c654ba7..a0b579a6fd8e1 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp @@ -32,7 +32,6 @@ #include #include - class PartialMapLoaderModule { using LoadPartialPointCloudMap = autoware_map_msgs::srv::LoadPartialPointCloudMap; @@ -48,9 +47,11 @@ class PartialMapLoaderModule rclcpp::Service::SharedPtr load_partial_pcd_maps_service_; bool onServiceLoadPartialPointCloudMap( - LoadPartialPointCloudMap::Request::SharedPtr req, LoadPartialPointCloudMap::Response::SharedPtr res); + LoadPartialPointCloudMap::Request::SharedPtr req, + LoadPartialPointCloudMap::Response::SharedPtr res); void partialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, LoadPartialPointCloudMap::Response::SharedPtr & response) const; + const autoware_map_msgs::msg::AreaInfo area, + LoadPartialPointCloudMap::Response::SharedPtr & response) const; void loadPointCloudMapWithID( const std::string path, const std::string map_id, autoware_map_msgs::msg::PointCloudMapWithID & pcd_map_with_id) const; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 7bb86b1a1ceb2..657cec9709d44 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -90,7 +90,8 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt } if (enable_differential_load) { - differential_map_loader_ = std::make_unique(this, pcd_metadata_dict_); + differential_map_loader_ = + std::make_unique(this, pcd_metadata_dict_); } } @@ -135,6 +136,5 @@ std::map PointCloudMapLoaderNode::generatePCDMetad return all_pcd_file_metadata_dict; } - #include RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudMapLoaderNode) diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp index 2733894efabf8..62e3d900b7cb3 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -15,9 +15,9 @@ #ifndef POINTCLOUD_MAP_LOADER__UTILS_HPP_ #define POINTCLOUD_MAP_LOADER__UTILS_HPP_ -#include #include #include +#include #include From 25fb036a29a18fde8731191b4dbdd9c566e11b83 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 3 Oct 2022 13:59:11 +0900 Subject: [PATCH 11/48] debugged --- map/map_loader/CMakeLists.txt | 1 + map/map_loader/src/pointcloud_map_loader/utils.cpp | 4 +--- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 9a9931905d889..98cf7a4751a1c 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -11,6 +11,7 @@ ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/pointcloud_map_loader_module.cpp src/pointcloud_map_loader/partial_map_loader_module.cpp src/pointcloud_map_loader/differential_map_loader_module.cpp + src/pointcloud_map_loader/utils.cpp ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index 2145c1564b8e2..8f65b0459b8e7 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "utils.hpp" #include @@ -57,5 +57,3 @@ bool isGridWithinQueriedArea( bool res = sphereAndBoxOverlapExists(position, radius, metadata.min, metadata.max); return res; } - -#endif // POINTCLOUD_MAP_LOADER__UTILS_HPP_ From b943ded3da110f715000f0a21f81a738d17701d6 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 3 Oct 2022 15:56:49 +0900 Subject: [PATCH 12/48] debugged Signed-off-by: kminoda --- map/map_loader/CMakeLists.txt | 1 + .../src/pointcloud_map_loader/pointcloud_map_loader_node.hpp | 3 --- map/map_loader/src/pointcloud_map_loader/utils.cpp | 4 +--- 3 files changed, 2 insertions(+), 6 deletions(-) diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 9a9931905d889..98cf7a4751a1c 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -11,6 +11,7 @@ ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/pointcloud_map_loader_module.cpp src/pointcloud_map_loader/partial_map_loader_module.cpp src/pointcloud_map_loader/differential_map_loader_module.cpp + src/pointcloud_map_loader/utils.cpp ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index dbd0c73811613..12a1b953c8b91 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -37,9 +37,6 @@ #include -#include -#include - #include #include #include diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index 2145c1564b8e2..8f65b0459b8e7 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "utils.hpp" #include @@ -57,5 +57,3 @@ bool isGridWithinQueriedArea( bool res = sphereAndBoxOverlapExists(position, radius, metadata.min, metadata.max); return res; } - -#endif // POINTCLOUD_MAP_LOADER__UTILS_HPP_ From 26bb8052730260866588c458910a64d68f21c440 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 11 Oct 2022 16:51:00 +0900 Subject: [PATCH 13/48] added min-max info and others Signed-off-by: kminoda --- .../differential_map_loader_module.cpp | 52 +++++++++++-------- .../differential_map_loader_module.hpp | 20 +++---- .../partial_map_loader_module.cpp | 34 +++++++----- .../partial_map_loader_module.hpp | 18 +++---- .../src/pointcloud_map_loader/utils.hpp | 1 - 5 files changed, 70 insertions(+), 55 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 727d5bfecbd38..9f51a32b80298 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -18,19 +18,19 @@ DifferentialMapLoaderModule::DifferentialMapLoaderModule( rclcpp::Node * node, const std::map & pcd_file_metadata_dict) : logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) { - load_differential_pcd_maps_service_ = node->create_service( + load_differential_pcd_maps_service_ = node->create_service( "service/load_differential_pcd_map", std::bind( - &DifferentialMapLoaderModule::onServiceLoadDifferentialPointCloudMap, this, + &DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap, this, std::placeholders::_1, std::placeholders::_2)); } void DifferentialMapLoaderModule::differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, const std::vector already_loaded_ids, - LoadDifferentialPointCloudMap::Response::SharedPtr & response) const + const autoware_map_msgs::msg::AreaInfo area, const std::vector cached_ids, + GetDifferentialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids - std::vector should_remove(int(already_loaded_ids.size()), true); + std::vector should_remove(int(cached_ids.size()), true); for (const auto & ele : all_pcd_file_metadata_dict_) { std::string path = ele.first; PCDFileMetadata metadata = ele.second; @@ -42,44 +42,52 @@ void DifferentialMapLoaderModule::differentialAreaLoad( if (!isGridWithinQueriedArea(area, metadata)) continue; auto id_in_already_loaded_list = - std::find(already_loaded_ids.begin(), already_loaded_ids.end(), map_id); - if (id_in_already_loaded_list != already_loaded_ids.end()) { - int index = id_in_already_loaded_list - already_loaded_ids.begin(); + std::find(cached_ids.begin(), cached_ids.end(), map_id); + if (id_in_already_loaded_list != cached_ids.end()) { + int index = id_in_already_loaded_list - cached_ids.begin(); should_remove[index] = false; } else { - autoware_map_msgs::msg::PointCloudMapWithID pcd_map_with_id; - loadPointCloudMapWithID(path, map_id, pcd_map_with_id); - response->loaded_pcds.push_back(pcd_map_with_id); + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_cell_with_id = + loadPointCloudMapCellWithID(path, map_id, metadata.min, metadata.max); + response->new_pointcloud_with_ids.push_back(pointcloud_cell_with_id); } } - for (int i = 0; i < int(already_loaded_ids.size()); ++i) { + for (int i = 0; i < int(cached_ids.size()); ++i) { if (should_remove[i]) { - response->ids_to_remove.push_back(already_loaded_ids[i]); + response->ids_to_remove.push_back(cached_ids[i]); } } RCLCPP_INFO_STREAM(logger_, "Finished diff area loading"); } -bool DifferentialMapLoaderModule::onServiceLoadDifferentialPointCloudMap( - LoadDifferentialPointCloudMap::Request::SharedPtr req, - LoadDifferentialPointCloudMap::Response::SharedPtr res) +bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( + GetDifferentialPointCloudMap::Request::SharedPtr req, + GetDifferentialPointCloudMap::Response::SharedPtr res) { auto area = req->area; - std::vector already_loaded_ids = req->already_loaded_ids; - differentialAreaLoad(area, already_loaded_ids, res); + std::vector cached_ids = req->cached_ids; + differentialAreaLoad(area, cached_ids, res); return true; } -void DifferentialMapLoaderModule::loadPointCloudMapWithID( +autoware_map_msgs::msg::PointCloudMapCellWithID DifferentialMapLoaderModule::loadPointCloudMapCellWithID( const std::string path, const std::string map_id, - autoware_map_msgs::msg::PointCloudMapWithID & pcd_map_with_id) const + const pcl::PointXYZ min_point, const pcl::PointXYZ max_point) const { sensor_msgs::msg::PointCloud2 pcd; if (pcl::io::loadPCDFile(path, pcd) == -1) { RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); } - pcd_map_with_id.pointcloud = pcd; - pcd_map_with_id.id = map_id; + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_cell_with_id; + pointcloud_cell_with_id.pointcloud = pcd; + pointcloud_cell_with_id.cell_id = map_id; + pointcloud_cell_with_id.min_point.x = min_point.x; + pointcloud_cell_with_id.min_point.y = min_point.y; + pointcloud_cell_with_id.min_point.z = min_point.z; + pointcloud_cell_with_id.max_point.x = max_point.x; + pointcloud_cell_with_id.max_point.y = max_point.y; + pointcloud_cell_with_id.max_point.z = max_point.z; + return pointcloud_cell_with_id; } diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index 23f25b699df49..644a4152895c1 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -19,7 +19,7 @@ #include -#include "autoware_map_msgs/srv/load_differential_point_cloud_map.hpp" +#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp" #include #include @@ -34,7 +34,7 @@ class DifferentialMapLoaderModule { - using LoadDifferentialPointCloudMap = autoware_map_msgs::srv::LoadDifferentialPointCloudMap; + using GetDifferentialPointCloudMap = autoware_map_msgs::srv::GetDifferentialPointCloudMap; public: explicit DifferentialMapLoaderModule( @@ -44,18 +44,18 @@ class DifferentialMapLoaderModule rclcpp::Logger logger_; std::map all_pcd_file_metadata_dict_; - rclcpp::Service::SharedPtr load_differential_pcd_maps_service_; + rclcpp::Service::SharedPtr load_differential_pcd_maps_service_; - bool onServiceLoadDifferentialPointCloudMap( - LoadDifferentialPointCloudMap::Request::SharedPtr req, - LoadDifferentialPointCloudMap::Response::SharedPtr res); + bool onServiceGetDifferentialPointCloudMap( + GetDifferentialPointCloudMap::Request::SharedPtr req, + GetDifferentialPointCloudMap::Response::SharedPtr res); void differentialAreaLoad( const autoware_map_msgs::msg::AreaInfo area_info, - const std::vector already_loaded_ids, - LoadDifferentialPointCloudMap::Response::SharedPtr & response) const; - void loadPointCloudMapWithID( + const std::vector cached_ids, + GetDifferentialPointCloudMap::Response::SharedPtr & response) const; + autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( const std::string path, const std::string map_id, - autoware_map_msgs::msg::PointCloudMapWithID & pcd_map_with_id) const; + const pcl::PointXYZ min_point, const pcl::PointXYZ max_point) const; }; #endif // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index a69511e20d48c..9c6143908bbc8 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -18,15 +18,15 @@ PartialMapLoaderModule::PartialMapLoaderModule( rclcpp::Node * node, const std::map & pcd_file_metadata_dict) : logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) { - load_partial_pcd_maps_service_ = node->create_service( + load_partial_pcd_maps_service_ = node->create_service( "service/load_partial_pcd_map", std::bind( - &PartialMapLoaderModule::onServiceLoadPartialPointCloudMap, + &PartialMapLoaderModule::onServiceGetPartialPointCloudMap, this, std::placeholders::_1, std::placeholders::_2)); } void PartialMapLoaderModule::partialAreaLoad( const autoware_map_msgs::msg::AreaInfo area, - LoadPartialPointCloudMap::Response::SharedPtr & response) const + GetPartialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids @@ -40,29 +40,37 @@ void PartialMapLoaderModule::partialAreaLoad( // skip if the pcd file is not within the queried area if (!isGridWithinQueriedArea(area, metadata)) continue; - autoware_map_msgs::msg::PointCloudMapWithID pcd_map_with_id; - loadPointCloudMapWithID(path, map_id, pcd_map_with_id); - response->loaded_pcds.push_back(pcd_map_with_id); + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_cell_with_id = + loadPointCloudMapCellWithID(path, map_id, metadata.min, metadata.max); + response->new_pointcloud_with_ids.push_back(pointcloud_cell_with_id); } } -bool PartialMapLoaderModule::onServiceLoadPartialPointCloudMap( - LoadPartialPointCloudMap::Request::SharedPtr req, - LoadPartialPointCloudMap::Response::SharedPtr res) +bool PartialMapLoaderModule::onServiceGetPartialPointCloudMap( + GetPartialPointCloudMap::Request::SharedPtr req, + GetPartialPointCloudMap::Response::SharedPtr res) { auto area = req->area; partialAreaLoad(area, res); return true; } -void PartialMapLoaderModule::loadPointCloudMapWithID( +autoware_map_msgs::msg::PointCloudMapCellWithID PartialMapLoaderModule::loadPointCloudMapCellWithID( const std::string path, const std::string map_id, - autoware_map_msgs::msg::PointCloudMapWithID & pcd_map_with_id) const + const pcl::PointXYZ min_point, const pcl::PointXYZ max_point) const { sensor_msgs::msg::PointCloud2 pcd; if (pcl::io::loadPCDFile(path, pcd) == -1) { RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); } - pcd_map_with_id.pointcloud = pcd; - pcd_map_with_id.id = map_id; + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_cell_with_id; + pointcloud_cell_with_id.pointcloud = pcd; + pointcloud_cell_with_id.cell_id = map_id; + pointcloud_cell_with_id.min_point.x = min_point.x; + pointcloud_cell_with_id.min_point.y = min_point.y; + pointcloud_cell_with_id.min_point.z = min_point.z; + pointcloud_cell_with_id.max_point.x = max_point.x; + pointcloud_cell_with_id.max_point.y = max_point.y; + pointcloud_cell_with_id.max_point.z = max_point.z; + return pointcloud_cell_with_id; } diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp index a0b579a6fd8e1..81a940b327560 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp @@ -19,7 +19,7 @@ #include -#include "autoware_map_msgs/srv/load_partial_point_cloud_map.hpp" +#include "autoware_map_msgs/srv/get_partial_point_cloud_map.hpp" #include #include @@ -34,7 +34,7 @@ class PartialMapLoaderModule { - using LoadPartialPointCloudMap = autoware_map_msgs::srv::LoadPartialPointCloudMap; + using GetPartialPointCloudMap = autoware_map_msgs::srv::GetPartialPointCloudMap; public: explicit PartialMapLoaderModule( @@ -44,17 +44,17 @@ class PartialMapLoaderModule rclcpp::Logger logger_; std::map all_pcd_file_metadata_dict_; - rclcpp::Service::SharedPtr load_partial_pcd_maps_service_; + rclcpp::Service::SharedPtr load_partial_pcd_maps_service_; - bool onServiceLoadPartialPointCloudMap( - LoadPartialPointCloudMap::Request::SharedPtr req, - LoadPartialPointCloudMap::Response::SharedPtr res); + bool onServiceGetPartialPointCloudMap( + GetPartialPointCloudMap::Request::SharedPtr req, + GetPartialPointCloudMap::Response::SharedPtr res); void partialAreaLoad( const autoware_map_msgs::msg::AreaInfo area, - LoadPartialPointCloudMap::Response::SharedPtr & response) const; - void loadPointCloudMapWithID( + GetPartialPointCloudMap::Response::SharedPtr & response) const; + autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( const std::string path, const std::string map_id, - autoware_map_msgs::msg::PointCloudMapWithID & pcd_map_with_id) const; + const pcl::PointXYZ min_point, const pcl::PointXYZ max_point) const; }; #endif // POINTCLOUD_MAP_LOADER__PARTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp index 62e3d900b7cb3..0a0d2912bb8c5 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -16,7 +16,6 @@ #define POINTCLOUD_MAP_LOADER__UTILS_HPP_ #include -#include #include #include From c265b64e2dcf43addedf5d4335d145960293c57b Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 11 Oct 2022 07:52:34 +0000 Subject: [PATCH 14/48] ci(pre-commit): autofix --- .../differential_map_loader_module.cpp | 12 ++++++------ .../differential_map_loader_module.hpp | 7 +++---- .../partial_map_loader_module.cpp | 9 ++++----- .../partial_map_loader_module.hpp | 4 ++-- 4 files changed, 15 insertions(+), 17 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 9f51a32b80298..7434f6c42fcac 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -41,13 +41,12 @@ void DifferentialMapLoaderModule::differentialAreaLoad( // skip if the pcd file is not within the queried area if (!isGridWithinQueriedArea(area, metadata)) continue; - auto id_in_already_loaded_list = - std::find(cached_ids.begin(), cached_ids.end(), map_id); + auto id_in_already_loaded_list = std::find(cached_ids.begin(), cached_ids.end(), map_id); if (id_in_already_loaded_list != cached_ids.end()) { int index = id_in_already_loaded_list - cached_ids.begin(); should_remove[index] = false; } else { - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_cell_with_id = + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_cell_with_id = loadPointCloudMapCellWithID(path, map_id, metadata.min, metadata.max); response->new_pointcloud_with_ids.push_back(pointcloud_cell_with_id); } @@ -72,9 +71,10 @@ bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( return true; } -autoware_map_msgs::msg::PointCloudMapCellWithID DifferentialMapLoaderModule::loadPointCloudMapCellWithID( - const std::string path, const std::string map_id, - const pcl::PointXYZ min_point, const pcl::PointXYZ max_point) const +autoware_map_msgs::msg::PointCloudMapCellWithID +DifferentialMapLoaderModule::loadPointCloudMapCellWithID( + const std::string path, const std::string map_id, const pcl::PointXYZ min_point, + const pcl::PointXYZ max_point) const { sensor_msgs::msg::PointCloud2 pcd; if (pcl::io::loadPCDFile(path, pcd) == -1) { diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index 644a4152895c1..833edd67ee81e 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -50,12 +50,11 @@ class DifferentialMapLoaderModule GetDifferentialPointCloudMap::Request::SharedPtr req, GetDifferentialPointCloudMap::Response::SharedPtr res); void differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area_info, - const std::vector cached_ids, + const autoware_map_msgs::msg::AreaInfo area_info, const std::vector cached_ids, GetDifferentialPointCloudMap::Response::SharedPtr & response) const; autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( - const std::string path, const std::string map_id, - const pcl::PointXYZ min_point, const pcl::PointXYZ max_point) const; + const std::string path, const std::string map_id, const pcl::PointXYZ min_point, + const pcl::PointXYZ max_point) const; }; #endif // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index 9c6143908bbc8..2bdc92a58a144 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -40,15 +40,14 @@ void PartialMapLoaderModule::partialAreaLoad( // skip if the pcd file is not within the queried area if (!isGridWithinQueriedArea(area, metadata)) continue; - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_cell_with_id = + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_cell_with_id = loadPointCloudMapCellWithID(path, map_id, metadata.min, metadata.max); response->new_pointcloud_with_ids.push_back(pointcloud_cell_with_id); } } bool PartialMapLoaderModule::onServiceGetPartialPointCloudMap( - GetPartialPointCloudMap::Request::SharedPtr req, - GetPartialPointCloudMap::Response::SharedPtr res) + GetPartialPointCloudMap::Request::SharedPtr req, GetPartialPointCloudMap::Response::SharedPtr res) { auto area = req->area; partialAreaLoad(area, res); @@ -56,8 +55,8 @@ bool PartialMapLoaderModule::onServiceGetPartialPointCloudMap( } autoware_map_msgs::msg::PointCloudMapCellWithID PartialMapLoaderModule::loadPointCloudMapCellWithID( - const std::string path, const std::string map_id, - const pcl::PointXYZ min_point, const pcl::PointXYZ max_point) const + const std::string path, const std::string map_id, const pcl::PointXYZ min_point, + const pcl::PointXYZ max_point) const { sensor_msgs::msg::PointCloud2 pcd; if (pcl::io::loadPCDFile(path, pcd) == -1) { diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp index 81a940b327560..6c50d1e08494a 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp @@ -53,8 +53,8 @@ class PartialMapLoaderModule const autoware_map_msgs::msg::AreaInfo area, GetPartialPointCloudMap::Response::SharedPtr & response) const; autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( - const std::string path, const std::string map_id, - const pcl::PointXYZ min_point, const pcl::PointXYZ max_point) const; + const std::string path, const std::string map_id, const pcl::PointXYZ min_point, + const pcl::PointXYZ max_point) const; }; #endif // POINTCLOUD_MAP_LOADER__PARTIAL_MAP_LOADER_MODULE_HPP_ From 64f09f43edbf0570319913f9cdad4bf3bbfe6220 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 11 Oct 2022 16:52:53 +0900 Subject: [PATCH 15/48] minor fix Signed-off-by: kminoda --- .../differential_map_loader_module.cpp | 24 +++++++++---------- .../partial_map_loader_module.cpp | 24 +++++++++---------- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 9f51a32b80298..8781aa12970e7 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -47,9 +47,9 @@ void DifferentialMapLoaderModule::differentialAreaLoad( int index = id_in_already_loaded_list - cached_ids.begin(); should_remove[index] = false; } else { - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_cell_with_id = + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = loadPointCloudMapCellWithID(path, map_id, metadata.min, metadata.max); - response->new_pointcloud_with_ids.push_back(pointcloud_cell_with_id); + response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); } } @@ -80,14 +80,14 @@ autoware_map_msgs::msg::PointCloudMapCellWithID DifferentialMapLoaderModule::loa if (pcl::io::loadPCDFile(path, pcd) == -1) { RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); } - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_cell_with_id; - pointcloud_cell_with_id.pointcloud = pcd; - pointcloud_cell_with_id.cell_id = map_id; - pointcloud_cell_with_id.min_point.x = min_point.x; - pointcloud_cell_with_id.min_point.y = min_point.y; - pointcloud_cell_with_id.min_point.z = min_point.z; - pointcloud_cell_with_id.max_point.x = max_point.x; - pointcloud_cell_with_id.max_point.y = max_point.y; - pointcloud_cell_with_id.max_point.z = max_point.z; - return pointcloud_cell_with_id; + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id; + pointcloud_map_cell_with_id.pointcloud = pcd; + pointcloud_map_cell_with_id.cell_id = map_id; + pointcloud_map_cell_with_id.min_point.x = min_point.x; + pointcloud_map_cell_with_id.min_point.y = min_point.y; + pointcloud_map_cell_with_id.min_point.z = min_point.z; + pointcloud_map_cell_with_id.max_point.x = max_point.x; + pointcloud_map_cell_with_id.max_point.y = max_point.y; + pointcloud_map_cell_with_id.max_point.z = max_point.z; + return pointcloud_map_cell_with_id; } diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index 9c6143908bbc8..ee2c53a10f831 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -40,9 +40,9 @@ void PartialMapLoaderModule::partialAreaLoad( // skip if the pcd file is not within the queried area if (!isGridWithinQueriedArea(area, metadata)) continue; - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_cell_with_id = + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = loadPointCloudMapCellWithID(path, map_id, metadata.min, metadata.max); - response->new_pointcloud_with_ids.push_back(pointcloud_cell_with_id); + response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); } } @@ -63,14 +63,14 @@ autoware_map_msgs::msg::PointCloudMapCellWithID PartialMapLoaderModule::loadPoin if (pcl::io::loadPCDFile(path, pcd) == -1) { RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); } - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_cell_with_id; - pointcloud_cell_with_id.pointcloud = pcd; - pointcloud_cell_with_id.cell_id = map_id; - pointcloud_cell_with_id.min_point.x = min_point.x; - pointcloud_cell_with_id.min_point.y = min_point.y; - pointcloud_cell_with_id.min_point.z = min_point.z; - pointcloud_cell_with_id.max_point.x = max_point.x; - pointcloud_cell_with_id.max_point.y = max_point.y; - pointcloud_cell_with_id.max_point.z = max_point.z; - return pointcloud_cell_with_id; + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id; + pointcloud_map_cell_with_id.pointcloud = pcd; + pointcloud_map_cell_with_id.cell_id = map_id; + pointcloud_map_cell_with_id.min_point.x = min_point.x; + pointcloud_map_cell_with_id.min_point.y = min_point.y; + pointcloud_map_cell_with_id.min_point.z = min_point.z; + pointcloud_map_cell_with_id.max_point.x = max_point.x; + pointcloud_map_cell_with_id.max_point.y = max_point.y; + pointcloud_map_cell_with_id.max_point.z = max_point.z; + return pointcloud_map_cell_with_id; } From 6d6a2eb5b6b91be588dd0f18dab438e50abf0b7b Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 11 Oct 2022 16:54:51 +0900 Subject: [PATCH 16/48] already_loaded -> cached Signed-off-by: kminoda --- .../differential_map_loader_module.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 66344af5f19b7..830a0c7bc00c6 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -41,9 +41,9 @@ void DifferentialMapLoaderModule::differentialAreaLoad( // skip if the pcd file is not within the queried area if (!isGridWithinQueriedArea(area, metadata)) continue; - auto id_in_already_loaded_list = std::find(cached_ids.begin(), cached_ids.end(), map_id); - if (id_in_already_loaded_list != cached_ids.end()) { - int index = id_in_already_loaded_list - cached_ids.begin(); + auto id_in_cached_list = std::find(cached_ids.begin(), cached_ids.end(), map_id); + if (id_in_cached_list != cached_ids.end()) { + int index = id_in_cached_list - cached_ids.begin(); should_remove[index] = false; } else { autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = From c7379a2db0525fe56745a8fc2466fc726a28829c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 11 Oct 2022 07:55:54 +0000 Subject: [PATCH 17/48] ci(pre-commit): autofix --- .../pointcloud_map_loader/differential_map_loader_module.cpp | 2 +- .../src/pointcloud_map_loader/partial_map_loader_module.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 830a0c7bc00c6..35590fdc90cf9 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -46,7 +46,7 @@ void DifferentialMapLoaderModule::differentialAreaLoad( int index = id_in_cached_list - cached_ids.begin(); should_remove[index] = false; } else { - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = loadPointCloudMapCellWithID(path, map_id, metadata.min, metadata.max); response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); } diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index 9f302018f51a6..caf0bb06cf0bd 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -40,7 +40,7 @@ void PartialMapLoaderModule::partialAreaLoad( // skip if the pcd file is not within the queried area if (!isGridWithinQueriedArea(area, metadata)) continue; - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = + autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = loadPointCloudMapCellWithID(path, map_id, metadata.min, metadata.max); response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); } From 894e28e2769bff5031993dbff68d9569369f731f Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 11 Oct 2022 16:58:18 +0900 Subject: [PATCH 18/48] load_ -> get_ Signed-off-by: kminoda --- launch/tier4_map_launch/launch/map.launch.py | 4 ++-- map/map_loader/launch/pointcloud_map_loader.launch.xml | 4 ++-- .../pointcloud_map_loader/differential_map_loader_module.cpp | 4 ++-- .../pointcloud_map_loader/differential_map_loader_module.hpp | 2 +- .../src/pointcloud_map_loader/partial_map_loader_module.cpp | 4 ++-- .../src/pointcloud_map_loader/partial_map_loader_module.hpp | 2 +- 6 files changed, 10 insertions(+), 10 deletions(-) diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index d4043c90c7edd..840fe4d907278 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -85,8 +85,8 @@ def launch_setup(context, *args, **kwargs): remappings=[ ("output/pointcloud_map", "pointcloud_map"), ("output/debug/downsampled_pointcloud_map", "pointcloud_map_downsampled"), - ("service/load_partial_pcd_map", "/map/load_partial_pointcloud_map"), - ("service/load_differential_pcd_map", "/map/load_differential_pointcloud_map"), + ("service/get_partial_pcd_map", "/map/get_partial_pointcloud_map"), + ("service/get_differential_pcd_map", "/map/get_differential_pointcloud_map"), ], parameters=[ {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]}, diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml index d3c800fc16a4f..d9a7851cc9016 100644 --- a/map/map_loader/launch/pointcloud_map_loader.launch.xml +++ b/map/map_loader/launch/pointcloud_map_loader.launch.xml @@ -5,8 +5,8 @@ - - + + diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index 830a0c7bc00c6..c3ec6f4f0fbe0 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -18,8 +18,8 @@ DifferentialMapLoaderModule::DifferentialMapLoaderModule( rclcpp::Node * node, const std::map & pcd_file_metadata_dict) : logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) { - load_differential_pcd_maps_service_ = node->create_service( - "service/load_differential_pcd_map", + get_differential_pcd_maps_service_ = node->create_service( + "service/get_differential_pcd_map", std::bind( &DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap, this, std::placeholders::_1, std::placeholders::_2)); diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index 833edd67ee81e..73803c84936f2 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -44,7 +44,7 @@ class DifferentialMapLoaderModule rclcpp::Logger logger_; std::map all_pcd_file_metadata_dict_; - rclcpp::Service::SharedPtr load_differential_pcd_maps_service_; + rclcpp::Service::SharedPtr get_differential_pcd_maps_service_; bool onServiceGetDifferentialPointCloudMap( GetDifferentialPointCloudMap::Request::SharedPtr req, diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index 9f302018f51a6..f4d60f142b365 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -18,8 +18,8 @@ PartialMapLoaderModule::PartialMapLoaderModule( rclcpp::Node * node, const std::map & pcd_file_metadata_dict) : logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) { - load_partial_pcd_maps_service_ = node->create_service( - "service/load_partial_pcd_map", std::bind( + get_partial_pcd_maps_service_ = node->create_service( + "service/get_partial_pcd_map", std::bind( &PartialMapLoaderModule::onServiceGetPartialPointCloudMap, this, std::placeholders::_1, std::placeholders::_2)); } diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp index 6c50d1e08494a..d1eb162d4d1dd 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp @@ -44,7 +44,7 @@ class PartialMapLoaderModule rclcpp::Logger logger_; std::map all_pcd_file_metadata_dict_; - rclcpp::Service::SharedPtr load_partial_pcd_maps_service_; + rclcpp::Service::SharedPtr get_partial_pcd_maps_service_; bool onServiceGetPartialPointCloudMap( GetPartialPointCloudMap::Request::SharedPtr req, From 5c0b2f60bb68b16d80729075c8664196c8171ecc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 11 Oct 2022 08:01:53 +0000 Subject: [PATCH 19/48] ci(pre-commit): autofix --- .../src/pointcloud_map_loader/partial_map_loader_module.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index 6ac643071549e..a1ffe47101b5f 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -20,8 +20,8 @@ PartialMapLoaderModule::PartialMapLoaderModule( { get_partial_pcd_maps_service_ = node->create_service( "service/get_partial_pcd_map", std::bind( - &PartialMapLoaderModule::onServiceGetPartialPointCloudMap, - this, std::placeholders::_1, std::placeholders::_2)); + &PartialMapLoaderModule::onServiceGetPartialPointCloudMap, + this, std::placeholders::_1, std::placeholders::_2)); } void PartialMapLoaderModule::partialAreaLoad( From 1b6691493992ea3556b6d737d48d460fb99ca8da Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 8 Nov 2022 13:45:17 +0900 Subject: [PATCH 20/48] resolve pre-commit Signed-off-by: kminoda --- .../pointcloud_map_loader/differential_map_loader_module.cpp | 4 ++-- .../pointcloud_map_loader/pointcloud_map_loader_module.cpp | 5 ++++- .../pointcloud_map_loader/pointcloud_map_loader_module.hpp | 3 +++ .../src/pointcloud_map_loader/pointcloud_map_loader_node.cpp | 2 ++ .../src/pointcloud_map_loader/pointcloud_map_loader_node.hpp | 2 ++ 5 files changed, 13 insertions(+), 3 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index c76558d3979d3..db6c9da1877b7 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -30,7 +30,7 @@ void DifferentialMapLoaderModule::differentialAreaLoad( GetDifferentialPointCloudMap::Response::SharedPtr & response) const { // iterate over all the available pcd map grids - std::vector should_remove(int(cached_ids.size()), true); + std::vector should_remove(static_cast(cached_ids.size()), true); for (const auto & ele : all_pcd_file_metadata_dict_) { std::string path = ele.first; PCDFileMetadata metadata = ele.second; @@ -52,7 +52,7 @@ void DifferentialMapLoaderModule::differentialAreaLoad( } } - for (int i = 0; i < int(cached_ids.size()); ++i) { + for (int i = 0; i < static_cast(cached_ids.size()); ++i) { if (should_remove[i]) { response->ids_to_remove.push_back(cached_ids[i]); } diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index 0fc1d4c6fd3dd..d0d489783ebed 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -18,6 +18,9 @@ #include #include +#include +#include + sensor_msgs::msg::PointCloud2 downsample( const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) { @@ -68,7 +71,7 @@ sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( sensor_msgs::msg::PointCloud2 whole_pcd; sensor_msgs::msg::PointCloud2 partial_pcd; - for (int i = 0; i < int(pcd_paths.size()); ++i) { + for (int i = 0; i < static_cast(pcd_paths.size()); ++i) { auto & path = pcd_paths[i]; if (i % 50 == 0) { RCLCPP_INFO_STREAM( diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index 42b463ee1411e..4cd0e981597c6 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -21,6 +21,9 @@ #include +#include +#include + class PointcloudMapLoaderModule { public: diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 657cec9709d44..dc64a92c027c2 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -38,6 +38,8 @@ #include #include #include +#include +#include namespace fs = std::filesystem; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index 12a1b953c8b91..116a96e31dfec 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -45,6 +45,8 @@ #include #include #include +#include +#include class PointCloudMapLoaderNode : public rclcpp::Node { From d29c6d01877b23565ffe1ebf3b62ffad2962e4a6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 8 Nov 2022 04:47:40 +0000 Subject: [PATCH 21/48] ci(pre-commit): autofix --- .../src/pointcloud_map_loader/pointcloud_map_loader_node.cpp | 4 ++-- .../src/pointcloud_map_loader/pointcloud_map_loader_node.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index dc64a92c027c2..57b022cdc5e09 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -36,10 +36,10 @@ #include #include +#include +#include #include #include -#include -#include namespace fs = std::filesystem; diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index 116a96e31dfec..bb51eb21df6c0 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -41,12 +41,12 @@ #include #include +#include +#include #include #include #include #include -#include -#include class PointCloudMapLoaderNode : public rclcpp::Node { From 4db33815571c5a3e8e2a5724c681722d1d2adea5 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 8 Nov 2022 13:53:43 +0900 Subject: [PATCH 22/48] minor fix Signed-off-by: kminoda --- map/map_loader/package.xml | 1 + .../differential_map_loader_module.cpp | 12 +++--------- .../differential_map_loader_module.hpp | 3 +-- .../partial_map_loader_module.cpp | 12 +++--------- .../partial_map_loader_module.hpp | 3 +-- .../pointcloud_map_loader_module.cpp | 3 ++- 6 files changed, 11 insertions(+), 23 deletions(-) diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 9fac58f667e86..ff6d2b44253bb 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -28,6 +28,7 @@ tier4_autoware_utils tier4_debug_msgs visualization_msgs + fmt ament_lint_auto autoware_lint_common diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp index db6c9da1877b7..89d2e4b2f0aa1 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp @@ -47,7 +47,7 @@ void DifferentialMapLoaderModule::differentialAreaLoad( should_remove[index] = false; } else { autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id, metadata.min, metadata.max); + loadPointCloudMapCellWithID(path, map_id); response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); } } @@ -68,13 +68,13 @@ bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( auto area = req->area; std::vector cached_ids = req->cached_ids; differentialAreaLoad(area, cached_ids, res); + res->header.frame_id = "map"; return true; } autoware_map_msgs::msg::PointCloudMapCellWithID DifferentialMapLoaderModule::loadPointCloudMapCellWithID( - const std::string path, const std::string map_id, const pcl::PointXYZ min_point, - const pcl::PointXYZ max_point) const + const std::string path, const std::string map_id) const { sensor_msgs::msg::PointCloud2 pcd; if (pcl::io::loadPCDFile(path, pcd) == -1) { @@ -83,11 +83,5 @@ DifferentialMapLoaderModule::loadPointCloudMapCellWithID( autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id; pointcloud_map_cell_with_id.pointcloud = pcd; pointcloud_map_cell_with_id.cell_id = map_id; - pointcloud_map_cell_with_id.min_point.x = min_point.x; - pointcloud_map_cell_with_id.min_point.y = min_point.y; - pointcloud_map_cell_with_id.min_point.z = min_point.z; - pointcloud_map_cell_with_id.max_point.x = max_point.x; - pointcloud_map_cell_with_id.max_point.y = max_point.y; - pointcloud_map_cell_with_id.max_point.z = max_point.z; return pointcloud_map_cell_with_id; } diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp index 73803c84936f2..81718a6791de9 100644 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp @@ -53,8 +53,7 @@ class DifferentialMapLoaderModule const autoware_map_msgs::msg::AreaInfo area_info, const std::vector cached_ids, GetDifferentialPointCloudMap::Response::SharedPtr & response) const; autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( - const std::string path, const std::string map_id, const pcl::PointXYZ min_point, - const pcl::PointXYZ max_point) const; + const std::string path, const std::string map_id) const; }; #endif // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp index a1ffe47101b5f..af53438e8e321 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -41,7 +41,7 @@ void PartialMapLoaderModule::partialAreaLoad( if (!isGridWithinQueriedArea(area, metadata)) continue; autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id, metadata.min, metadata.max); + loadPointCloudMapCellWithID(path, map_id); response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); } } @@ -51,12 +51,12 @@ bool PartialMapLoaderModule::onServiceGetPartialPointCloudMap( { auto area = req->area; partialAreaLoad(area, res); + res->header.frame_id = "map"; return true; } autoware_map_msgs::msg::PointCloudMapCellWithID PartialMapLoaderModule::loadPointCloudMapCellWithID( - const std::string path, const std::string map_id, const pcl::PointXYZ min_point, - const pcl::PointXYZ max_point) const + const std::string path, const std::string map_id) const { sensor_msgs::msg::PointCloud2 pcd; if (pcl::io::loadPCDFile(path, pcd) == -1) { @@ -65,11 +65,5 @@ autoware_map_msgs::msg::PointCloudMapCellWithID PartialMapLoaderModule::loadPoin autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id; pointcloud_map_cell_with_id.pointcloud = pcd; pointcloud_map_cell_with_id.cell_id = map_id; - pointcloud_map_cell_with_id.min_point.x = min_point.x; - pointcloud_map_cell_with_id.min_point.y = min_point.y; - pointcloud_map_cell_with_id.min_point.z = min_point.z; - pointcloud_map_cell_with_id.max_point.x = max_point.x; - pointcloud_map_cell_with_id.max_point.y = max_point.y; - pointcloud_map_cell_with_id.max_point.z = max_point.z; return pointcloud_map_cell_with_id; } diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp index d1eb162d4d1dd..052aa4245c099 100644 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp @@ -53,8 +53,7 @@ class PartialMapLoaderModule const autoware_map_msgs::msg::AreaInfo area, GetPartialPointCloudMap::Response::SharedPtr & response) const; autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( - const std::string path, const std::string map_id, const pcl::PointXYZ min_point, - const pcl::PointXYZ max_point) const; + const std::string path, const std::string map_id) const; }; #endif // POINTCLOUD_MAP_LOADER__PARTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index d0d489783ebed..ad25ff745d1aa 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -75,7 +76,7 @@ sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( auto & path = pcd_paths[i]; if (i % 50 == 0) { RCLCPP_INFO_STREAM( - logger_, "Load " << path << " (" << i + 1 << " out of " << int(pcd_paths.size()) << ")"); + logger_, fmt::format("Load {} ({} out of {})", path, i+1, int(pcd_paths.size()))); } if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { From 7405783f71b7fdbbe746d1d0c289023dfce9231f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 8 Nov 2022 04:55:52 +0000 Subject: [PATCH 23/48] ci(pre-commit): autofix --- map/map_loader/package.xml | 2 +- .../pointcloud_map_loader/pointcloud_map_loader_module.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index ff6d2b44253bb..70ec499a08003 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -14,6 +14,7 @@ autoware_auto_mapping_msgs autoware_map_msgs + fmt geometry_msgs lanelet2_extension libpcl-all-dev @@ -28,7 +29,6 @@ tier4_autoware_utils tier4_debug_msgs visualization_msgs - fmt ament_lint_auto autoware_lint_common diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index ad25ff745d1aa..e1a4f0b28ca97 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -14,10 +14,10 @@ #include "pointcloud_map_loader_module.hpp" +#include #include #include #include -#include #include #include @@ -76,7 +76,7 @@ sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( auto & path = pcd_paths[i]; if (i % 50 == 0) { RCLCPP_INFO_STREAM( - logger_, fmt::format("Load {} ({} out of {})", path, i+1, int(pcd_paths.size()))); + logger_, fmt::format("Load {} ({} out of {})", path, i + 1, int(pcd_paths.size()))); } if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { From d26bd0bf53ab6bc70ee1a22ebcbce4abec4743f8 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 8 Nov 2022 14:08:38 +0900 Subject: [PATCH 24/48] update readme Signed-off-by: kminoda --- map/map_loader/README.md | 33 ++++++++++++++++++++++++--------- 1 file changed, 24 insertions(+), 9 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 90c1e8891a180..21bab27cd0c94 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -6,15 +6,30 @@ This package provides the features of loading various maps. ### Feature -pointcloud_map_loader loads PointCloud file and publishes the map data as sensor_msgs/PointCloud2 message. - -### How to run - -`ros2 run map_loader pointcloud_map_loader --ros-args -p "pcd_paths_or_directory:=[path/to/pointcloud1.pcd, path/to/pointcloud2.pcd, ...]"` - -### Published Topics - -- pointcloud_map (sensor_msgs/PointCloud2) : PointCloud Map +`pointcloud_map_loader` provides pointcloud maps to the other Autoware nodes in various configurations. +Currently, it supports the following four types: +- Publish raw pointcloud map as `sensor_msgs/msg/PointCloud2` +- Publish downsampled pointcloud map as `sensor_msgs/msg/PointCloud2` +- Send partial pointcloud map loading as `autoware_map_msgs/srv/GetPartialPointCloudMap` +- Send differential pointcloud map loading as `autoware_map_msgs/srv/GetDifferentialPointCloudMap` + +### Parameters + +| Name | Type | Description | Default value | +| :---------------------------- | :----- | :--------------------------------------------------------------------------------- | :------------ | +| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | +| enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | true | +| enable_partial_load | bool | A flag to enable partial pointcloud map server | true | +| enable_differential_load | bool | A flag to enable differential pointcloud map server | true | +| leaf_size | double | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | + + +### Interfaces + +- `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map +- `output/debug/downsampled_pointcloud_map` (sensor_msgs/msg/PointCloud2) : Downsampled pointcloud map +- `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map +- `service/get_differential_pcd_map` (autoware_map_msgs/srv/GetDifferentialPointCloudMap) : Differential pointcloud map --- From 39b36837b5438d7edc9d80f8f79fff7444703a9a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 8 Nov 2022 05:10:39 +0000 Subject: [PATCH 25/48] ci(pre-commit): autofix --- map/map_loader/README.md | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 21bab27cd0c94..12995ba51409a 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -8,6 +8,7 @@ This package provides the features of loading various maps. `pointcloud_map_loader` provides pointcloud maps to the other Autoware nodes in various configurations. Currently, it supports the following four types: + - Publish raw pointcloud map as `sensor_msgs/msg/PointCloud2` - Publish downsampled pointcloud map as `sensor_msgs/msg/PointCloud2` - Send partial pointcloud map loading as `autoware_map_msgs/srv/GetPartialPointCloudMap` @@ -15,14 +16,13 @@ Currently, it supports the following four types: ### Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :----- | :--------------------------------------------------------------------------------- | :------------ | -| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | -| enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | true | -| enable_partial_load | bool | A flag to enable partial pointcloud map server | true | -| enable_differential_load | bool | A flag to enable differential pointcloud map server | true | -| leaf_size | double | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | - +| Name | Type | Description | Default value | +| :---------------------------- | :----- | :-------------------------------------------------------------------------------- | :------------ | +| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | +| enable_downsampled_whole_load | bool | A flag to enable downsampled pointcloud map publishing | true | +| enable_partial_load | bool | A flag to enable partial pointcloud map server | true | +| enable_differential_load | bool | A flag to enable differential pointcloud map server | true | +| leaf_size | double | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | ### Interfaces From 09b02a91ee1daa48fa247f9ce81712ffc3c3f76c Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 8 Nov 2022 14:20:57 +0900 Subject: [PATCH 26/48] update readme Signed-off-by: kminoda --- map/map_loader/README.md | 27 +++++++++++++++++++++++---- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 21bab27cd0c94..498ce0bc3edc6 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -8,10 +8,29 @@ This package provides the features of loading various maps. `pointcloud_map_loader` provides pointcloud maps to the other Autoware nodes in various configurations. Currently, it supports the following four types: -- Publish raw pointcloud map as `sensor_msgs/msg/PointCloud2` -- Publish downsampled pointcloud map as `sensor_msgs/msg/PointCloud2` -- Send partial pointcloud map loading as `autoware_map_msgs/srv/GetPartialPointCloudMap` -- Send differential pointcloud map loading as `autoware_map_msgs/srv/GetDifferentialPointCloudMap` +- Publish raw pointcloud map +- Publish downsampled pointcloud map +- Send partial pointcloud map loading via ROS 2 service +- Send differential pointcloud map loading via ROS 2 service + + +#### Publish raw pointcloud map +The node publishes the raw pointcloud map loaded from `.pcd` file(s) as a ROS 2 topic. + +#### Publish downsampled pointcloud map +The node publishes the downsampled pointcloud map loaded from `.pcd` file(s) as a ROS 2 topic. You can specify the downsample resolution by changing `leaf_size` parameter. + +#### Send partial pointcloud map +Here, we assume that the pointcloud maps are divided into grids. + +Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area. +Please see [the definition of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details. + +#### Send differential pointcloud map +Here, we assume that the pointcloud maps are divided into grids. + +Given a query and a set of map IDs, the node sends a set of pointcloud maps that overlaps with the queried area and does not included in the set of map IDs. +Please see [the definition of `GetDifferentialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getdifferentialpointcloudmapsrv) for details. ### Parameters From cc47a118b3aa293a7122d46991c4ca2ea14803fc Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 8 Nov 2022 14:22:49 +0900 Subject: [PATCH 27/48] minor fix in readme Signed-off-by: kminoda --- map/map_loader/README.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index cba14b2675f76..5863a5b25959a 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -14,19 +14,19 @@ Currently, it supports the following four types: - Send differential pointcloud map loading via ROS 2 service -#### Publish raw pointcloud map -The node publishes the raw pointcloud map loaded from `.pcd` file(s) as a ROS 2 topic. +#### Publish raw pointcloud map (ROS 2 topic) +The node publishes the raw pointcloud map loaded from `.pcd` file(s). -#### Publish downsampled pointcloud map -The node publishes the downsampled pointcloud map loaded from `.pcd` file(s) as a ROS 2 topic. You can specify the downsample resolution by changing `leaf_size` parameter. +#### Publish downsampled pointcloud map (ROS 2 topic) +The node publishes the downsampled pointcloud map loaded from `.pcd` file(s). You can specify the downsample resolution by changing `leaf_size` parameter. -#### Send partial pointcloud map +#### Send partial pointcloud map (ROS 2 service) Here, we assume that the pointcloud maps are divided into grids. Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area. Please see [the definition of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details. -#### Send differential pointcloud map +#### Send differential pointcloud map (ROS 2 service) Here, we assume that the pointcloud maps are divided into grids. Given a query and a set of map IDs, the node sends a set of pointcloud maps that overlaps with the queried area and does not included in the set of map IDs. From 1f249776ccf28df92d4211c42c8bbe1319618905 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 8 Nov 2022 14:25:30 +0900 Subject: [PATCH 28/48] grammarly Signed-off-by: kminoda --- map/map_loader/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 5863a5b25959a..64f05a41a6991 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -15,22 +15,22 @@ Currently, it supports the following four types: #### Publish raw pointcloud map (ROS 2 topic) -The node publishes the raw pointcloud map loaded from `.pcd` file(s). +The node publishes the raw pointcloud map loaded from the `.pcd` file(s). #### Publish downsampled pointcloud map (ROS 2 topic) -The node publishes the downsampled pointcloud map loaded from `.pcd` file(s). You can specify the downsample resolution by changing `leaf_size` parameter. +The node publishes the downsampled pointcloud map loaded from the `.pcd` file(s). You can specify the downsample resolution by changing the `leaf_size` parameter. #### Send partial pointcloud map (ROS 2 service) Here, we assume that the pointcloud maps are divided into grids. Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area. -Please see [the definition of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details. +Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details. #### Send differential pointcloud map (ROS 2 service) Here, we assume that the pointcloud maps are divided into grids. -Given a query and a set of map IDs, the node sends a set of pointcloud maps that overlaps with the queried area and does not included in the set of map IDs. -Please see [the definition of `GetDifferentialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getdifferentialpointcloudmapsrv) for details. +Given a query and set of map IDs, the node sends a set of pointcloud maps that overlap with the queried area and are not included in the set of map IDs. +Please see [the description of `GetDifferentialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getdifferentialpointcloudmapsrv) for details. ### Parameters From b9c35166534d7022e5d1fadb8e66d1d2ed8896bc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 8 Nov 2022 05:27:40 +0000 Subject: [PATCH 29/48] ci(pre-commit): autofix --- map/map_loader/README.md | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 64f05a41a6991..3d440a437f334 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -8,28 +8,32 @@ This package provides the features of loading various maps. `pointcloud_map_loader` provides pointcloud maps to the other Autoware nodes in various configurations. Currently, it supports the following four types: + - Publish raw pointcloud map - Publish downsampled pointcloud map - Send partial pointcloud map loading via ROS 2 service - Send differential pointcloud map loading via ROS 2 service - #### Publish raw pointcloud map (ROS 2 topic) + The node publishes the raw pointcloud map loaded from the `.pcd` file(s). #### Publish downsampled pointcloud map (ROS 2 topic) + The node publishes the downsampled pointcloud map loaded from the `.pcd` file(s). You can specify the downsample resolution by changing the `leaf_size` parameter. #### Send partial pointcloud map (ROS 2 service) + Here, we assume that the pointcloud maps are divided into grids. Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area. Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details. #### Send differential pointcloud map (ROS 2 service) + Here, we assume that the pointcloud maps are divided into grids. -Given a query and set of map IDs, the node sends a set of pointcloud maps that overlap with the queried area and are not included in the set of map IDs. +Given a query and set of map IDs, the node sends a set of pointcloud maps that overlap with the queried area and are not included in the set of map IDs. Please see [the description of `GetDifferentialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getdifferentialpointcloudmapsrv) for details. ### Parameters From cecd73ce8a36de0273f2a4f2335462cc9100fb77 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 9 Nov 2022 06:58:04 +0000 Subject: [PATCH 30/48] ci(pre-commit): autofix --- map/map_loader/README.md | 13 +++++++------ .../pointcloud_map_loader_node.cpp | 3 +-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 67b21fd5d305b..510f204689d22 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -8,6 +8,7 @@ This package provides the features of loading various maps. `pointcloud_map_loader` provides pointcloud maps to the other Autoware nodes in various configurations. Currently, it supports the following three types: + - Publish raw pointcloud map - Send partial pointcloud map loading via ROS 2 service - Send differential pointcloud map loading via ROS 2 service @@ -32,12 +33,12 @@ Please see [the description of `GetDifferentialPointCloudMap.srv`](https://githu ### Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :----- | :-------------------------------------------------------------------------------- | :------------ | -| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | -| enable_partial_load | bool | A flag to enable partial pointcloud map server | true | -| enable_differential_load | bool | A flag to enable differential pointcloud map server | true | -| leaf_size | double | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | +| Name | Type | Description | Default value | +| :----------------------- | :----- | :-------------------------------------------------------------------------------- | :------------ | +| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | +| enable_partial_load | bool | A flag to enable partial pointcloud map server | true | +| enable_differential_load | bool | A flag to enable differential pointcloud map server | true | +| leaf_size | double | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | ### Interfaces diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 216e34871971f..17fc93dde52ae 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -55,8 +55,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt if (enable_whole_load) { std::string publisher_name = "output/pointcloud_map"; - pcd_map_loader_ = - std::make_unique(this, pcd_paths, publisher_name); + pcd_map_loader_ = std::make_unique(this, pcd_paths, publisher_name); } if (enable_partial_load | enable_differential_load) { From b009ee6675bb89d37ae578d5f408d9dd1cc151b3 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 9 Nov 2022 15:58:40 +0900 Subject: [PATCH 31/48] fix copyright Signed-off-by: kminoda --- .../pointcloud_map_loader_node.hpp | 18 +----------------- 1 file changed, 1 insertion(+), 17 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index bb51eb21df6c0..d321902131a81 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2022 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,22 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -/* - * Copyright 2015-2019 Autoware Foundation. 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. - */ - #ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ #define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ From 85457dc3e6454c676e23d93618aa1dd05baa4696 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 9 Nov 2022 16:42:54 +0900 Subject: [PATCH 32/48] fix launch file Signed-off-by: kminoda --- launch/tier4_map_launch/launch/map.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 7b1bedc9058ec..75400320f7e43 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -12,7 +12,7 @@ - + From 1f999db7129683032078693f5e101cfd7768bff8 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 9 Nov 2022 16:44:56 +0900 Subject: [PATCH 33/48] remove leaf_size param Signed-off-by: kminoda --- .../tier4_map_launch/config/pointcloud_map_loader.param.yaml | 3 --- launch/tier4_map_launch/launch/map.launch.xml | 1 + map/map_loader/config/pointcloud_map_loader.param.yaml | 3 --- 3 files changed, 1 insertion(+), 6 deletions(-) diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml index 37f617e7f40bb..ac20f2e191b47 100644 --- a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml +++ b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml @@ -3,6 +3,3 @@ enable_whole_load: true enable_partial_load: true enable_differential_load: true - - # only used when downsample_whole_load enabled - leaf_size: 3.0 # downsample leaf size [m] diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 75400320f7e43..539f5c2242933 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -3,6 +3,7 @@ + diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index 37f617e7f40bb..ac20f2e191b47 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -3,6 +3,3 @@ enable_whole_load: true enable_partial_load: true enable_differential_load: true - - # only used when downsample_whole_load enabled - leaf_size: 3.0 # downsample leaf size [m] From 583400b25582c90b33e0faddd468db4f8cfea9d0 Mon Sep 17 00:00:00 2001 From: kminoda Date: Wed, 9 Nov 2022 16:56:05 +0900 Subject: [PATCH 34/48] removed unnecessary things Signed-off-by: kminoda --- launch/tier4_map_launch/launch/map.launch.xml | 4 ++-- map/map_loader/package.xml | 2 -- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 539f5c2242933..8fe3f141a0dbd 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -11,8 +11,8 @@ - - + + diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 4833fe79f49aa..c71b4b32498a2 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -25,11 +25,9 @@ rclcpp rclcpp_components std_msgs - tf2 tf2_geometry_msgs tf2_ros tier4_autoware_utils - tier4_debug_msgs visualization_msgs ament_lint_auto From d9cbf318511756bf6641a31dbdfec82645ad21d2 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 14 Nov 2022 16:34:47 +0900 Subject: [PATCH 35/48] removed downsample for now Signed-off-by: kminoda --- launch/tier4_map_launch/launch/map.launch.py | 1 - 1 file changed, 1 deletion(-) diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index 840fe4d907278..bf396921fcb7a 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -84,7 +84,6 @@ def launch_setup(context, *args, **kwargs): name="pointcloud_map_loader", remappings=[ ("output/pointcloud_map", "pointcloud_map"), - ("output/debug/downsampled_pointcloud_map", "pointcloud_map_downsampled"), ("service/get_partial_pcd_map", "/map/get_partial_pointcloud_map"), ("service/get_differential_pcd_map", "/map/get_differential_pointcloud_map"), ], From 25da66ca5a24febfab084611f738b4e023947f40 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 14 Nov 2022 16:37:37 +0900 Subject: [PATCH 36/48] removed differential_map_loader for this PR (would make another PR for this) Signed-off-by: kminoda --- .../config/pointcloud_map_loader.param.yaml | 1 - map/map_loader/README.md | 12 +-- .../config/pointcloud_map_loader.param.yaml | 1 - .../launch/pointcloud_map_loader.launch.xml | 2 - .../differential_map_loader_module.cpp | 87 ------------------- .../differential_map_loader_module.hpp | 59 ------------- 6 files changed, 1 insertion(+), 161 deletions(-) delete mode 100644 map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp delete mode 100644 map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml index ac20f2e191b47..76026167f81a7 100644 --- a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml +++ b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml @@ -2,4 +2,3 @@ ros__parameters: enable_whole_load: true enable_partial_load: true - enable_differential_load: true diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 510f204689d22..8198cffcf17b1 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -7,11 +7,10 @@ This package provides the features of loading various maps. ### Feature `pointcloud_map_loader` provides pointcloud maps to the other Autoware nodes in various configurations. -Currently, it supports the following three types: +Currently, it supports the following two types: - Publish raw pointcloud map - Send partial pointcloud map loading via ROS 2 service -- Send differential pointcloud map loading via ROS 2 service #### Publish raw pointcloud map (ROS 2 topic) @@ -24,27 +23,18 @@ Here, we assume that the pointcloud maps are divided into grids. Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area. Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details. -#### Send differential pointcloud map (ROS 2 service) - -Here, we assume that the pointcloud maps are divided into grids. - -Given a query and set of map IDs, the node sends a set of pointcloud maps that overlap with the queried area and are not included in the set of map IDs. -Please see [the description of `GetDifferentialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getdifferentialpointcloudmapsrv) for details. - ### Parameters | Name | Type | Description | Default value | | :----------------------- | :----- | :-------------------------------------------------------------------------------- | :------------ | | enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | | enable_partial_load | bool | A flag to enable partial pointcloud map server | true | -| enable_differential_load | bool | A flag to enable differential pointcloud map server | true | | leaf_size | double | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | ### Interfaces - `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map - `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map -- `service/get_differential_pcd_map` (autoware_map_msgs/srv/GetDifferentialPointCloudMap) : Differential pointcloud map --- diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index ac20f2e191b47..76026167f81a7 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -2,4 +2,3 @@ ros__parameters: enable_whole_load: true enable_partial_load: true - enable_differential_load: true diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml index d9a7851cc9016..3cfc457c15d22 100644 --- a/map/map_loader/launch/pointcloud_map_loader.launch.xml +++ b/map/map_loader/launch/pointcloud_map_loader.launch.xml @@ -4,9 +4,7 @@ - - diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp deleted file mode 100644 index 89d2e4b2f0aa1..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// 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. - -#include "differential_map_loader_module.hpp" - -DifferentialMapLoaderModule::DifferentialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) -{ - get_differential_pcd_maps_service_ = node->create_service( - "service/get_differential_pcd_map", - std::bind( - &DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap, this, - std::placeholders::_1, std::placeholders::_2)); -} - -void DifferentialMapLoaderModule::differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area, const std::vector cached_ids, - GetDifferentialPointCloudMap::Response::SharedPtr & response) const -{ - // iterate over all the available pcd map grids - std::vector should_remove(static_cast(cached_ids.size()), true); - for (const auto & ele : all_pcd_file_metadata_dict_) { - std::string path = ele.first; - PCDFileMetadata metadata = ele.second; - - // assume that the map ID = map path (for now) - std::string map_id = path; - - // skip if the pcd file is not within the queried area - if (!isGridWithinQueriedArea(area, metadata)) continue; - - auto id_in_cached_list = std::find(cached_ids.begin(), cached_ids.end(), map_id); - if (id_in_cached_list != cached_ids.end()) { - int index = id_in_cached_list - cached_ids.begin(); - should_remove[index] = false; - } else { - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id); - response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); - } - } - - for (int i = 0; i < static_cast(cached_ids.size()); ++i) { - if (should_remove[i]) { - response->ids_to_remove.push_back(cached_ids[i]); - } - } - - RCLCPP_INFO_STREAM(logger_, "Finished diff area loading"); -} - -bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( - GetDifferentialPointCloudMap::Request::SharedPtr req, - GetDifferentialPointCloudMap::Response::SharedPtr res) -{ - auto area = req->area; - std::vector cached_ids = req->cached_ids; - differentialAreaLoad(area, cached_ids, res); - res->header.frame_id = "map"; - return true; -} - -autoware_map_msgs::msg::PointCloudMapCellWithID -DifferentialMapLoaderModule::loadPointCloudMapCellWithID( - const std::string path, const std::string map_id) const -{ - sensor_msgs::msg::PointCloud2 pcd; - if (pcl::io::loadPCDFile(path, pcd) == -1) { - RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); - } - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id; - pointcloud_map_cell_with_id.pointcloud = pcd; - pointcloud_map_cell_with_id.cell_id = map_id; - return pointcloud_map_cell_with_id; -} diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp deleted file mode 100644 index 81718a6791de9..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// 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. - -#ifndef POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ -#define POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ - -#include "utils.hpp" - -#include - -#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -class DifferentialMapLoaderModule -{ - using GetDifferentialPointCloudMap = autoware_map_msgs::srv::GetDifferentialPointCloudMap; - -public: - explicit DifferentialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict); - -private: - rclcpp::Logger logger_; - - std::map all_pcd_file_metadata_dict_; - rclcpp::Service::SharedPtr get_differential_pcd_maps_service_; - - bool onServiceGetDifferentialPointCloudMap( - GetDifferentialPointCloudMap::Request::SharedPtr req, - GetDifferentialPointCloudMap::Response::SharedPtr res); - void differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo area_info, const std::vector cached_ids, - GetDifferentialPointCloudMap::Response::SharedPtr & response) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( - const std::string path, const std::string map_id) const; -}; - -#endif // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ From ae70461ee3bd35c7c378c29bbd13247e09791443 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 14 Nov 2022 07:39:12 +0000 Subject: [PATCH 37/48] ci(pre-commit): autofix --- map/map_loader/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 8198cffcf17b1..aa8104abc9188 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -25,11 +25,11 @@ Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com ### Parameters -| Name | Type | Description | Default value | -| :----------------------- | :----- | :-------------------------------------------------------------------------------- | :------------ | -| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | -| enable_partial_load | bool | A flag to enable partial pointcloud map server | true | -| leaf_size | double | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | +| Name | Type | Description | Default value | +| :------------------ | :----- | :-------------------------------------------------------------------------------- | :------------ | +| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | +| enable_partial_load | bool | A flag to enable partial pointcloud map server | true | +| leaf_size | double | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | ### Interfaces From 480a32dc6f244b4a3d965c130d2233959296ee02 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 14 Nov 2022 16:43:36 +0900 Subject: [PATCH 38/48] removed differential_map_loader, debugged Signed-off-by: kminoda --- map/map_loader/CMakeLists.txt | 1 - .../pointcloud_map_loader_node.cpp | 10 +--------- .../pointcloud_map_loader_node.hpp | 3 --- 3 files changed, 1 insertion(+), 13 deletions(-) diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 98cf7a4751a1c..a0f347f16c8ae 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -10,7 +10,6 @@ ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/pointcloud_map_loader_node.cpp src/pointcloud_map_loader/pointcloud_map_loader_module.cpp src/pointcloud_map_loader/partial_map_loader_module.cpp - src/pointcloud_map_loader/differential_map_loader_module.cpp src/pointcloud_map_loader/utils.cpp ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 17fc93dde52ae..4ed3ea49e0744 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -51,25 +51,17 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt getPcdPaths(declare_parameter>("pcd_paths_or_directory")); bool enable_whole_load = declare_parameter("enable_whole_load"); bool enable_partial_load = declare_parameter("enable_partial_load"); - bool enable_differential_load = declare_parameter("enable_differential_load"); if (enable_whole_load) { std::string publisher_name = "output/pointcloud_map"; pcd_map_loader_ = std::make_unique(this, pcd_paths, publisher_name); } - if (enable_partial_load | enable_differential_load) { - pcd_metadata_dict_ = generatePCDMetadata(pcd_paths); - } - if (enable_partial_load) { + pcd_metadata_dict_ = generatePCDMetadata(pcd_paths); partial_map_loader_ = std::make_unique(this, pcd_metadata_dict_); } - if (enable_differential_load) { - differential_map_loader_ = - std::make_unique(this, pcd_metadata_dict_); - } } std::vector PointCloudMapLoaderNode::getPcdPaths( diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp index d321902131a81..7a297c900be12 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -15,7 +15,6 @@ #ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ #define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ -#include "differential_map_loader_module.hpp" #include "partial_map_loader_module.hpp" #include "pointcloud_map_loader_module.hpp" @@ -42,9 +41,7 @@ class PointCloudMapLoaderNode : public rclcpp::Node std::map pcd_metadata_dict_; std::unique_ptr pcd_map_loader_; - std::unique_ptr downsampled_pcd_map_loader_; std::unique_ptr partial_map_loader_; - std::unique_ptr differential_map_loader_; std::vector getPcdPaths( const std::vector & pcd_paths_or_directory) const; From 0e25a5f5df2889fd48058bd289738b7d7e1b911b Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 14 Nov 2022 07:45:37 +0000 Subject: [PATCH 39/48] ci(pre-commit): autofix --- .../src/pointcloud_map_loader/pointcloud_map_loader_node.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 4ed3ea49e0744..e6a93694737cf 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -61,7 +61,6 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt pcd_metadata_dict_ = generatePCDMetadata(pcd_paths); partial_map_loader_ = std::make_unique(this, pcd_metadata_dict_); } - } std::vector PointCloudMapLoaderNode::getPcdPaths( From 3bff6ecc2dd2ec2f46cc7a2d3029f5d5e401edc9 Mon Sep 17 00:00:00 2001 From: kminoda Date: Fri, 18 Nov 2022 18:35:42 +0900 Subject: [PATCH 40/48] removed leaf_size description Signed-off-by: kminoda --- map/map_loader/README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index aa8104abc9188..12633c6bfaf79 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -29,7 +29,6 @@ Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com | :------------------ | :----- | :-------------------------------------------------------------------------------- | :------------ | | enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | | enable_partial_load | bool | A flag to enable partial pointcloud map server | true | -| leaf_size | double | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | ### Interfaces From d7615577f1db3cf1866d74f355ef076440316334 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 18 Nov 2022 09:38:48 +0000 Subject: [PATCH 41/48] ci(pre-commit): autofix --- map/map_loader/README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 12633c6bfaf79..baafe9729387e 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -25,10 +25,10 @@ Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com ### Parameters -| Name | Type | Description | Default value | -| :------------------ | :----- | :-------------------------------------------------------------------------------- | :------------ | -| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | -| enable_partial_load | bool | A flag to enable partial pointcloud map server | true | +| Name | Type | Description | Default value | +| :------------------ | :--- | :--------------------------------------------- | :------------ | +| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | +| enable_partial_load | bool | A flag to enable partial pointcloud map server | true | ### Interfaces From 0cc613ee578104614f53b9876a7409282375ab90 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 21 Nov 2022 09:39:49 +0900 Subject: [PATCH 42/48] refactor sphereAndBoxOverlapExists Signed-off-by: kminoda --- .../src/pointcloud_map_loader/utils.cpp | 61 +++++++++++-------- 1 file changed, 37 insertions(+), 24 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index 8f65b0459b8e7..a51676220ac4b 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -17,32 +17,45 @@ #include bool sphereAndBoxOverlapExists( - const geometry_msgs::msg::Point position, const double radius, const pcl::PointXYZ position_min, - const pcl::PointXYZ position_max) + const geometry_msgs::msg::Point center, const double radius, const pcl::PointXYZ box_min_point, + const pcl::PointXYZ box_max_point) { - if ( - (position_min.x - radius <= position.x && position.x <= position_max.x + radius && - position_min.y <= position.y && position.y <= position_max.y && position_min.z <= position.z && - position.z <= position_max.z) || - (position_min.x <= position.x && position.x <= position_max.x && - position_min.y - radius <= position.y && position.y <= position_max.y + radius && - position_min.z <= position.z && position.z <= position_max.z) || - (position_min.x <= position.x && position.x <= position_max.x && position_min.y <= position.y && - position.y <= position_max.y && position_min.z - radius <= position.z && - position.z <= position_max.z + radius)) { + // Collision detection with x-axis plane + if (box_min_point.x - radius <= center.x && center.x <= box_max_point.x + radius && + box_min_point.y <= center.y && center.y <= box_max_point.y && + box_min_point.z <= center.z && center.z <= box_max_point.z) + { + return true; + } + + // Collision detection with y-axis plane + if (box_min_point.x <= center.x && center.x <= box_max_point.x && + box_min_point.y - radius <= center.y && center.y <= box_max_point.y + radius && + box_min_point.z <= center.z && center.z <= box_max_point.z) + { return true; } - double r2 = std::pow(radius, 2.0); - double minx2 = std::pow(position.x - position_min.x, 2.0); - double maxx2 = std::pow(position.x - position_max.x, 2.0); - double miny2 = std::pow(position.y - position_min.y, 2.0); - double maxy2 = std::pow(position.y - position_max.y, 2.0); - double minz2 = std::pow(position.z - position_min.z, 2.0); - double maxz2 = std::pow(position.z - position_max.z, 2.0); + + // Collision detection with z-axis plane + if (box_min_point.x <= center.x && center.x <= box_max_point.x && + box_min_point.y <= center.y && center.y <= box_max_point.y && + box_min_point.z - radius <= center.z && center.z <= box_max_point.z + radius) + { + return true; + } + + // Collision detection with box edges + const double dx0 = center.x - box_min_point.x; + const double dx1 = center.x - box_max_point.x; + const double dy0 = center.y - box_min_point.y; + const double dy1 = center.y - box_max_point.y; + const double dz0 = center.z - box_min_point.z; + const double dz1 = center.z - box_max_point.z; if ( - minx2 + miny2 + minz2 <= r2 || minx2 + miny2 + maxz2 <= r2 || minx2 + maxy2 + minz2 <= r2 || - minx2 + maxy2 + maxz2 <= r2 || maxx2 + miny2 + minz2 <= r2 || maxx2 + miny2 + maxz2 <= r2 || - maxx2 + maxy2 + minz2 <= r2 || maxx2 + maxy2 + maxz2 <= r2) { + std::hypot(dx0, dy0, dz0) <= radius || std::hypot(dx1, dy0, dz0) <= radius || + std::hypot(dx0, dy1, dz0) <= radius || std::hypot(dx0, dy0, dz1) <= radius || + std::hypot(dx0, dy1, dz1) <= radius || std::hypot(dx1, dy0, dz1) <= radius || + std::hypot(dx1, dy1, dz0) <= radius || std::hypot(dx1, dy1, dz1) <= radius) { return true; } return false; @@ -52,8 +65,8 @@ bool isGridWithinQueriedArea( const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata) { // Currently, the area load only supports spherical area - geometry_msgs::msg::Point position = area.center; + geometry_msgs::msg::Point center = area.center; double radius = area.radius; - bool res = sphereAndBoxOverlapExists(position, radius, metadata.min, metadata.max); + bool res = sphereAndBoxOverlapExists(center, radius, metadata.min, metadata.max); return res; } From 79622a781ee65ec57d6fa216b8b6a594270181c5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 21 Nov 2022 00:42:30 +0000 Subject: [PATCH 43/48] ci(pre-commit): autofix --- .../src/pointcloud_map_loader/utils.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index a51676220ac4b..8af8c0dfd8ec0 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -21,26 +21,26 @@ bool sphereAndBoxOverlapExists( const pcl::PointXYZ box_max_point) { // Collision detection with x-axis plane - if (box_min_point.x - radius <= center.x && center.x <= box_max_point.x + radius && - box_min_point.y <= center.y && center.y <= box_max_point.y && - box_min_point.z <= center.z && center.z <= box_max_point.z) - { + if ( + box_min_point.x - radius <= center.x && center.x <= box_max_point.x + radius && + box_min_point.y <= center.y && center.y <= box_max_point.y && box_min_point.z <= center.z && + center.z <= box_max_point.z) { return true; } // Collision detection with y-axis plane - if (box_min_point.x <= center.x && center.x <= box_max_point.x && - box_min_point.y - radius <= center.y && center.y <= box_max_point.y + radius && - box_min_point.z <= center.z && center.z <= box_max_point.z) - { + if ( + box_min_point.x <= center.x && center.x <= box_max_point.x && + box_min_point.y - radius <= center.y && center.y <= box_max_point.y + radius && + box_min_point.z <= center.z && center.z <= box_max_point.z) { return true; } // Collision detection with z-axis plane - if (box_min_point.x <= center.x && center.x <= box_max_point.x && - box_min_point.y <= center.y && center.y <= box_max_point.y && - box_min_point.z - radius <= center.z && center.z <= box_max_point.z + radius) - { + if ( + box_min_point.x <= center.x && center.x <= box_max_point.x && box_min_point.y <= center.y && + center.y <= box_max_point.y && box_min_point.z - radius <= center.z && + center.z <= box_max_point.z + radius) { return true; } From 103cec595af2850cea22fff65289db5796983ee5 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 22 Nov 2022 09:44:13 +0900 Subject: [PATCH 44/48] added test for sphereAndBoxOverlapExists Signed-off-by: kminoda --- map/map_loader/CMakeLists.txt | 13 ++ map/map_loader/package.xml | 1 + .../test/test_sphere_box_overlap.cpp | 143 ++++++++++++++++++ 3 files changed, 157 insertions(+) create mode 100644 map/map_loader/test/test_sphere_box_overlap.cpp diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index a0f347f16c8ae..14a9598f91112 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -51,6 +51,19 @@ if(BUILD_TESTING) test/data/ DESTINATION share/${PROJECT_NAME}/test/data/ ) + + function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gmock(${test_name} ${filepath}) + target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + target_link_libraries(${test_name} fmt pointcloud_map_loader_node) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) + endfunction() + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + add_testcase(test/test_sphere_box_overlap.cpp) endif() install(PROGRAMS diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index c71b4b32498a2..7a22a369744ab 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -30,6 +30,7 @@ tier4_autoware_utils visualization_msgs + ament_cmake_gmock ament_lint_auto autoware_lint_common ros_testing diff --git a/map/map_loader/test/test_sphere_box_overlap.cpp b/map/map_loader/test/test_sphere_box_overlap.cpp new file mode 100644 index 0000000000000..ab468f730eeaf --- /dev/null +++ b/map/map_loader/test/test_sphere_box_overlap.cpp @@ -0,0 +1,143 @@ +// Copyright 2022 The Autoware Contributors +// +// 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. + +#include "../src/pointcloud_map_loader/utils.hpp" + +#include + +TEST(SphereAndBoxOverlapExists, NoOverlap1) +{ + // Sphere: center = (5, 0, 0), radius = 4 - 0.1 + // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1) + geometry_msgs::msg::Point center; + center.x = 5.0; + center.y = 0.0; + center.z = 0.0; + + const double radius = 4.0 - 0.1; + + pcl::PointXYZ p_min; + p_min.x = -1.0; + p_min.y = -1.0; + p_min.z = -1.0; + + pcl::PointXYZ p_max; + p_max.x = 1.0; + p_max.y = 1.0; + p_max.z = 1.0; + + bool result = sphereAndBoxOverlapExists(center, radius, p_min, p_max); + EXPECT_FALSE(result); +} + +TEST(SphereAndBoxOverlapExists, NoOverlap2) +{ + // Sphere: center = (2, 2, 0), radius = sqrt(2) - 0.1 + // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1) + geometry_msgs::msg::Point center; + center.x = 2.0; + center.y = 2.0; + center.z = 0.0; + + const double radius = std::sqrt(2) - 0.1; + + pcl::PointXYZ p_min; + p_min.x = -1.0; + p_min.y = -1.0; + p_min.z = -1.0; + + pcl::PointXYZ p_max; + p_max.x = 1.0; + p_max.y = 1.0; + p_max.z = 1.0; + + bool result = sphereAndBoxOverlapExists(center, radius, p_min, p_max); + EXPECT_FALSE(result); +} + +TEST(SphereAndBoxOverlapExists, Overlap1) +{ + // Sphere: center = (0, 0, 0), radius = 0.5 + // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1) + geometry_msgs::msg::Point center; + center.x = 0.0; + center.y = 0.0; + center.z = 0.0; + + const double radius = 0.5; + + pcl::PointXYZ p_min; + p_min.x = -1.0; + p_min.y = -1.0; + p_min.z = -1.0; + + pcl::PointXYZ p_max; + p_max.x = 1.0; + p_max.y = 1.0; + p_max.z = 1.0; + + bool result = sphereAndBoxOverlapExists(center, radius, p_min, p_max); + EXPECT_TRUE(result); +} + +TEST(SphereAndBoxOverlapExists, Overlap2) +{ + // Sphere: center = (0, 0, 5), radius = 4 + 0.1 + // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1) + geometry_msgs::msg::Point center; + center.x = 0.0; + center.y = 0.0; + center.z = 5.0; + + const double radius = 4 + 0.1; + + pcl::PointXYZ p_min; + p_min.x = -1.0; + p_min.y = -1.0; + p_min.z = -1.0; + + pcl::PointXYZ p_max; + p_max.x = 1.0; + p_max.y = 1.0; + p_max.z = 1.0; + + bool result = sphereAndBoxOverlapExists(center, radius, p_min, p_max); + EXPECT_TRUE(result); +} + +TEST(SphereAndBoxOverlapExists, Overlap3) +{ + // Sphere: center = (2, 2, 2), radius = sqrt(3) + 0.1 + // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1) + geometry_msgs::msg::Point center; + center.x = 2.0; + center.y = 2.0; + center.z = 2.0; + + const double radius = std::sqrt(3) + 0.1; + + pcl::PointXYZ p_min; + p_min.x = -1.0; + p_min.y = -1.0; + p_min.z = -1.0; + + pcl::PointXYZ p_max; + p_max.x = 1.0; + p_max.y = 1.0; + p_max.z = 1.0; + + bool result = sphereAndBoxOverlapExists(center, radius, p_min, p_max); + EXPECT_TRUE(result); +} + From ca6276aaea983e22bff798c53a40bef8674341ff Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 22 Nov 2022 00:46:37 +0000 Subject: [PATCH 45/48] ci(pre-commit): autofix --- map/map_loader/test/test_sphere_box_overlap.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/map/map_loader/test/test_sphere_box_overlap.cpp b/map/map_loader/test/test_sphere_box_overlap.cpp index ab468f730eeaf..f7f76cc74007d 100644 --- a/map/map_loader/test/test_sphere_box_overlap.cpp +++ b/map/map_loader/test/test_sphere_box_overlap.cpp @@ -26,7 +26,7 @@ TEST(SphereAndBoxOverlapExists, NoOverlap1) center.z = 0.0; const double radius = 4.0 - 0.1; - + pcl::PointXYZ p_min; p_min.x = -1.0; p_min.y = -1.0; @@ -51,7 +51,7 @@ TEST(SphereAndBoxOverlapExists, NoOverlap2) center.z = 0.0; const double radius = std::sqrt(2) - 0.1; - + pcl::PointXYZ p_min; p_min.x = -1.0; p_min.y = -1.0; @@ -76,7 +76,7 @@ TEST(SphereAndBoxOverlapExists, Overlap1) center.z = 0.0; const double radius = 0.5; - + pcl::PointXYZ p_min; p_min.x = -1.0; p_min.y = -1.0; @@ -101,7 +101,7 @@ TEST(SphereAndBoxOverlapExists, Overlap2) center.z = 5.0; const double radius = 4 + 0.1; - + pcl::PointXYZ p_min; p_min.x = -1.0; p_min.y = -1.0; @@ -126,7 +126,7 @@ TEST(SphereAndBoxOverlapExists, Overlap3) center.z = 2.0; const double radius = std::sqrt(3) + 0.1; - + pcl::PointXYZ p_min; p_min.x = -1.0; p_min.y = -1.0; @@ -140,4 +140,3 @@ TEST(SphereAndBoxOverlapExists, Overlap3) bool result = sphereAndBoxOverlapExists(center, radius, p_min, p_max); EXPECT_TRUE(result); } - From 805a725dfa9a6e6e75f5cab52d0245e45a582238 Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 22 Nov 2022 09:50:54 +0900 Subject: [PATCH 46/48] remove downsample function for now Signed-off-by: kminoda --- .../pointcloud_map_loader_module.cpp | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index 6b1b2ea1534bc..e83b0df4d21c6 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -15,30 +15,12 @@ #include "pointcloud_map_loader_module.hpp" #include -#include #include #include #include #include -sensor_msgs::msg::PointCloud2 downsample( - const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) -{ - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(msg_input, *pcl_input); - pcl::VoxelGrid filter; - filter.setInputCloud(pcl_input); - filter.setLeafSize(leaf_size, leaf_size, leaf_size); - filter.filter(*pcl_output); - - sensor_msgs::msg::PointCloud2 msg_output; - pcl::toROSMsg(*pcl_output, msg_output); - msg_output.header = msg_input.header; - return msg_output; -} - PointcloudMapLoaderModule::PointcloudMapLoaderModule( rclcpp::Node * node, const std::vector & pcd_paths, const std::string publisher_name) : logger_(node->get_logger()) From 5e5cf77fa1259b82d12767dccbb2cd25480261eb Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 22 Nov 2022 09:55:24 +0900 Subject: [PATCH 47/48] remove fmt from target_link_libraries in test Signed-off-by: kminoda --- map/map_loader/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 14a9598f91112..20eac3e0a6dd8 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -57,7 +57,7 @@ if(BUILD_TESTING) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gmock(${test_name} ${filepath}) target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) - target_link_libraries(${test_name} fmt pointcloud_map_loader_node) + target_link_libraries(${test_name} pointcloud_map_loader_node) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() From 06a607139841ded5d621a8c45a935cb1b7260b0a Mon Sep 17 00:00:00 2001 From: kminoda Date: Tue, 22 Nov 2022 10:53:40 +0900 Subject: [PATCH 48/48] minor fix in cmakelists.txt Signed-off-by: kminoda --- map/map_loader/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 20eac3e0a6dd8..6bfcffde71d60 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -57,7 +57,7 @@ if(BUILD_TESTING) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gmock(${test_name} ${filepath}) target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) - target_link_libraries(${test_name} pointcloud_map_loader_node) + target_link_libraries(${test_name} ${${PROJECT_NAME}_LIBRARIES}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction()