diff --git a/perception/autoware_detection_by_tracker/package.xml b/perception/autoware_detection_by_tracker/package.xml
index 6f23baeefbd3e..651dfadb05a0a 100644
--- a/perception/autoware_detection_by_tracker/package.xml
+++ b/perception/autoware_detection_by_tracker/package.xml
@@ -24,6 +24,9 @@
tf2_geometry_msgs
tf2_ros
tier4_perception_msgs
+<<<<<<< HEAD
+=======
+>>>>>>> original/main
ament_lint_auto
autoware_lint_common
diff --git a/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp b/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp
index 9c7d801332bb8..4d635b9e836e9 100644
--- a/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp
+++ b/perception/autoware_detection_by_tracker/src/tracker/tracker_handler.cpp
@@ -23,12 +23,20 @@ namespace autoware::detection_by_tracker
{
void TrackerHandler::onTrackedObjects(
+<<<<<<< HEAD
+ const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr msg)
+=======
const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objects_msg)
+>>>>>>> original/main
{
constexpr size_t max_buffer_size = 10;
// Add tracked objects to buffer
+<<<<<<< HEAD
+ objects_buffer_.push_front(*msg);
+=======
objects_buffer_.push_front(*input_objects_msg);
+>>>>>>> original/main
// Remove old data
while (max_buffer_size < objects_buffer_.size()) {
diff --git a/perception/autoware_ground_segmentation/CMakeLists.txt b/perception/autoware_ground_segmentation/CMakeLists.txt
index 9cf256b9492bb..298e108b408e3 100644
--- a/perception/autoware_ground_segmentation/CMakeLists.txt
+++ b/perception/autoware_ground_segmentation/CMakeLists.txt
@@ -104,7 +104,11 @@ if(BUILD_TESTING)
test/test_ransac_ground_filter.cpp
)
target_link_libraries(test_ransac_ground_filter
+<<<<<<< HEAD:perception/ground_segmentation/CMakeLists.txt
+ ground_segmentation
+=======
autoware_ground_segmentation
+>>>>>>> original/main:perception/autoware_ground_segmentation/CMakeLists.txt
${YAML_CPP_LIBRARIES})
endif()
diff --git a/perception/autoware_ground_segmentation/launch/scan_ground_filter.launch.py b/perception/autoware_ground_segmentation/launch/scan_ground_filter.launch.py
index 6c85fb9302fdc..b1aa16747a719 100644
--- a/perception/autoware_ground_segmentation/launch/scan_ground_filter.launch.py
+++ b/perception/autoware_ground_segmentation/launch/scan_ground_filter.launch.py
@@ -35,7 +35,11 @@ def launch_setup(context, *args, **kwargs):
nodes = [
ComposableNode(
+<<<<<<< HEAD:perception/ground_segmentation/launch/scan_ground_filter.launch.py
+ package="ground_segmentation",
+=======
package="autoware_ground_segmentation",
+>>>>>>> original/main:perception/autoware_ground_segmentation/launch/scan_ground_filter.launch.py
plugin="autoware::ground_segmentation::ScanGroundFilterComponent",
name="scan_ground_filter",
remappings=[
diff --git a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp
index dd5ce96a858cd..774c19e866357 100644
--- a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp
+++ b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp
@@ -80,9 +80,16 @@ PlaneBasis getPlaneBasis(const Eigen::Vector3d & plane_normal)
return basis;
}
-using autoware::pointcloud_preprocessor::get_param;
-
-RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options)
+< < < < < < < < HEAD : perception / ground_segmentation / src / ransac_ground_filter /
+ node.cpp using pointcloud_preprocessor::get_param;
+== == == == using autoware::pointcloud_preprocessor::get_param;
+>>>>>>>> original /
+ main
+: perception /
+ autoware_ground_segmentation / src / ransac_ground_filter /
+ node.cpp
+
+ RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options)
: Filter("RANSACGroundFilter", options)
{
base_frame_ = declare_parameter("base_frame", "base_link");
diff --git a/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp
index 54de09b37ad96..9a2a2d5081a93 100644
--- a/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp
+++ b/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp
@@ -58,8 +58,16 @@
#include
#endif
+<<<<<<<<
+ HEAD : perception / ground_segmentation / src / ray_ground_filter / node.hpp
+#include "gencolors.hpp"
+#include "pointcloud_preprocessor/filter.hpp"
+ == == == ==
#include "autoware/pointcloud_preprocessor/filter.hpp"
#include "gencolors.hpp"
+ >>>>>>>> original /
+ main : perception / autoware_ground_segmentation / src / ray_ground_filter /
+ node.hpp
#include
#include
@@ -75,7 +83,7 @@
#include
#include
-namespace bg = boost::geometry;
+ namespace bg = boost::geometry;
using Point = bg::model::d2::point_xy;
using Polygon = bg::model::polygon;
@@ -184,26 +192,33 @@ class RayGroundFilterComponent : public autoware::pointcloud_preprocessor::Filte
* @param out_only_indices_cloud_ptr Resulting PointCloud with the indices kept
* @param out_removed_indices_cloud_ptr Resulting PointCloud with the indices removed
*/
- void ExtractPointsIndices(
- const PointCloud2::ConstSharedPtr in_cloud_ptr, const pcl::PointIndices & in_indices,
+ void ExtractPointsIndices(const PointCloud2::ConstSharedPtr in_cloud_ptr,
+ const pcl::PointIndices & in_indices, < < < < < < < < HEAD
+ : perception / ground_segmentation / src / ray_ground_filter /
+ node.hpp PointCloud2::SharedPtr out_only_indices_cloud_ptr,
+ PointCloud2::SharedPtr out_removed_indices_cloud_ptr);
+========
PointCloud2::SharedPtr ground_cloud_msg_ptr, PointCloud2::SharedPtr no_ground_cloud_msg_ptr);
+>>>>>>>> original / main : perception / autoware_ground_segmentation / src / ray_ground_filter /
+ node.hpp
- boost::optional calcPointVehicleIntersection(const Point & point);
+ boost::optional
+ calcPointVehicleIntersection(const Point & point);
- void setVehicleFootprint(
- const double min_x, const double max_x, const double min_y, const double max_y);
- void initializePointCloud2(
- const PointCloud2::ConstSharedPtr & in_cloud_ptr,
- const PointCloud2::SharedPtr & out_cloud_msg_ptr);
- /** \brief Parameter service callback result : needed to be hold */
- OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+void setVehicleFootprint(
+ const double min_x, const double max_x, const double min_y, const double max_y);
+void initializePointCloud2(
+ const PointCloud2::ConstSharedPtr & in_cloud_ptr,
+ const PointCloud2::SharedPtr & out_cloud_msg_ptr);
+/** \brief Parameter service callback result : needed to be hold */
+OnSetParametersCallbackHandle::SharedPtr set_param_res_;
- /** \brief Parameter service callback */
- rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p);
+/** \brief Parameter service callback */
+rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p);
public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- explicit RayGroundFilterComponent(const rclcpp::NodeOptions & options);
+EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+explicit RayGroundFilterComponent(const rclcpp::NodeOptions & options);
};
} // namespace autoware::ground_segmentation
diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp
index 2745606697a20..52fd0e71ca11d 100644
--- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp
+++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp
@@ -32,8 +32,11 @@ using autoware::universe_utils::normalizeDegree;
using autoware::universe_utils::normalizeRadian;
using autoware::vehicle_info_utils::VehicleInfoUtils;
-ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & options)
-: autoware::pointcloud_preprocessor::Filter("ScanGroundFilter", options)
+ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & options)<<<<<<<<
+ HEAD : perception / ground_segmentation / src / scan_ground_filter /
+ node.cpp : pointcloud_preprocessor::Filter("ScanGroundFilter", options) ==
+ == == == : autoware::pointcloud_preprocessor::Filter("ScanGroundFilter", options)>>>>>>>> original
+ / main : perception / autoware_ground_segmentation / src / scan_ground_filter / node.cpp
{
// set initial parameters
{
diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp
index ad5d0ea4b6bc6..3f6ad785fa519 100644
--- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp
+++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp
@@ -15,9 +15,18 @@
#ifndef SCAN_GROUND_FILTER__NODE_HPP_
#define SCAN_GROUND_FILTER__NODE_HPP_
+<<<<<<<<
+ HEAD : perception / ground_segmentation / src / scan_ground_filter / node.hpp
+#include "autoware_vehicle_info_utils/vehicle_info.hpp"
+#include "pointcloud_preprocessor/filter.hpp"
+#include "pointcloud_preprocessor/transform_info.hpp"
+ == == == ==
#include "autoware/pointcloud_preprocessor/filter.hpp"
#include "autoware/pointcloud_preprocessor/transform_info.hpp"
#include "autoware_vehicle_info_utils/vehicle_info.hpp"
+ >>>>>>>> original /
+ main : perception / autoware_ground_segmentation / src / scan_ground_filter /
+ node.hpp
#include
@@ -38,7 +47,7 @@
#include
#include
-class ScanGroundFilterTest;
+ class ScanGroundFilterTest;
namespace autoware::ground_segmentation
{
diff --git a/perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp b/perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp
index e18c166adb8b3..388812b3d49cd 100644
--- a/perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp
+++ b/perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp
@@ -134,8 +134,12 @@ void convertPCL2PointCloud2(
TEST_F(RansacGroundFilterTestSuite, TestCase1)
{
+<<<<<<< HEAD:perception/ground_segmentation/test/test_ransac_ground_filter.cpp
+ const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation");
+=======
const auto share_dir =
ament_index_cpp::get_package_share_directory("autoware_ground_segmentation");
+>>>>>>> original/main:perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp
const auto config_path = share_dir + "/config/ransac_ground_filter.param.yaml";
YAML::Node config = YAML::LoadFile(config_path);
auto params = config["/**"]["ros__parameters"];
diff --git a/perception/autoware_image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/autoware_image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md
index ec2bffca58462..a67b13df99818 100644
--- a/perception/autoware_image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md
+++ b/perception/autoware_image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md
@@ -32,8 +32,14 @@ The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud
### Core Parameters
+<<<<<<< HEAD:perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md
+{{ json_to_markdown("perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json") }}
+=======
+
{{ json_to_markdown("perception/autoware_image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json") }}
+> > > > > > > original/main:perception/autoware_image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md
+
## Assumptions / Known limits
+<<<<<<< HEAD:perception/shape_estimation/launch/shape_estimation.launch.xml
+
+=======
+>>>>>>> original/main:perception/autoware_shape_estimation/launch/shape_estimation.launch.xml
diff --git a/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.cpp b/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.cpp
new file mode 100644
index 0000000000000..e5a19245bc5b1
--- /dev/null
+++ b/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.cpp
@@ -0,0 +1,101 @@
+// Copyright 2021 Tier IV, Inc. 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 "node.hpp"
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+#include // To be replaced by std::filesystem in C++17
+
+#include
+#include
+#include
+
+namespace autoware::compare_map_segmentation
+{
+CompareElevationMapFilterComponent::CompareElevationMapFilterComponent(
+ const rclcpp::NodeOptions & options)
+: Filter("CompareElevationMapFilter", options)
+{
+ unsubscribe();
+ layer_name_ = declare_parameter("map_layer_name");
+ height_diff_thresh_ = declare_parameter("height_diff_thresh");
+ map_frame_ = declare_parameter("map_frame");
+
+ rclcpp::QoS durable_qos{1};
+ durable_qos.transient_local();
+
+ sub_map_ = this->create_subscription(
+ "input/elevation_map", durable_qos,
+ std::bind(
+ &CompareElevationMapFilterComponent::elevationMapCallback, this, std::placeholders::_1));
+}
+
+void CompareElevationMapFilterComponent::elevationMapCallback(
+ const grid_map_msgs::msg::GridMap::ConstSharedPtr elevation_map)
+{
+ grid_map::GridMapRosConverter::fromMessage(*elevation_map, elevation_map_);
+ elevation_map_data_ = elevation_map_.get(layer_name_);
+
+ const float min_value = elevation_map_.get(layer_name_).minCoeffOfFinites();
+ const float max_value = elevation_map_.get(layer_name_).maxCoeffOfFinites();
+ grid_map::GridMapCvConverter::toImage(
+ elevation_map_, layer_name_, CV_16UC1, min_value, max_value, elevation_image_);
+ subscribe();
+}
+
+void CompareElevationMapFilterComponent::filter(
+ const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
+ PointCloud2 & output)
+{
+ pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud);
+ pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud);
+ std::string output_frame = map_frame_;
+
+ output_frame = elevation_map_.getFrameId();
+ elevation_map_.setTimestamp(input->header.stamp.nanosec);
+ pcl::fromROSMsg(*input, *pcl_input);
+ pcl_output->points.reserve(pcl_input->points.size());
+ for (const auto & point : pcl_input->points) {
+ if (elevation_map_.isInside(grid_map::Position(point.x, point.y))) {
+ float elevation_value = elevation_map_.atPosition(
+ layer_name_, grid_map::Position(point.x, point.y),
+ grid_map::InterpolationMethods::INTER_LINEAR);
+ const float height_diff = point.z - elevation_value;
+ if (height_diff > height_diff_thresh_) {
+ pcl_output->points.push_back(point);
+ }
+ }
+ }
+
+ pcl::toROSMsg(*pcl_output, output);
+ output.header.stamp = input->header.stamp;
+ output.header.frame_id = output_frame;
+}
+} // namespace autoware::compare_map_segmentation
+
+#include
+RCLCPP_COMPONENTS_REGISTER_NODE(
+ autoware::compare_map_segmentation::CompareElevationMapFilterComponent)
diff --git a/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.hpp b/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.hpp
new file mode 100644
index 0000000000000..ceddaa67ac2cd
--- /dev/null
+++ b/perception/compare_map_segmentation/src/compare_elevation_map_filter/node.hpp
@@ -0,0 +1,61 @@
+// Copyright 2021 Tier IV, Inc. 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 COMPARE_ELEVATION_MAP_FILTER__NODE_HPP_
+#define COMPARE_ELEVATION_MAP_FILTER__NODE_HPP_
+
+#include "autoware/pointcloud_preprocessor/filter.hpp"
+
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+namespace autoware::compare_map_segmentation
+{
+class CompareElevationMapFilterComponent : public autoware::pointcloud_preprocessor::Filter
+{
+protected:
+ void filter(
+ const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;
+
+private:
+ rclcpp::Subscription::SharedPtr sub_map_;
+
+ rclcpp::Publisher::SharedPtr pub_filtered_cloud_;
+ grid_map::GridMap elevation_map_;
+ cv::Mat elevation_image_;
+ grid_map::Matrix elevation_map_data_;
+ std::string layer_name_;
+ std::string map_frame_;
+ double height_diff_thresh_;
+
+ void setVerbosityLevelToDebugIfFlagSet();
+ void processPointcloud(grid_map::GridMapPclLoader * gridMapPclLoader);
+ void elevationMapCallback(const grid_map_msgs::msg::GridMap::ConstSharedPtr elevation_map);
+
+public:
+ PCL_MAKE_ALIGNED_OPERATOR_NEW
+ explicit CompareElevationMapFilterComponent(const rclcpp::NodeOptions & options);
+};
+} // namespace autoware::compare_map_segmentation
+
+#endif // COMPARE_ELEVATION_MAP_FILTER__NODE_HPP_
diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp
new file mode 100644
index 0000000000000..53183cd7dc3f0
--- /dev/null
+++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp
@@ -0,0 +1,186 @@
+// 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.
+
+#include "node.hpp"
+
+#include "autoware/universe_utils/ros/debug_publisher.hpp"
+#include "autoware/universe_utils/system/stop_watch.hpp"
+
+#include
+#include
+#include
+
+#include
+
+namespace autoware::compare_map_segmentation
+{
+
+void DistanceBasedStaticMapLoader::onMapCallback(
+ const sensor_msgs::msg::PointCloud2::ConstSharedPtr map)
+{
+ pcl::PointCloud map_pcl;
+ pcl::fromROSMsg(*map, map_pcl);
+ const auto map_pcl_ptr = pcl::make_shared>(map_pcl);
+
+ (*mutex_ptr_).lock();
+ map_ptr_ = map_pcl_ptr;
+ *tf_map_input_frame_ = map_ptr_->header.frame_id;
+ if (!tree_) {
+ if (map_ptr_->isOrganized()) {
+ tree_.reset(new pcl::search::OrganizedNeighbor());
+ } else {
+ tree_.reset(new pcl::search::KdTree(false));
+ }
+ }
+ tree_->setInputCloud(map_ptr_);
+
+ (*mutex_ptr_).unlock();
+}
+
+bool DistanceBasedStaticMapLoader::is_close_to_map(
+ const pcl::PointXYZ & point, const double distance_threshold)
+{
+ if (map_ptr_ == NULL) {
+ return false;
+ }
+ if (tree_ == NULL) {
+ return false;
+ }
+
+ std::vector nn_indices(1);
+ std::vector nn_distances(1);
+ if (!isFinite(point)) {
+ return false;
+ }
+ if (!tree_->nearestKSearch(point, 1, nn_indices, nn_distances)) {
+ return false;
+ }
+ if (nn_distances[0] > distance_threshold) {
+ return false;
+ }
+ return true;
+}
+
+bool DistanceBasedDynamicMapLoader::is_close_to_map(
+ const pcl::PointXYZ & point, const double distance_threshold)
+{
+ if (current_voxel_grid_dict_.size() == 0) {
+ return false;
+ }
+ if (!isFinite(point)) {
+ return false;
+ }
+
+ const int map_grid_index = static_cast(
+ std::floor((point.x - origin_x_) / map_grid_size_x_) +
+ map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_));
+
+ if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) {
+ return false;
+ }
+ if (current_voxel_grid_array_.at(map_grid_index) != NULL) {
+ if (current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree == NULL) {
+ return false;
+ }
+ std::vector nn_indices(1);
+ std::vector nn_distances(1);
+ if (!current_voxel_grid_array_.at(map_grid_index)
+ ->map_cell_kdtree->nearestKSearch(point, 1, nn_indices, nn_distances)) {
+ return false;
+ }
+ if (nn_distances[0] <= distance_threshold) {
+ return true;
+ }
+ }
+ return false;
+}
+
+DistanceBasedCompareMapFilterComponent::DistanceBasedCompareMapFilterComponent(
+ const rclcpp::NodeOptions & options)
+: Filter("DistanceBasedCompareMapFilter", options)
+{
+ // initialize debug tool
+ {
+ using autoware::universe_utils::DebugPublisher;
+ using autoware::universe_utils::StopWatch;
+ stop_watch_ptr_ = std::make_unique>();
+ debug_publisher_ = std::make_unique(this, "distance_based_compare_map_filter");
+ stop_watch_ptr_->tic("cyclic_time");
+ stop_watch_ptr_->tic("processing_time");
+ }
+
+ distance_threshold_ = declare_parameter("distance_threshold");
+ bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading");
+ if (use_dynamic_map_loading) {
+ rclcpp::CallbackGroup::SharedPtr main_callback_group;
+ main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+ distance_based_map_loader_ = std::make_unique(
+ this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group);
+ } else {
+ distance_based_map_loader_ = std::make_unique(
+ this, distance_threshold_, &tf_input_frame_, &mutex_);
+ }
+}
+
+void DistanceBasedCompareMapFilterComponent::filter(
+ const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
+ PointCloud2 & output)
+{
+ std::scoped_lock lock(mutex_);
+ stop_watch_ptr_->toc("processing_time", true);
+
+ int point_step = input->point_step;
+ int offset_x = input->fields[pcl::getFieldIndex(*input, "x")].offset;
+ int offset_y = input->fields[pcl::getFieldIndex(*input, "y")].offset;
+ int offset_z = input->fields[pcl::getFieldIndex(*input, "z")].offset;
+
+ output.data.resize(input->data.size());
+ output.point_step = point_step;
+ size_t output_size = 0;
+ for (size_t global_offset = 0; global_offset < input->data.size(); global_offset += point_step) {
+ pcl::PointXYZ point{};
+ std::memcpy(&point.x, &input->data[global_offset + offset_x], sizeof(float));
+ std::memcpy(&point.y, &input->data[global_offset + offset_y], sizeof(float));
+ std::memcpy(&point.z, &input->data[global_offset + offset_z], sizeof(float));
+ if (distance_based_map_loader_->is_close_to_map(point, distance_threshold_)) {
+ continue;
+ }
+ std::memcpy(&output.data[output_size], &input->data[global_offset], point_step);
+ output_size += point_step;
+ }
+ output.header = input->header;
+ output.fields = input->fields;
+ output.data.resize(output_size);
+ output.height = input->height;
+ output.width = output_size / point_step / output.height;
+ output.row_step = output_size / output.height;
+ output.is_bigendian = input->is_bigendian;
+ output.is_dense = input->is_dense;
+
+ // add processing time for debug
+ if (debug_publisher_) {
+ const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
+ const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
+ debug_publisher_->publish(
+ "debug/cyclic_time_ms", cyclic_time_ms);
+ debug_publisher_->publish(
+ "debug/processing_time_ms", processing_time_ms);
+ }
+}
+
+} // namespace autoware::compare_map_segmentation
+
+#include
+RCLCPP_COMPONENTS_REGISTER_NODE(
+ autoware::compare_map_segmentation::DistanceBasedCompareMapFilterComponent)
diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp
new file mode 100644
index 0000000000000..3ff6dc3a32c7b
--- /dev/null
+++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter/node.hpp
@@ -0,0 +1,128 @@
+// 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.
+
+#ifndef DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_
+#define DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_
+
+#include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp"
+<<<<<<<<
+ HEAD : perception / compare_map_segmentation / src / distance_based_compare_map_filter / node.hpp
+#include "pointcloud_preprocessor/filter.hpp"
+ == == == ==
+#include "autoware/pointcloud_preprocessor/filter.hpp"
+ >>>>>>>> original /
+ main : perception / autoware_compare_map_segmentation / src / distance_based_compare_map_filter /
+ node.hpp
+
+#include // for pcl::isFinite
+#include
+#include
+
+#include
+#include
+#include
+
+ namespace autoware::compare_map_segmentation
+{
+ typedef typename pcl::Filter::PointCloud PointCloud;
+ typedef typename PointCloud::Ptr PointCloudPtr;
+ typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+ class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
+ {
+ private:
+ PointCloudConstPtr map_ptr_;
+ pcl::search::Search::Ptr tree_;
+
+ public:
+ DistanceBasedStaticMapLoader(
+ rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex)
+ : VoxelGridStaticMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex)
+ {
+ RCLCPP_INFO(logger_, "DistanceBasedStaticMapLoader initialized.\n");
+ }
+
+ void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map) override;
+ bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
+ };
+
+ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
+ {
+ public:
+ DistanceBasedDynamicMapLoader(
+ rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex,
+ rclcpp::CallbackGroup::SharedPtr main_callback_group)
+ : VoxelGridDynamicMapLoader(
+ node, leaf_size, 1.0, tf_map_input_frame, mutex, main_callback_group)
+ {
+ RCLCPP_INFO(logger_, "DistanceBasedDynamicMapLoader initialized.\n");
+ }
+ bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
+
+ inline void addMapCellAndFilter(
+ const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) override
+ {
+ map_grid_size_x_ = map_cell_to_add.metadata.max_x - map_cell_to_add.metadata.min_x;
+ map_grid_size_y_ = map_cell_to_add.metadata.max_y - map_cell_to_add.metadata.min_y;
+
+ pcl::PointCloud map_cell_pc_tmp;
+ pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp);
+
+ auto map_cell_voxel_input_tmp_ptr =
+ std::make_shared>(map_cell_pc_tmp);
+
+ MapGridVoxelInfo current_voxel_grid_list_item;
+ current_voxel_grid_list_item.min_b_x = map_cell_to_add.metadata.min_x;
+ current_voxel_grid_list_item.min_b_y = map_cell_to_add.metadata.min_y;
+ current_voxel_grid_list_item.max_b_x = map_cell_to_add.metadata.max_x;
+ current_voxel_grid_list_item.max_b_y = map_cell_to_add.metadata.max_y;
+
+ // add kdtree
+ pcl::search::Search::Ptr tree_tmp;
+ if (!tree_tmp) {
+ if (map_cell_voxel_input_tmp_ptr->isOrganized()) {
+ tree_tmp.reset(new pcl::search::OrganizedNeighbor());
+ } else {
+ tree_tmp.reset(new pcl::search::KdTree(false));
+ }
+ }
+ tree_tmp->setInputCloud(map_cell_voxel_input_tmp_ptr);
+ current_voxel_grid_list_item.map_cell_kdtree = tree_tmp;
+
+ // add
+ (*mutex_ptr_).lock();
+ current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item});
+ (*mutex_ptr_).unlock();
+ }
+ };
+
+ class DistanceBasedCompareMapFilterComponent : public autoware::pointcloud_preprocessor::Filter
+ {
+ protected:
+ virtual void filter(
+ const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
+
+ private:
+ double distance_threshold_;
+ std::unique_ptr distance_based_map_loader_;
+
+ public:
+ PCL_MAKE_ALIGNED_OPERATOR_NEW
+ explicit DistanceBasedCompareMapFilterComponent(const rclcpp::NodeOptions & options);
+ };
+} // namespace autoware::compare_map_segmentation
+
+// clang-format off
+#endif // DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT
+// clang-format on
diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp
new file mode 100644
index 0000000000000..8bc22f8c31184
--- /dev/null
+++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp
@@ -0,0 +1,155 @@
+// 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.
+
+#include "node.hpp"
+
+#include "autoware/universe_utils/ros/debug_publisher.hpp"
+#include "autoware/universe_utils/system/stop_watch.hpp"
+
+#include
+#include
+#include
+
+#include
+
+namespace autoware::compare_map_segmentation
+{
+
+bool VoxelBasedApproximateStaticMapLoader::is_close_to_map(
+ const pcl::PointXYZ & point, [[maybe_unused]] const double distance_threshold)
+{
+ if (voxel_map_ptr_ == NULL) {
+ return false;
+ }
+ const int index =
+ voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z));
+ if (index == -1) {
+ return false;
+ } else {
+ return true;
+ }
+}
+
+bool VoxelBasedApproximateDynamicMapLoader::is_close_to_map(
+ const pcl::PointXYZ & point, [[maybe_unused]] const double distance_threshold)
+{
+ if (current_voxel_grid_dict_.size() == 0) {
+ return false;
+ }
+
+ const int map_grid_index = static_cast(
+ std::floor((point.x - origin_x_) / map_grid_size_x_) +
+ map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_));
+
+ if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) {
+ return false;
+ }
+ if (current_voxel_grid_array_.at(map_grid_index) != NULL) {
+ const int index = current_voxel_grid_array_.at(map_grid_index)
+ ->map_cell_voxel_grid.getCentroidIndexAt(
+ current_voxel_grid_array_.at(map_grid_index)
+ ->map_cell_voxel_grid.getGridCoordinates(point.x, point.y, point.z));
+ if (index == -1) {
+ return false;
+ } else {
+ return true;
+ }
+ }
+ return false;
+}
+
+VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapFilterComponent(
+ const rclcpp::NodeOptions & options)
+: Filter("VoxelBasedApproximateCompareMapFilter", options)
+{
+ // initialize debug tool
+ {
+ using autoware::universe_utils::DebugPublisher;
+ using autoware::universe_utils::StopWatch;
+ stop_watch_ptr_ = std::make_unique>();
+ debug_publisher_ =
+ std::make_unique(this, "voxel_based_approximate_compare_map_filter");
+ stop_watch_ptr_->tic("cyclic_time");
+ stop_watch_ptr_->tic("processing_time");
+ }
+
+ distance_threshold_ = declare_parameter("distance_threshold");
+ bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading");
+ double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis");
+ if (downsize_ratio_z_axis <= 0.0) {
+ RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive");
+ return;
+ }
+ if (use_dynamic_map_loading) {
+ rclcpp::CallbackGroup::SharedPtr main_callback_group;
+ main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+ voxel_based_approximate_map_loader_ = std::make_unique(
+ this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_,
+ main_callback_group);
+ } else {
+ voxel_based_approximate_map_loader_ = std::make_unique(
+ this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_);
+ }
+}
+
+void VoxelBasedApproximateCompareMapFilterComponent::filter(
+ const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
+ PointCloud2 & output)
+{
+ std::scoped_lock lock(mutex_);
+ stop_watch_ptr_->toc("processing_time", true);
+ int point_step = input->point_step;
+ int offset_x = input->fields[pcl::getFieldIndex(*input, "x")].offset;
+ int offset_y = input->fields[pcl::getFieldIndex(*input, "y")].offset;
+ int offset_z = input->fields[pcl::getFieldIndex(*input, "z")].offset;
+
+ output.data.resize(input->data.size());
+ output.point_step = point_step;
+ size_t output_size = 0;
+ for (size_t global_offset = 0; global_offset < input->data.size(); global_offset += point_step) {
+ pcl::PointXYZ point{};
+ std::memcpy(&point.x, &input->data[global_offset + offset_x], sizeof(float));
+ std::memcpy(&point.y, &input->data[global_offset + offset_y], sizeof(float));
+ std::memcpy(&point.z, &input->data[global_offset + offset_z], sizeof(float));
+ if (voxel_based_approximate_map_loader_->is_close_to_map(point, distance_threshold_)) {
+ continue;
+ }
+ std::memcpy(&output.data[output_size], &input->data[global_offset], point_step);
+ output_size += point_step;
+ }
+ output.header = input->header;
+ output.fields = input->fields;
+ output.data.resize(output_size);
+ output.height = input->height;
+ output.width = output_size / point_step / output.height;
+ output.row_step = output_size / output.height;
+ output.is_bigendian = input->is_bigendian;
+ output.is_dense = input->is_dense;
+
+ // add processing time for debug
+ if (debug_publisher_) {
+ const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
+ const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
+ debug_publisher_->publish(
+ "debug/cyclic_time_ms", cyclic_time_ms);
+ debug_publisher_->publish(
+ "debug/processing_time_ms", processing_time_ms);
+ }
+}
+
+} // namespace autoware::compare_map_segmentation
+
+#include
+RCLCPP_COMPONENTS_REGISTER_NODE(
+ autoware::compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent)
diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp
new file mode 100644
index 0000000000000..653f6bf821f69
--- /dev/null
+++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.hpp
@@ -0,0 +1,86 @@
+// 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.
+
+#ifndef VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT
+#define VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT
+
+#include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp"
+<<<<<<<<
+ HEAD : perception / compare_map_segmentation / src / voxel_based_approximate_compare_map_filter /
+ node.hpp
+#include "pointcloud_preprocessor/filter.hpp"
+ == == == ==
+#include "autoware/pointcloud_preprocessor/filter.hpp"
+ >>>>>>>> original /
+ main : perception / autoware_compare_map_segmentation / src /
+ voxel_based_approximate_compare_map_filter /
+ node.hpp
+
+#include
+#include
+
+#include
+#include
+#include
+
+ namespace autoware::compare_map_segmentation
+{
+ class VoxelBasedApproximateStaticMapLoader : public VoxelGridStaticMapLoader
+ {
+ public:
+ explicit VoxelBasedApproximateStaticMapLoader(
+ rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
+ std::string * tf_map_input_frame, std::mutex * mutex)
+ : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
+ {
+ RCLCPP_INFO(logger_, "VoxelBasedApproximateStaticMapLoader initialized.\n");
+ }
+ bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
+ };
+
+ class VoxelBasedApproximateDynamicMapLoader : public VoxelGridDynamicMapLoader
+ {
+ public:
+ VoxelBasedApproximateDynamicMapLoader(
+ rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
+ std::string * tf_map_input_frame, std::mutex * mutex,
+ rclcpp::CallbackGroup::SharedPtr main_callback_group)
+ : VoxelGridDynamicMapLoader(
+ node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group)
+ {
+ RCLCPP_INFO(logger_, "VoxelBasedApproximateDynamicMapLoader initialized.\n");
+ }
+ bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) override;
+ };
+
+ class VoxelBasedApproximateCompareMapFilterComponent
+ : public autoware::pointcloud_preprocessor::Filter
+ {
+ protected:
+ virtual void filter(
+ const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
+
+ private:
+ double distance_threshold_;
+ std::unique_ptr voxel_based_approximate_map_loader_;
+
+ public:
+ PCL_MAKE_ALIGNED_OPERATOR_NEW
+ explicit VoxelBasedApproximateCompareMapFilterComponent(const rclcpp::NodeOptions & options);
+ };
+} // namespace autoware::compare_map_segmentation
+
+// clang-format off
+#endif // VOXEL_BASED_APPROXIMATE_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT
+// clang-format on
diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp
new file mode 100644
index 0000000000000..a3eb866d77e31
--- /dev/null
+++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp
@@ -0,0 +1,116 @@
+// 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.
+
+#include "node.hpp"
+
+#include "autoware/universe_utils/ros/debug_publisher.hpp"
+#include "autoware/universe_utils/system/stop_watch.hpp"
+
+#include
+#include
+#include
+
+#include
+#include
+
+namespace autoware::compare_map_segmentation
+{
+using autoware::pointcloud_preprocessor::get_param;
+
+VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent(
+ const rclcpp::NodeOptions & options)
+: Filter("VoxelBasedCompareMapFilter", options)
+{
+ // initialize debug tool
+ {
+ using autoware::universe_utils::DebugPublisher;
+ using autoware::universe_utils::StopWatch;
+ stop_watch_ptr_ = std::make_unique>();
+ debug_publisher_ = std::make_unique(this, "voxel_based_compare_map_filter");
+ stop_watch_ptr_->tic("cyclic_time");
+ stop_watch_ptr_->tic("processing_time");
+ }
+
+ distance_threshold_ = declare_parameter("distance_threshold");
+ bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading");
+ double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis");
+ if (downsize_ratio_z_axis <= 0.0) {
+ RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive");
+ return;
+ }
+ set_map_in_voxel_grid_ = false;
+ if (use_dynamic_map_loading) {
+ rclcpp::CallbackGroup::SharedPtr main_callback_group;
+ main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+ voxel_grid_map_loader_ = std::make_unique(
+ this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_,
+ main_callback_group);
+ } else {
+ voxel_grid_map_loader_ = std::make_unique(
+ this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_);
+ }
+ tf_input_frame_ = *(voxel_grid_map_loader_->tf_map_input_frame_);
+ RCLCPP_INFO(this->get_logger(), "tf_map_input_frame: %s", tf_input_frame_.c_str());
+}
+
+void VoxelBasedCompareMapFilterComponent::filter(
+ const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
+ PointCloud2 & output)
+{
+ std::scoped_lock lock(mutex_);
+ stop_watch_ptr_->toc("processing_time", true);
+ int point_step = input->point_step;
+ int offset_x = input->fields[pcl::getFieldIndex(*input, "x")].offset;
+ int offset_y = input->fields[pcl::getFieldIndex(*input, "y")].offset;
+ int offset_z = input->fields[pcl::getFieldIndex(*input, "z")].offset;
+
+ output.data.resize(input->data.size());
+ output.point_step = point_step;
+ size_t output_size = 0;
+ for (size_t global_offset = 0; global_offset < input->data.size(); global_offset += point_step) {
+ pcl::PointXYZ point{};
+ std::memcpy(&point.x, &input->data[global_offset + offset_x], sizeof(float));
+ std::memcpy(&point.y, &input->data[global_offset + offset_y], sizeof(float));
+ std::memcpy(&point.z, &input->data[global_offset + offset_z], sizeof(float));
+ if (voxel_grid_map_loader_->is_close_to_map(point, distance_threshold_)) {
+ continue;
+ }
+ std::memcpy(&output.data[output_size], &input->data[global_offset], point_step);
+ output_size += point_step;
+ }
+ output.header = input->header;
+ output.fields = input->fields;
+ output.data.resize(output_size);
+ output.height = input->height;
+ output.width = output_size / point_step / output.height;
+ output.row_step = output_size / output.height;
+ output.is_bigendian = input->is_bigendian;
+ output.is_dense = input->is_dense;
+
+ // add processing time for debug
+ if (debug_publisher_) {
+ const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
+ const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
+ debug_publisher_->publish(
+ "debug/cyclic_time_ms", cyclic_time_ms);
+ debug_publisher_->publish(
+ "debug/processing_time_ms", processing_time_ms);
+ }
+}
+
+} // namespace autoware::compare_map_segmentation
+
+#include
+RCLCPP_COMPONENTS_REGISTER_NODE(
+ autoware::compare_map_segmentation::VoxelBasedCompareMapFilterComponent)
diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp
new file mode 100644
index 0000000000000..db62669c87094
--- /dev/null
+++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp
@@ -0,0 +1,57 @@
+// 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.
+
+#ifndef VOXEL_BASED_COMPARE_MAP_FILTER__NODE_HPP_
+#define VOXEL_BASED_COMPARE_MAP_FILTER__NODE_HPP_
+
+#include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp"
+<<<<<<<<
+ HEAD : perception / compare_map_segmentation / src / voxel_based_compare_map_filter / node.hpp
+#include "pointcloud_preprocessor/filter.hpp"
+ == == == ==
+#include "autoware/pointcloud_preprocessor/filter.hpp"
+ >>>>>>>> original /
+ main : perception / autoware_compare_map_segmentation / src / voxel_based_compare_map_filter /
+ node.hpp
+
+#include
+#include
+
+#include
+#include
+
+ namespace autoware::compare_map_segmentation
+{
+ class VoxelBasedCompareMapFilterComponent : public autoware::pointcloud_preprocessor::Filter
+ {
+ protected:
+ virtual void filter(
+ const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
+
+ private:
+ // pcl::SegmentDifferences impl_;
+ std::unique_ptr voxel_grid_map_loader_;
+ rclcpp::Subscription::SharedPtr sub_map_;
+ double distance_threshold_;
+ bool set_map_in_voxel_grid_;
+
+ bool dynamic_map_load_enable_;
+
+ public:
+ PCL_MAKE_ALIGNED_OPERATOR_NEW
+ explicit VoxelBasedCompareMapFilterComponent(const rclcpp::NodeOptions & options);
+ };
+} // namespace autoware::compare_map_segmentation
+
+#endif // VOXEL_BASED_COMPARE_MAP_FILTER__NODE_HPP_
diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp
new file mode 100644
index 0000000000000..7fdeb5d8ea163
--- /dev/null
+++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp
@@ -0,0 +1,184 @@
+// 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.
+
+#include "node.hpp"
+
+#include "autoware/universe_utils/ros/debug_publisher.hpp"
+#include "autoware/universe_utils/system/stop_watch.hpp"
+
+#include
+#include
+#include
+
+#include
+
+namespace autoware::compare_map_segmentation
+{
+
+void VoxelDistanceBasedStaticMapLoader::onMapCallback(
+ const sensor_msgs::msg::PointCloud2::ConstSharedPtr map)
+{
+ pcl::PointCloud map_pcl;
+ pcl::fromROSMsg(*map, map_pcl);
+ const auto map_pcl_ptr = pcl::make_shared>(map_pcl);
+
+ (*mutex_ptr_).lock();
+ map_ptr_ = map_pcl_ptr;
+ *tf_map_input_frame_ = map_ptr_->header.frame_id;
+ // voxel
+ voxel_map_ptr_.reset(new pcl::PointCloud);
+ voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_);
+ voxel_grid_.setInputCloud(map_pcl_ptr);
+ voxel_grid_.setSaveLeafLayout(true);
+ voxel_grid_.filter(*voxel_map_ptr_);
+ // kdtree
+ map_ptr_ = map_pcl_ptr;
+
+ if (!tree_) {
+ if (map_ptr_->isOrganized()) {
+ tree_.reset(new pcl::search::OrganizedNeighbor());
+ } else {
+ tree_.reset(new pcl::search::KdTree(false));
+ }
+ }
+ tree_->setInputCloud(map_ptr_);
+ (*mutex_ptr_).unlock();
+}
+
+bool VoxelDistanceBasedStaticMapLoader::is_close_to_map(
+ const pcl::PointXYZ & point, const double distance_threshold)
+{
+ if (voxel_map_ptr_ == NULL) {
+ return false;
+ }
+ if (map_ptr_ == NULL) {
+ return false;
+ }
+ if (tree_ == NULL) {
+ return false;
+ }
+ if (is_close_to_neighbor_voxels(point, distance_threshold, voxel_grid_, tree_)) {
+ return true;
+ }
+ return false;
+}
+
+bool VoxelDistanceBasedDynamicMapLoader::is_close_to_map(
+ const pcl::PointXYZ & point, const double distance_threshold)
+{
+ if (current_voxel_grid_dict_.size() == 0) {
+ return false;
+ }
+
+ const int map_grid_index = static_cast(
+ std::floor((point.x - origin_x_) / map_grid_size_x_) +
+ map_grids_x_ * std::floor((point.y - origin_y_) / map_grid_size_y_));
+
+ if (static_cast(map_grid_index) >= current_voxel_grid_array_.size()) {
+ return false;
+ }
+ if (
+ current_voxel_grid_array_.at(map_grid_index) != NULL &&
+ is_close_to_neighbor_voxels(
+ point, distance_threshold, current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid,
+ current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree)) {
+ return true;
+ }
+ return false;
+}
+
+VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterComponent(
+ const rclcpp::NodeOptions & options)
+: Filter("VoxelDistanceBasedCompareMapFilter", options)
+{
+ // initialize debug tool
+ {
+ using autoware::universe_utils::DebugPublisher;
+ using autoware::universe_utils::StopWatch;
+ stop_watch_ptr_ = std::make_unique>();
+ debug_publisher_ =
+ std::make_unique(this, "voxel_distance_based_compare_map_filter");
+ stop_watch_ptr_->tic("cyclic_time");
+ stop_watch_ptr_->tic("processing_time");
+ }
+
+ distance_threshold_ = declare_parameter("distance_threshold");
+ bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading");
+ double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis");
+ if (downsize_ratio_z_axis <= 0.0) {
+ RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive");
+ return;
+ }
+ if (use_dynamic_map_loading) {
+ rclcpp::CallbackGroup::SharedPtr main_callback_group;
+ main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+ voxel_distance_based_map_loader_ = std::make_unique(
+ this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_,
+ main_callback_group);
+ } else {
+ voxel_distance_based_map_loader_ = std::make_unique(
+ this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_);
+ }
+}
+
+void VoxelDistanceBasedCompareMapFilterComponent::filter(
+ const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
+ PointCloud2 & output)
+{
+ std::scoped_lock lock(mutex_);
+ stop_watch_ptr_->toc("processing_time", true);
+ int point_step = input->point_step;
+ int offset_x = input->fields[pcl::getFieldIndex(*input, "x")].offset;
+ int offset_y = input->fields[pcl::getFieldIndex(*input, "y")].offset;
+ int offset_z = input->fields[pcl::getFieldIndex(*input, "z")].offset;
+
+ output.data.resize(input->data.size());
+ output.point_step = point_step;
+ size_t output_size = 0;
+ for (size_t global_offset = 0; global_offset < input->data.size(); global_offset += point_step) {
+ pcl::PointXYZ point{};
+ std::memcpy(&point.x, &input->data[global_offset + offset_x], sizeof(float));
+ std::memcpy(&point.y, &input->data[global_offset + offset_y], sizeof(float));
+ std::memcpy(&point.z, &input->data[global_offset + offset_z], sizeof(float));
+ if (voxel_distance_based_map_loader_->is_close_to_map(point, distance_threshold_)) {
+ continue;
+ }
+ std::memcpy(&output.data[output_size], &input->data[global_offset], point_step);
+ output_size += point_step;
+ }
+
+ output.header = input->header;
+ output.fields = input->fields;
+ output.data.resize(output_size);
+ output.height = input->height;
+ output.width = output_size / point_step / output.height;
+ output.row_step = output_size / output.height;
+ output.is_bigendian = input->is_bigendian;
+ output.is_dense = input->is_dense;
+
+ // add processing time for debug
+ if (debug_publisher_) {
+ const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
+ const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
+ debug_publisher_->publish(
+ "debug/cyclic_time_ms", cyclic_time_ms);
+ debug_publisher_->publish(
+ "debug/processing_time_ms", processing_time_ms);
+ }
+}
+} // namespace autoware::compare_map_segmentation
+
+#include
+RCLCPP_COMPONENTS_REGISTER_NODE(
+ autoware::compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent)
diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp
new file mode 100644
index 0000000000000..2199e5ccf476d
--- /dev/null
+++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.hpp
@@ -0,0 +1,155 @@
+// 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.
+
+#ifndef VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT
+#define VOXEL_DISTANCE_BASED_COMPARE_MAP_FILTER__NODE_HPP_ // NOLINT
+
+#include "../voxel_grid_map_loader/voxel_grid_map_loader.hpp"
+<<<<<<<<
+ HEAD : perception / compare_map_segmentation / src / voxel_distance_based_compare_map_filter /
+ node.hpp
+#include "pointcloud_preprocessor/filter.hpp"
+ == == == ==
+#include "autoware/pointcloud_preprocessor/filter.hpp"
+ >>>>>>>> original /
+ main : perception / autoware_compare_map_segmentation / src /
+ voxel_distance_based_compare_map_filter /
+ node.hpp
+
+#include
+#include
+
+#include