From f8308935547e2113146ab0803304edfbc25af000 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Tue, 4 Jun 2024 15:26:38 +0900 Subject: [PATCH 1/4] feat: add utils to handle z height filter Signed-off-by: yoshiri --- .../utility/utilities.hpp | 11 +++++--- .../src/utility/utilities.cpp | 26 +++++++++++++------ 2 files changed, 25 insertions(+), 12 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp index 5de644fd8ffd1..654057e7bb8d7 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp @@ -26,6 +26,7 @@ #include #include +#include #include using K = CGAL::Exact_predicates_inexact_constructions_kernel; @@ -49,28 +50,30 @@ void to_cgal_polygon(const lanelet::BasicPolygon2d & polygon_in, PolygonCgal & p */ void remove_polygon_cgal_from_cloud( const sensor_msgs::msg::PointCloud2 & cloud_in, const PolygonCgal & polyline_polygon, - sensor_msgs::msg::PointCloud2 & cloud_out); + sensor_msgs::msg::PointCloud2 & cloud_out, const std::optional & max_z = std::nullopt); /** * @brief remove points in the given polygon */ void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, const PolygonCgal & polyline_polygon, - pcl::PointCloud & cloud_out); + pcl::PointCloud & cloud_out, const std::optional & max_z = std::nullopt); /** * @brief remove points in the given polygons */ void remove_polygon_cgal_from_cloud( const sensor_msgs::msg::PointCloud2 & cloud_in, - const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out); + const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out, + const std::optional & max_z = std::nullopt); /** * @brief remove points in the given polygons */ void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, - const std::vector & polyline_polygons, pcl::PointCloud & cloud_out); + const std::vector & polyline_polygons, pcl::PointCloud & cloud_out, + const std::optional & max_z = std::nullopt); /** * @brief return true if the given point is inside the at least one of the polygons diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp index fe9fde7301fd7..6783bf8f80f14 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp @@ -46,14 +46,16 @@ void to_cgal_polygon(const lanelet::BasicPolygon2d & polygon_in, PolygonCgal & p void remove_polygon_cgal_from_cloud( const sensor_msgs::msg::PointCloud2 & cloud_in, const PolygonCgal & polyline_polygon, - sensor_msgs::msg::PointCloud2 & cloud_out) + sensor_msgs::msg::PointCloud2 & cloud_out, const std::optional & max_z) { pcl::PointCloud pcl_output; for (sensor_msgs::PointCloud2ConstIterator iter_x(cloud_in, "x"), iter_y(cloud_in, "y"), iter_z(cloud_in, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - // check if the point is inside the polygon + if (max_z && *iter_z > *max_z) { + continue; + } if ( CGAL::bounded_side_2( polyline_polygon.begin(), polyline_polygon.end(), PointCgal(*iter_x, *iter_y), K()) == @@ -72,13 +74,15 @@ void remove_polygon_cgal_from_cloud( void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, const PolygonCgal & polyline_polygon, - pcl::PointCloud & cloud_out) + pcl::PointCloud & cloud_out, const std::optional & max_z) { cloud_out.clear(); cloud_out.header = cloud_in.header; for (const auto & p : cloud_in) { - // check if the point is inside the polygon + if (max_z && p.z > *max_z) { + continue; + } if ( CGAL::bounded_side_2( polyline_polygon.begin(), polyline_polygon.end(), PointCgal(p.x, p.y), K()) == @@ -90,7 +94,8 @@ void remove_polygon_cgal_from_cloud( void remove_polygon_cgal_from_cloud( const sensor_msgs::msg::PointCloud2 & cloud_in, - const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out) + const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out, + const std::optional & max_z) { if (polyline_polygons.empty()) { cloud_out = cloud_in; @@ -101,7 +106,9 @@ void remove_polygon_cgal_from_cloud( for (sensor_msgs::PointCloud2ConstIterator iter_x(cloud_in, "x"), iter_y(cloud_in, "y"), iter_z(cloud_in, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - // if the point is inside the polygon, skip inserting and check the next point + if (max_z && *iter_z > *max_z) { + continue; + } pcl::PointXYZ p(*iter_x, *iter_y, *iter_z); if (point_within_cgal_polys(p, polyline_polygons)) { continue; @@ -115,7 +122,8 @@ void remove_polygon_cgal_from_cloud( void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, - const std::vector & polyline_polygons, pcl::PointCloud & cloud_out) + const std::vector & polyline_polygons, pcl::PointCloud & cloud_out, + const std::optional & max_z) { if (polyline_polygons.empty()) { cloud_out = cloud_in; @@ -124,7 +132,9 @@ void remove_polygon_cgal_from_cloud( pcl::PointCloud filtered_cloud; for (const auto & p : cloud_in) { - // if the point is inside the polygon, skip inserting and check the next point + if (max_z && p.z > *max_z) { + continue; + } if (point_within_cgal_polys(p, polyline_polygons)) { continue; } From 2b32c4840488157fe94116974e1a842fc87b2997 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Thu, 6 Jun 2024 14:29:15 +0900 Subject: [PATCH 2/4] feat: add test to utilities and fix code Signed-off-by: yoshiri --- .../pointcloud_preprocessor/CMakeLists.txt | 8 ++ .../src/utility/utilities.cpp | 44 +++--- .../test/test_utilities.cpp | 125 ++++++++++++++++++ 3 files changed, 154 insertions(+), 23 deletions(-) create mode 100644 sensing/pointcloud_preprocessor/test/test_utilities.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 0cbb04345477e..ecb84c8a7f3b3 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -221,6 +221,14 @@ ament_auto_package(INSTALL_TO_SHARE ) if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest) + ament_lint_auto_find_test_dependencies() + ament_add_gtest(test_utilities + test/test_utilities.cpp + ) + target_link_libraries(test_utilities pointcloud_preprocessor_filter) + add_ros_test( test/test_distortion_corrector.py TIMEOUT "30" diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp index 6783bf8f80f14..ec84a2467db78 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp @@ -53,13 +53,12 @@ void remove_polygon_cgal_from_cloud( for (sensor_msgs::PointCloud2ConstIterator iter_x(cloud_in, "x"), iter_y(cloud_in, "y"), iter_z(cloud_in, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - if (max_z && *iter_z > *max_z) { - continue; - } - if ( - CGAL::bounded_side_2( - polyline_polygon.begin(), polyline_polygon.end(), PointCgal(*iter_x, *iter_y), K()) == - CGAL::ON_UNBOUNDED_SIDE) { + const bool within_max_z = max_z ? *iter_z <= *max_z : true; + const bool within_polygon = CGAL::bounded_side_2( + polyline_polygon.begin(), polyline_polygon.end(), + PointCgal(*iter_x, *iter_y), K()) == CGAL::ON_BOUNDED_SIDE; + // remove points within the polygon and max_z + if (!(within_max_z && within_polygon)) { pcl::PointXYZ p; p.x = *iter_x; p.y = *iter_y; @@ -80,13 +79,12 @@ void remove_polygon_cgal_from_cloud( cloud_out.header = cloud_in.header; for (const auto & p : cloud_in) { - if (max_z && p.z > *max_z) { - continue; - } - if ( - CGAL::bounded_side_2( - polyline_polygon.begin(), polyline_polygon.end(), PointCgal(p.x, p.y), K()) == - CGAL::ON_UNBOUNDED_SIDE) { + const bool within_max_z = max_z ? p.z <= *max_z : true; + const bool within_polygon = CGAL::bounded_side_2( + polyline_polygon.begin(), polyline_polygon.end(), + PointCgal(p.x, p.y), K()) == CGAL::ON_BOUNDED_SIDE; + // remove points within the polygon and max_z + if (!(within_max_z && within_polygon)) { cloud_out.emplace_back(p); } } @@ -106,11 +104,11 @@ void remove_polygon_cgal_from_cloud( for (sensor_msgs::PointCloud2ConstIterator iter_x(cloud_in, "x"), iter_y(cloud_in, "y"), iter_z(cloud_in, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - if (max_z && *iter_z > *max_z) { - continue; - } - pcl::PointXYZ p(*iter_x, *iter_y, *iter_z); - if (point_within_cgal_polys(p, polyline_polygons)) { + const bool within_max_z = max_z ? *iter_z <= *max_z : true; + const pcl::PointXYZ p(*iter_x, *iter_y, *iter_z); + const bool within_polygon = point_within_cgal_polys(p, polyline_polygons); + // remove points within the polygon and max_z + if (within_max_z && within_polygon) { continue; } filtered_cloud.emplace_back(p); @@ -132,10 +130,10 @@ void remove_polygon_cgal_from_cloud( pcl::PointCloud filtered_cloud; for (const auto & p : cloud_in) { - if (max_z && p.z > *max_z) { - continue; - } - if (point_within_cgal_polys(p, polyline_polygons)) { + const bool within_max_z = max_z ? p.z <= *max_z : true; + const bool within_polygon = point_within_cgal_polys(p, polyline_polygons); + // remove points within the polygon and max_z + if (within_max_z && within_polygon) { continue; } filtered_cloud.emplace_back(p); diff --git a/sensing/pointcloud_preprocessor/test/test_utilities.cpp b/sensing/pointcloud_preprocessor/test/test_utilities.cpp new file mode 100644 index 0000000000000..68c86dfbd0505 --- /dev/null +++ b/sensing/pointcloud_preprocessor/test/test_utilities.cpp @@ -0,0 +1,125 @@ +// Copyright 2024 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 "pointcloud_preprocessor/utility/utilities.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include + +constexpr double EPSILON = 1e-6; + +class RemovePolygonCgalFromCloudTest : public ::testing::Test +{ +protected: + PolygonCgal polygon; + sensor_msgs::msg::PointCloud2 cloud_out; + + virtual void SetUp() + { + // Set up a simple square polygon + polygon.push_back(PointCgal(0.0, 0.0)); + polygon.push_back(PointCgal(0.0, 1.0)); + polygon.push_back(PointCgal(1.0, 1.0)); + polygon.push_back(PointCgal(1.0, 0.0)); + polygon.push_back(PointCgal(0.0, 0.0)); + } + + void CreatePointCloud2(sensor_msgs::msg::PointCloud2 & cloud, double x, double y, double z) + { + pcl::PointCloud pcl_cloud; + pcl_cloud.push_back(pcl::PointXYZ(x, y, z)); + pcl::toROSMsg(pcl_cloud, cloud); + cloud.header.frame_id = "map"; + } +}; + +// Test case 1: without max_z, points inside the polygon are removed +TEST_F(RemovePolygonCgalFromCloudTest, PointsInsidePolygonAreRemoved) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 0.5, 0.5, 0.1); // point inside the polygon + + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(cloud_in, polygon, cloud_out); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 0); +} + +// Test case 2: without max_z, points outside the polygon remain +TEST_F(RemovePolygonCgalFromCloudTest, PointsOutsidePolygonRemain) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 1.5, 1.5, 0.1); // point outside the polygon + + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(cloud_in, polygon, cloud_out); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 1); + EXPECT_NEAR(pcl_output.points[0].x, 1.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].y, 1.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].z, 0.1, EPSILON); +} + +// Test case 3: with max_z, points inside the polygon and below max_z are removed +TEST_F(RemovePolygonCgalFromCloudTest, PointsBelowMaxZAreRemoved) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 0.5, 0.5, 0.1); // point inside the polygon, below max_z + + std::optional max_z = 1.0f; + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + cloud_in, polygon, cloud_out, max_z); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 0); +} + +// Test case 4: with max_z, points inside the polygon but above max_z remain +TEST_F(RemovePolygonCgalFromCloudTest, PointsAboveMaxZRemain) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 0.5, 0.5, 1.5); // point inside the polygon, above max_z + + std::optional max_z = 1.0f; + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + cloud_in, polygon, cloud_out, max_z); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 1); + EXPECT_NEAR(pcl_output.points[0].x, 0.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].y, 0.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].z, 1.5, EPSILON); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 89a5a1e720a3c1a5424c25d5d273907ab72ce3fe Mon Sep 17 00:00:00 2001 From: yoshiri Date: Thu, 6 Jun 2024 18:25:08 +0900 Subject: [PATCH 3/4] feat: add node param to filter by z Signed-off-by: yoshiri --- .../vector_map_inside_area_filter.hpp | 2 ++ .../vector_map_inside_area_filter.cpp | 29 +++++++++++++++++-- 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index 74209c1a22e4c..03b0d452bc07e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp @@ -43,6 +43,8 @@ class VectorMapInsideAreaFilterComponent : public pointcloud_preprocessor::Filte // parameter std::string polygon_type_; + bool use_z_filter_ = false; + float z_threshold_; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp index 5b8755714b375..7d63c3379129c 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp @@ -40,7 +40,8 @@ lanelet::ConstPolygons3d calcIntersectedPolygons( } pcl::PointCloud removePointsWithinPolygons( - const pcl::PointCloud::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons) + const pcl::PointCloud::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons, + const std::optional & z_threshold_) { std::vector cgal_polys; @@ -53,7 +54,7 @@ pcl::PointCloud removePointsWithinPolygons( pcl::PointCloud filtered_cloud; pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( - *cloud_in, cgal_polys, filtered_cloud); + *cloud_in, cgal_polys, filtered_cloud, z_threshold_); return filtered_cloud; } @@ -74,6 +75,10 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent( map_sub_ = this->create_subscription( "input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&VectorMapInsideAreaFilterComponent::mapCallback, this, _1)); + + // Set parameters + use_z_filter_ = declare_parameter("use_z_filter", false); + z_threshold_ = declare_parameter("z_threshold", 0.0f); // defined in the base_link frame } void VectorMapInsideAreaFilterComponent::filter( @@ -99,7 +104,25 @@ void VectorMapInsideAreaFilterComponent::filter( const auto intersected_lanelets = calcIntersectedPolygons(bounding_box, polygon_lanelets_); // filter pointcloud by lanelet - const auto filtered_pc = removePointsWithinPolygons(pc_input, intersected_lanelets); + std::optional z_threshold_in_base_link = std::nullopt; + if (use_z_filter_) { + // assume z_max is defined in the base_link frame + const std::string base_link_frame = "base_link"; + z_threshold_in_base_link = z_threshold_; + if (input->header.frame_id != base_link_frame) { + try { + // get z difference from baselink to input frame + const auto transform = + tf_buffer_->lookupTransform(input->header.frame_id, base_link_frame, input->header.stamp); + *z_threshold_in_base_link += transform.transform.translation.z; + } catch (const tf2::TransformException & e) { + RCLCPP_WARN(get_logger(), "Failed to transform z_threshold to base_link frame"); + z_threshold_in_base_link = std::nullopt; + } + } + } + const auto filtered_pc = + removePointsWithinPolygons(pc_input, intersected_lanelets, z_threshold_in_base_link); // convert to ROS message pcl::toROSMsg(filtered_pc, output); From 59c25786d0ea5a1f9c2e3c046fa06f34e3d1a86f Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 7 Jun 2024 15:04:44 +0900 Subject: [PATCH 4/4] feat: cherry-pick lanelet filter update Signed-off-by: yoshiri --- .../config/object_lanelet_filter.param.yaml | 10 ++ ...acle_pointcloud_based_validator.param.yaml | 1 + .../occupancy_grid_based_validator.param.yaml | 3 + .../object_lanelet_filter.hpp | 25 ++- .../object_position_filter.hpp | 1 + ...acle_pointcloud_based_validator.launch.xml | 1 - .../occupancy_grid_based_validator.launch.xml | 3 +- .../detected_object_validation/package.xml | 4 +- .../src/object_lanelet_filter.cpp | 159 +++++++++++++++--- .../test/test_utils.cpp | 67 ++++++++ 10 files changed, 249 insertions(+), 25 deletions(-) create mode 100644 perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml create mode 100644 perception/detected_object_validation/test/test_utils.cpp diff --git a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml index dfdee95642fed..99050d9738ae6 100644 --- a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml +++ b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml @@ -9,3 +9,13 @@ MOTORCYCLE : false BICYCLE : false PEDESTRIAN : false + + filter_settings: + # polygon overlap based filter + polygon_overlap_filter: + enabled: true + # velocity direction based filter + lanelet_direction_filter: + enabled: false + velocity_yaw_threshold: 0.785398 # [rad] (45 deg) + object_speed_threshold: 3.0 # [m/s] diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml index f7a27a52bfa8a..cddee782e8af0 100644 --- a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml +++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -13,3 +13,4 @@ [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] using_2d_validator: false + enable_debugger: false diff --git a/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml b/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml new file mode 100644 index 0000000000000..fc5c634735e23 --- /dev/null +++ b/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + enable_debug: false diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index 4331b5c19d3f1..8c70f020dabd9 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -17,8 +17,10 @@ #include "detected_object_validation/utils/utils.hpp" +#include #include #include +#include #include #include @@ -27,6 +29,7 @@ #include #include +#include #include namespace object_lanelet_filter @@ -48,6 +51,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr object_pub_; rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr object_sub_; + std::unique_ptr debug_publisher_{nullptr}; lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::ConstLanelets road_lanelets_; @@ -58,11 +62,30 @@ class ObjectLaneletFilterNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; utils::FilterTargetLabel filter_target_; - + struct FilterSettings + { + bool polygon_overlap_filter; + bool lanelet_direction_filter; + double lanelet_direction_filter_velocity_yaw_threshold; + double lanelet_direction_filter_object_speed_threshold; + } filter_settings_; + + bool filterObject( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object, + const autoware_auto_perception_msgs::msg::DetectedObject & input_object, + const lanelet::ConstLanelets & intersected_road_lanelets, + const lanelet::ConstLanelets & intersected_shoulder_lanelets, + autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg); LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &); lanelet::ConstLanelets getIntersectedLanelets( const LinearRing2d &, const lanelet::ConstLanelets &); + bool isObjectOverlapLanelets( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const lanelet::ConstLanelets & intersected_lanelets); bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &); + bool isSameDirectionWithLanelets( + const lanelet::ConstLanelets & lanelets, + const autoware_auto_perception_msgs::msg::DetectedObject & object); geometry_msgs::msg::Polygon setFootprint( const autoware_auto_perception_msgs::msg::DetectedObject &); }; diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp index be25eb6a353e6..059f97cbad431 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace object_position_filter diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index 799b605658345..a1d941e66db4b 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -9,7 +9,6 @@ - diff --git a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml index 04bcbd87172b3..3acb1f2d1907a 100644 --- a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml @@ -7,6 +7,7 @@ + @@ -22,6 +23,6 @@ - + diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index 31bb633294748..f18f8ab922c49 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -6,7 +6,9 @@ The ROS 2 detected_object_validation package Yukihiro Saito Shunsuke Miura - badai nguyen + Dai Nguyen + Shintaro Tomie + Yoshi RI Apache License 2.0 ament_cmake_auto diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index cd16d414425cb..46af50c311013 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -43,6 +43,15 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE", false); filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE", false); filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN", false); + // Set filter settings + filter_settings_.polygon_overlap_filter = + declare_parameter("filter_settings.polygon_overlap_filter.enabled"); + filter_settings_.lanelet_direction_filter = + declare_parameter("filter_settings.lanelet_direction_filter.enabled"); + filter_settings_.lanelet_direction_filter_velocity_yaw_threshold = + declare_parameter("filter_settings.lanelet_direction_filter.velocity_yaw_threshold"); + filter_settings_.lanelet_direction_filter_object_speed_threshold = + declare_parameter("filter_settings.lanelet_direction_filter.object_speed_threshold"); // Set publisher/subscriber map_sub_ = this->create_subscription( @@ -52,6 +61,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod "input/object", rclcpp::QoS{1}, std::bind(&ObjectLaneletFilterNode::objectCallback, this, _1)); object_pub_ = this->create_publisher( "output/object", rclcpp::QoS{1}); + + debug_publisher_ = + std::make_unique(this, "object_lanelet_filter"); } void ObjectLaneletFilterNode::mapCallback( @@ -93,29 +105,65 @@ void ObjectLaneletFilterNode::objectCallback( lanelet::ConstLanelets intersected_shoulder_lanelets = getIntersectedLanelets(convex_hull, shoulder_lanelets_); - int index = 0; - for (const auto & object : transformed_objects.objects) { - const auto footprint = setFootprint(object); - const auto & label = object.classification.front().label; - if (filter_target_.isTarget(label)) { - Polygon2d polygon; - for (const auto & point : footprint.points) { - const geometry_msgs::msg::Point32 point_transformed = - tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); - polygon.outer().emplace_back(point_transformed.x, point_transformed.y); - } - polygon.outer().push_back(polygon.outer().front()); - if (isPolygonOverlapLanelets(polygon, intersected_road_lanelets)) { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } else if (isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets)) { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } - } else { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } - ++index; + // filtering process + for (size_t index = 0; index < transformed_objects.objects.size(); ++index) { + const auto & transformed_object = transformed_objects.objects.at(index); + const auto & input_object = input_msg->objects.at(index); + filterObject( + transformed_object, input_object, intersected_road_lanelets, intersected_shoulder_lanelets, + output_object_msg); } object_pub_->publish(output_object_msg); + + // Publish debug info + const double pipeline_latency = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - output_object_msg.header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency); +} + +bool ObjectLaneletFilterNode::filterObject( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object, + const autoware_auto_perception_msgs::msg::DetectedObject & input_object, + const lanelet::ConstLanelets & intersected_road_lanelets, + const lanelet::ConstLanelets & intersected_shoulder_lanelets, + autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg) +{ + const auto & label = transformed_object.classification.front().label; + if (filter_target_.isTarget(label)) { + bool filter_pass = true; + // 1. is polygon overlap with road lanelets or shoulder lanelets + if (filter_settings_.polygon_overlap_filter) { + const bool is_polygon_overlap = + isObjectOverlapLanelets(transformed_object, intersected_road_lanelets) || + isObjectOverlapLanelets(transformed_object, intersected_shoulder_lanelets); + filter_pass = filter_pass && is_polygon_overlap; + } + + // 2. check if objects velocity is the same with the lanelet direction + const bool orientation_not_available = + transformed_object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + if (filter_settings_.lanelet_direction_filter && !orientation_not_available) { + const bool is_same_direction = + isSameDirectionWithLanelets(intersected_road_lanelets, transformed_object) || + isSameDirectionWithLanelets(intersected_shoulder_lanelets, transformed_object); + filter_pass = filter_pass && is_same_direction; + } + + // push back to output object + if (filter_pass) { + output_object_msg.objects.emplace_back(input_object); + return true; + } + } else { + output_object_msg.objects.emplace_back(input_object); + return true; + } + return false; } geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint( @@ -176,6 +224,39 @@ lanelet::ConstLanelets ObjectLaneletFilterNode::getIntersectedLanelets( return intersected_lanelets; } +bool ObjectLaneletFilterNode::isObjectOverlapLanelets( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const lanelet::ConstLanelets & intersected_lanelets) +{ + // if has bounding box, use polygon overlap + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::POLYGON) { + Polygon2d polygon; + const auto footprint = setFootprint(object); + for (const auto & point : footprint.points) { + const geometry_msgs::msg::Point32 point_transformed = + tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + polygon.outer().emplace_back(point_transformed.x, point_transformed.y); + } + polygon.outer().push_back(polygon.outer().front()); + return isPolygonOverlapLanelets(polygon, intersected_lanelets); + } else { + // if object do not have bounding box, check each footprint is inside polygon + for (const auto & point : object.shape.footprint.points) { + const geometry_msgs::msg::Point32 point_transformed = + tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + geometry_msgs::msg::Pose point2d; + point2d.position.x = point_transformed.x; + point2d.position.y = point_transformed.y; + for (const auto & lanelet : intersected_lanelets) { + if (lanelet::utils::isInLanelet(point2d, lanelet, 0.0)) { + return true; + } + } + } + return false; + } +} + bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( const Polygon2d & polygon, const lanelet::ConstLanelets & intersected_lanelets) { @@ -187,6 +268,42 @@ bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( return false; } +bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( + const lanelet::ConstLanelets & lanelets, + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double object_velocity_norm = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const double object_velocity_yaw = std::atan2( + object.kinematics.twist_with_covariance.twist.linear.y, + object.kinematics.twist_with_covariance.twist.linear.x) + + object_yaw; + + if (object_velocity_norm < filter_settings_.lanelet_direction_filter_object_speed_threshold) { + return true; + } + for (const auto & lanelet : lanelets) { + const bool is_in_lanelet = + lanelet::utils::isInLanelet(object.kinematics.pose_with_covariance.pose, lanelet, 0.0); + if (!is_in_lanelet) { + continue; + } + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_velocity_yaw - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); + + if (abs_norm_delta_yaw < filter_settings_.lanelet_direction_filter_velocity_yaw_threshold) { + return true; + } + } + + return false; +} + } // namespace object_lanelet_filter #include diff --git a/perception/detected_object_validation/test/test_utils.cpp b/perception/detected_object_validation/test/test_utils.cpp new file mode 100644 index 0000000000000..ace4dd10a191f --- /dev/null +++ b/perception/detected_object_validation/test/test_utils.cpp @@ -0,0 +1,67 @@ +// Copyright 2024 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 "detected_object_validation/utils/utils.hpp" + +#include + +#include + +using AutowareLabel = autoware_auto_perception_msgs::msg::ObjectClassification; + +utils::FilterTargetLabel createFilterTargetAll() +{ + utils::FilterTargetLabel filter; + filter.UNKNOWN = true; + filter.CAR = true; + filter.TRUCK = true; + filter.BUS = true; + filter.TRAILER = true; + filter.MOTORCYCLE = true; + filter.BICYCLE = true; + filter.PEDESTRIAN = true; + return filter; +} + +utils::FilterTargetLabel createFilterTargetVehicle() +{ + utils::FilterTargetLabel filter; + filter.UNKNOWN = false; + filter.CAR = true; + filter.TRUCK = true; + filter.BUS = true; + filter.TRAILER = true; + filter.MOTORCYCLE = false; + filter.BICYCLE = false; + filter.PEDESTRIAN = false; + return filter; +} + +TEST(IsTargetTest, AllTarget) +{ + auto filter = createFilterTargetAll(); + auto label = AutowareLabel::CAR; + EXPECT_TRUE(filter.isTarget(label)); +} + +TEST(IsTargetTest, VehicleTarget) +{ + auto filter = createFilterTargetVehicle(); + + auto car_label = AutowareLabel::CAR; + EXPECT_TRUE(filter.isTarget(car_label)); + + auto unknown_label = AutowareLabel::UNKNOWN; + EXPECT_FALSE(filter.isTarget(unknown_label)); +}