From b76cbd8348805bf44545a28df19cabae884c99be Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 9 Nov 2022 15:26:46 +0900 Subject: [PATCH] refactor(map_loader): modularization (#2243) * refactor(map_loader): modularization Signed-off-by: kminoda * ci(pre-commit): autofix * simplified Signed-off-by: kminoda * removed autoware_msgs dependency (not yet necessary at this moment) Signed-off-by: kminoda * ci(pre-commit): autofix * remove unnecessary changes Signed-off-by: kminoda * pre-commit Signed-off-by: kminoda * ci(pre-commit): autofix * edit copyright Signed-off-by: kminoda Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- map/map_loader/CMakeLists.txt | 1 + .../map_loader/pointcloud_map_loader_node.hpp | 52 ------------- map/map_loader/package.xml | 1 + .../pointcloud_map_loader_module.cpp | 75 +++++++++++++++++++ .../pointcloud_map_loader_module.hpp | 41 ++++++++++ .../pointcloud_map_loader_node.cpp | 74 ++++-------------- .../pointcloud_map_loader_node.hpp | 45 +++++++++++ 7 files changed, 176 insertions(+), 113 deletions(-) delete mode 100644 map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp create mode 100644 map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp create mode 100644 map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp create mode 100644 map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index a59094efc7870..bd0155cdc610c 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -18,6 +18,7 @@ ament_auto_find_build_dependencies() 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 ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) diff --git a/map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp deleted file mode 100644 index 6bc55ef642706..0000000000000 --- a/map/map_loader/include/map_loader/pointcloud_map_loader_node.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// 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. - -/* - * 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 MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ -#define MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ - -#include - -#include - -#include -#include - -class PointCloudMapLoaderNode : public rclcpp::Node -{ -public: - explicit PointCloudMapLoaderNode(const rclcpp::NodeOptions & options); - -private: - rclcpp::Publisher::SharedPtr pub_pointcloud_map_; - - sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector & pcd_paths); -}; - -#endif // MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index d15a2ab1d6b07..4618d134844c0 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -11,6 +11,7 @@ ament_cmake_auto autoware_auto_mapping_msgs + fmt geometry_msgs lanelet2_extension libpcl-all-dev 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 new file mode 100644 index 0000000000000..0d75cd9dab01c --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -0,0 +1,75 @@ +// 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_loader_module.hpp" + +#include +#include +#include +#include + +#include +#include + +PointcloudMapLoaderModule::PointcloudMapLoaderModule( + rclcpp::Node * node, const std::vector & pcd_paths, const std::string publisher_name) +: 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); + + 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 PointcloudMapLoaderModule::loadPCDFiles( + const std::vector & pcd_paths) const +{ + sensor_msgs::msg::PointCloud2 whole_pcd; + sensor_msgs::msg::PointCloud2 partial_pcd; + + for (int i = 0; i < static_cast(pcd_paths.size()); ++i) { + auto & path = pcd_paths[i]; + if (i % 50 == 0) { + RCLCPP_INFO_STREAM( + logger_, + fmt::format("Load {} ({} out of {})", path, i + 1, static_cast(pcd_paths.size()))); + } + + if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { + RCLCPP_ERROR_STREAM(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.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_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp new file mode 100644 index 0000000000000..35e644efb8fe3 --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -0,0 +1,41 @@ +// 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__POINTCLOUD_MAP_LOADER_MODULE_HPP_ +#define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_MODULE_HPP_ + +#include + +#include + +#include + +#include +#include + +class PointcloudMapLoaderModule +{ +public: + explicit PointcloudMapLoaderModule( + rclcpp::Node * node, const std::vector & pcd_paths, + const std::string publisher_name); + +private: + rclcpp::Logger logger_; + rclcpp::Publisher::SharedPtr pub_pointcloud_map_; + + sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector & pcd_paths) const; +}; + +#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 99f46ab58fa71..d9b1336ca33a7 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 @@ -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,29 +12,14 @@ // 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. - */ - -#include "map_loader/pointcloud_map_loader_node.hpp" +#include "pointcloud_map_loader_node.hpp" #include #include #include #include +#include #include #include @@ -61,16 +46,17 @@ 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); - - 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 pcd_paths{}; + std::string publisher_name = "output/pointcloud_map"; + pcd_map_loader_ = std::make_unique(this, pcd_paths, publisher_name); +} +std::vector PointCloudMapLoaderNode::getPcdPaths( + const std::vector & pcd_paths_or_directory) const +{ + std::vector pcd_paths; for (const auto & p : pcd_paths_or_directory) { if (!fs::exists(p)) { RCLCPP_ERROR_STREAM(get_logger(), "invalid path: " << p); @@ -89,41 +75,7 @@ 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); -} - -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; + return pcd_paths; } #include 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 new file mode 100644 index 0000000000000..598665a84cd3f --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp @@ -0,0 +1,45 @@ +// 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__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ + +#include "pointcloud_map_loader_module.hpp" + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +class PointCloudMapLoaderNode : public rclcpp::Node +{ +public: + explicit PointCloudMapLoaderNode(const rclcpp::NodeOptions & options); + +private: + std::unique_ptr pcd_map_loader_; + + std::vector getPcdPaths( + const std::vector & pcd_paths_or_directory) const; +}; + +#endif // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_