diff --git a/perception/autoware_detected_object_validation/CMakeLists.txt b/perception/autoware_detected_object_validation/CMakeLists.txt index bf205fb32bb1d..dd8d3616c362e 100644 --- a/perception/autoware_detected_object_validation/CMakeLists.txt +++ b/perception/autoware_detected_object_validation/CMakeLists.txt @@ -83,6 +83,10 @@ if(BUILD_TESTING) test/test_utils.cpp test/object_position_filter/test_object_position_filter.cpp ) + ament_auto_add_gtest(object_lanelet_filter_tests + test/test_utils.cpp + test/lanelet_filter/test_lanelet_filter.cpp + ) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml index d15b2c81cf6db..c6b3b5538fc44 100644 --- a/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml +++ b/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml @@ -21,3 +21,6 @@ object_speed_threshold: 3.0 # [m/s] debug: false lanelet_extra_margin: 0.0 + use_height_threshold: false + max_height_threshold: 10.0 # [m] from the base_link height in map frame + min_height_threshold: -1.0 # [m] from the base_link height in map frame diff --git a/perception/autoware_detected_object_validation/object-lanelet-filter.md b/perception/autoware_detected_object_validation/object-lanelet-filter.md index b748885c188b4..4da7b7fdea3de 100644 --- a/perception/autoware_detected_object_validation/object-lanelet-filter.md +++ b/perception/autoware_detected_object_validation/object-lanelet-filter.md @@ -24,10 +24,19 @@ The objects only inside of the vector map will be published. ## Parameters -| Name | Type | Description | -| ---------------------- | -------- | ------------------------------------------------------------------------------------ | -| `debug` | `bool` | if true, publish debug markers | -| `lanelet_extra_margin` | `double` | `if > 0` lanelet polygons are expanded by extra margin, `if <= 0` margin is disabled | +Description of the `filter_settings` in the parameters of the `object_lanelet_filter` node. + +| Name | Type | Description | +| ------------------------------------------------- | -------- | ------------------------------------------------------------------------------------------------------------------------------------- | +| `debug` | `bool` | If `true`, publishes additional debug information, including visualization markers on the `~/debug/marker` topic for tools like RViz. | +| `lanelet_extra_margin` | `double` | If `> 0`, expands the lanelet polygons used for overlap checks by this margin (in meters). If `<= 0`, polygon expansion is disabled. | +| `polygon_overlap_filter.enabled` | `bool` | If `true`, enables filtering of objects based on their overlap with lanelet polygons. | +| `lanelet_direction_filter.enabled` | `bool` | If `true`, enables filtering of objects based on their velocity direction relative to the lanelet. | +| `lanelet_direction_filter.velocity_yaw_threshold` | `double` | The yaw angle difference threshold (in radians) between the object’s velocity vector and the lanelet direction. | +| `lanelet_direction_filter.object_speed_threshold` | `double` | The minimum speed (in m/s) of an object required for the direction filter to be applied. | +| `use_height_threshold` | `bool` | If `true`, enables filtering of objects based on their height relative to the base_link frame. | +| `max_height_threshold` | `double` | The maximum allowable height (in meters) of an object relative to the base_link in the map frame. | +| `min_height_threshold` | `double` | The minimum allowable height (in meters) of an object relative to the base_link in the map frame. | ### Core Parameters diff --git a/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json b/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json index fd4e39f0ca02e..8e1b641eebbf8 100644 --- a/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json +++ b/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json @@ -95,9 +95,42 @@ } }, "required": ["enabled", "velocity_yaw_threshold", "object_speed_threshold"] + }, + "debug": { + "type": "boolean", + "default": false, + "description": "If true, debug information is enabled." + }, + "lanelet_extra_margin": { + "type": "number", + "default": 0.0, + "description": "Extra margin added to the lanelet boundaries." + }, + "use_height_threshold": { + "type": "boolean", + "default": false, + "description": "If true, height thresholds are used to filter objects." + }, + "max_height_threshold": { + "type": "number", + "default": 10.0, + "description": "Maximum height threshold for filtering objects (in meters)." + }, + "min_height_threshold": { + "type": "number", + "default": -1.0, + "description": "Minimum height threshold for filtering objects (in meters)." } }, - "required": ["polygon_overlap_filter", "lanelet_direction_filter"] + "required": [ + "polygon_overlap_filter", + "lanelet_direction_filter", + "debug", + "lanelet_extra_margin", + "use_height_threshold", + "max_height_threshold", + "min_height_threshold" + ] } }, "required": ["filter_target_label", "filter_settings"], diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index f1c25e8573e85..85add97e20078 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -66,6 +66,12 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod filter_settings_.debug = declare_parameter("filter_settings.debug"); filter_settings_.lanelet_extra_margin = declare_parameter("filter_settings.lanelet_extra_margin"); + filter_settings_.use_height_threshold = + declare_parameter("filter_settings.use_height_threshold"); + filter_settings_.max_height_threshold = + declare_parameter("filter_settings.max_height_threshold"); + filter_settings_.min_height_threshold = + declare_parameter("filter_settings.min_height_threshold"); // Set publisher/subscriber map_sub_ = this->create_subscription( @@ -146,6 +152,19 @@ void ObjectLaneletFilterNode::objectCallback( RCLCPP_ERROR(get_logger(), "Failed transform to %s.", lanelet_frame_id_.c_str()); return; } + // vehicle base pose :map -> base_link + if (filter_settings_.use_height_threshold) { + try { + ego_base_height_ = tf_buffer_ + .lookupTransform( + lanelet_frame_id_, "base_link", transformed_objects.header.stamp, + rclcpp::Duration::from_seconds(0.5)) + .transform.translation.z; + } catch (const tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM(get_logger(), "Failed to get transform: " << ex.what()); + return; + } + } if (!transformed_objects.objects.empty()) { // calculate convex hull @@ -199,6 +218,16 @@ bool ObjectLaneletFilterNode::filterObject( return false; } + // 0. check height threshold + if (filter_settings_.use_height_threshold) { + const double object_height = transformed_object.shape.dimensions.z; + if ( + object_height > ego_base_height_ + filter_settings_.max_height_threshold || + object_height < ego_base_height_ + filter_settings_.min_height_threshold) { + return false; + } + } + bool filter_pass = true; // 1. is polygon overlap with road lanelets or shoulder lanelets if (filter_settings_.polygon_overlap_filter) { diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp index 724e60df27e49..bbb0f7c0d43ce 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -88,6 +89,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; utils::FilterTargetLabel filter_target_; + double ego_base_height_ = 0.0; struct FilterSettings { bool polygon_overlap_filter; @@ -96,6 +98,9 @@ class ObjectLaneletFilterNode : public rclcpp::Node double lanelet_direction_filter_object_speed_threshold; bool debug; double lanelet_extra_margin; + bool use_height_threshold; + double max_height_threshold = std::numeric_limits::infinity(); + double min_height_threshold = -std::numeric_limits::infinity(); } filter_settings_; bool filterObject( diff --git a/perception/autoware_detected_object_validation/test/lanelet_filter/test_lanelet_filter.cpp b/perception/autoware_detected_object_validation/test/lanelet_filter/test_lanelet_filter.cpp new file mode 100644 index 0000000000000..b1e061261f8b6 --- /dev/null +++ b/perception/autoware_detected_object_validation/test/lanelet_filter/test_lanelet_filter.cpp @@ -0,0 +1,302 @@ +// Copyright 2025 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 +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +// Include the filter node header with include guard + +#include "../../src/lanelet_filter/lanelet_filter.hpp" +#include "../test_lanelet_utils.hpp" + +using autoware::detected_object_validation::lanelet_filter::ObjectLaneletFilterNode; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::ObjectClassification; + +namespace +{ +/// @brief Create a test manager (custom utility for Autoware tests) +std::shared_ptr generateTestManager() +{ + return std::make_shared(); +} + +/// @brief Create an instance of ObjectLaneletFilterNode with a parameter file +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + // Example parameter file path + const auto detected_object_validation_dir = + ament_index_cpp::get_package_share_directory("autoware_detected_object_validation"); + node_options.arguments( + {"--ros-args", "--params-file", + detected_object_validation_dir + "/config/object_lanelet_filter.param.yaml"}); + + return std::make_shared(node_options); +} + +std::shared_ptr createStaticTfBroadcasterNode( + const std::string & parent_frame_id, const std::string & child_frame_id, + const geometry_msgs::msg::Vector3 & translation, const geometry_msgs::msg::Quaternion & rotation, + const std::string & node_name = "test_tf_broadcaster") +{ + // Create a dedicated node for broadcasting the static transform + auto broadcaster_node = std::make_shared(node_name); + + // Create the static transform broadcaster + auto static_broadcaster = std::make_shared(broadcaster_node); + + // Prepare the transform message + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.stamp = broadcaster_node->now(); + transform_stamped.header.frame_id = parent_frame_id; + transform_stamped.child_frame_id = child_frame_id; + transform_stamped.transform.translation = translation; + transform_stamped.transform.rotation = rotation; + + // Broadcast the transform + static_broadcaster->sendTransform(transform_stamped); + + return broadcaster_node; +} + +// publish sample LaneletMapBin message +void publishLaneletMapBin( + const std::shared_ptr & test_manager, + const std::shared_ptr & test_target_node, + const std::string & input_map_topic = "input/vector_map") +{ + auto qos = rclcpp::QoS(1).transient_local(); + LaneletMapBin map_msg = createSimpleLaneletMapMsg(); + test_manager->test_pub_msg(test_target_node, input_map_topic, map_msg, qos); +} +} // namespace + +/// @brief Test LaneletMapBin publishing + object filtering +TEST(DetectedObjectValidationTest, testObjectLaneletFilterWithMap) +{ + rclcpp::init(0, nullptr); + + // Create the test node (lanelet_filter_node) + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + // Create and add a TF broadcaster node + auto tf_node = createStaticTfBroadcasterNode( + "map", "base_link", geometry_msgs::build().x(0.0).y(0.0).z(0.0), + geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0), + "my_test_tf_broadcaster"); + + // Subscribe to the output topic + const std::string output_topic = "output/object"; + DetectedObjects latest_msg; + auto output_callback = [&latest_msg](const DetectedObjects::ConstSharedPtr msg) { + latest_msg = *msg; + }; + test_manager->set_subscriber(output_topic, output_callback); + + // 1) Publish a LaneletMapBin + // In a real test, you'd have an actual map message. + publishLaneletMapBin(test_manager, test_target_node); + + // 2) Publish DetectedObjects + const std::string input_object_topic = "input/object"; + DetectedObjects input_objects; + input_objects.header.frame_id = "base_link"; + + // Prepare two unknown objects + { // Object 1: not in the lanelet + DetectedObject obj; + obj.kinematics.pose_with_covariance.pose.position.x = 100.0; + obj.kinematics.pose_with_covariance.pose.position.y = 5.0; + obj.classification.resize(1); + obj.classification[0].label = ObjectClassification::UNKNOWN; + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(1.0).y(1.0).z(0.0)); + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(1.0).y(-1.0).z(0.0)); + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(-1.0).y(-1.0).z(0.0)); + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(-1.0).y(1.0).z(0.0)); + input_objects.objects.push_back(obj); + } + { // Object 2: in the lanelet. To be filtered out. + DetectedObject obj; + obj.kinematics.pose_with_covariance.pose.position.x = 0.0; + obj.kinematics.pose_with_covariance.pose.position.y = 0.0; + obj.classification.resize(1); + obj.classification[0].label = ObjectClassification::UNKNOWN; + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(1.0).y(1.0).z(0.0)); + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(1.0).y(-1.0).z(0.0)); + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(-1.0).y(-1.0).z(0.0)); + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(-1.0).y(1.0).z(0.0)); + input_objects.objects.push_back(obj); + } + + // Publish the DetectedObjects + test_manager->test_pub_msg(test_target_node, input_object_topic, input_objects); + + EXPECT_EQ(latest_msg.objects.size(), 1) + << "Expected one object to pass through (or receive something)."; + + rclcpp::shutdown(); +} + +/// @brief Test with an empty object list +TEST(DetectedObjectValidationTest, testObjectLaneletFilterEmptyObjects) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + // Create and add a TF broadcaster node + auto tf_node = createStaticTfBroadcasterNode( + "map", "base_link", geometry_msgs::build().x(0.0).y(0.0).z(0.0), + geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0), + "my_test_tf_broadcaster"); + + // Publish simple LaneletMapBin + publishLaneletMapBin(test_manager, test_target_node); + + // Subscribe to the output topic + const std::string output_topic = "output/object"; + int callback_counter = 0; + auto output_callback = [&callback_counter](const DetectedObjects::ConstSharedPtr msg) { + (void)msg; + ++callback_counter; + }; + test_manager->set_subscriber(output_topic, output_callback); + + // Publish empty objects (not publishing map) + DetectedObjects input_objects; + input_objects.header.frame_id = "base_link"; + + test_manager->test_pub_msg(test_target_node, "input/object", input_objects); + + // The callback should still be called at least once + EXPECT_GE(callback_counter, 1); + + rclcpp::shutdown(); +} + +TEST(DetectedObjectValidationTest, testObjectLaneletFilterHeightThreshold) +{ + rclcpp::init(0, nullptr); + + // 1) Setup test manager and node with a custom param override (height filter enabled) + auto test_manager = generateTestManager(); + + auto node_options = rclcpp::NodeOptions{}; + const auto detected_object_validation_dir = + ament_index_cpp::get_package_share_directory("autoware_detected_object_validation"); + node_options.arguments( + {"--ros-args", "--params-file", + detected_object_validation_dir + "/config/object_lanelet_filter.param.yaml"}); + node_options.append_parameter_override("filter_settings.use_height_threshold", true); + node_options.append_parameter_override("filter_settings.max_height_threshold", 2.0); + node_options.append_parameter_override("filter_settings.min_height_threshold", 0.0); + + auto test_target_node = std::make_shared(node_options); + + // 2) Create a TF broadcaster node (map->base_link at z=0.0) + auto tf_node = createStaticTfBroadcasterNode( + "map", "base_link", geometry_msgs::build().x(0.0).y(0.0).z(0.0), + geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0), + "my_test_tf_broadcaster_1"); + + // Subscribe to the output topic + const std::string output_topic = "output/object"; + DetectedObjects latest_msg; + auto output_callback = [&latest_msg](const DetectedObjects::ConstSharedPtr msg) { + latest_msg = *msg; + }; + test_manager->set_subscriber(output_topic, output_callback); + + // 3) Publish a simple LaneletMapBin (50m straight lane) + publishLaneletMapBin(test_manager, test_target_node); + + // 4) Publish DetectedObjects + const std::string input_object_topic = "input/object"; + DetectedObjects input_objects; + input_objects.header.frame_id = "base_link"; + + // (A) Object in-lane, height=1.5 => expected to remain + { + DetectedObject obj; + obj.classification.resize(1); + obj.classification[0].label = ObjectClassification::UNKNOWN; + obj.kinematics.pose_with_covariance.pose.position.x = 0.0; + obj.kinematics.pose_with_covariance.pose.position.y = 0.0; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 2.0; + obj.shape.dimensions.z = 1.5; + input_objects.objects.push_back(obj); + } + + // (B) Object in-lane, height=3.0 => expected to be filtered out + { + DetectedObject obj; + obj.classification.resize(1); + obj.classification[0].label = ObjectClassification::UNKNOWN; + obj.kinematics.pose_with_covariance.pose.position.x = 5.0; + obj.kinematics.pose_with_covariance.pose.position.y = 0.0; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 2.0; + obj.shape.dimensions.z = 3.0; + input_objects.objects.push_back(obj); + } + + // Publish the objects (Round 1) + test_manager->test_pub_msg(test_target_node, input_object_topic, input_objects); + + // 5) Check result => only the first object remains + EXPECT_EQ(latest_msg.objects.size(), 1U) + << "Height filter is enabled => only the shorter object (1.5m) should remain."; + + // 6) Second scenario: place ego at z=1.3, effectively lowering object's relative height + auto tf_node_after = createStaticTfBroadcasterNode( + "map", "base_link", geometry_msgs::build().x(0.0).y(0.0).z(1.3), + geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0), + "my_test_tf_broadcaster_2"); + + // Publish the same objects (Round 2) + test_manager->test_pub_msg(test_target_node, input_object_topic, input_objects); + + // 7) Check result => now both objects remain because each one's height above base_link is < 2.0 + EXPECT_EQ(latest_msg.objects.size(), 2U) + << "With ego at z=1.3, the 3.0m object is effectively only ~1.7m above ego => remain."; + + rclcpp::shutdown(); +} diff --git a/perception/autoware_detected_object_validation/test/test_lanelet_utils.hpp b/perception/autoware_detected_object_validation/test/test_lanelet_utils.hpp new file mode 100644 index 0000000000000..3e9ff5372044a --- /dev/null +++ b/perception/autoware_detected_object_validation/test/test_lanelet_utils.hpp @@ -0,0 +1,71 @@ +// Copyright 2025 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 TEST_LANELET_UTILS_HPP_ +#define TEST_LANELET_UTILS_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include + +/** + * @brief Create a simple LaneletMapBin message containing a single 50[m] straight lanelet. + * + * The lanelet has left boundary at y=+1.5 and right boundary at y=-1.5, + * forming a lane of width 3.0[m] and length 50.0[m]. + * + * @return autoware_map_msgs::msg::LaneletMapBin The generated map message. + */ +inline autoware_map_msgs::msg::LaneletMapBin createSimpleLaneletMapMsg() +{ + // 1) Create left boundary (y = +1.5). + // This is a straight line from (0, +1.5, 0) to (50, +1.5, 0). + lanelet::Point3d p1_left(lanelet::utils::getId(), 0.0, 1.5, 0.0); + lanelet::Point3d p2_left(lanelet::utils::getId(), 50.0, 1.5, 0.0); + lanelet::LineString3d ls_left(lanelet::utils::getId(), {p1_left, p2_left}); + ls_left.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::RoadBorder; + + // 2) Create right boundary (y = -1.5). + // This is a straight line from (0, -1.5, 0) to (50, -1.5, 0). + lanelet::Point3d p1_right(lanelet::utils::getId(), 0.0, -1.5, 0.0); + lanelet::Point3d p2_right(lanelet::utils::getId(), 50.0, -1.5, 0.0); + lanelet::LineString3d ls_right(lanelet::utils::getId(), {p1_right, p2_right}); + ls_right.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::RoadBorder; + + // 3) Create a lanelet by specifying the left and right boundaries, + // then set the subtype to "Road". + lanelet::Lanelet lanelet_section(lanelet::utils::getId(), ls_left, ls_right); + lanelet_section.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Road; + + // 4) Create a LaneletMap and add the lanelet to it. + auto lanelet_map = std::make_shared(); + lanelet_map->add(lanelet_section); + + // 5) Convert the LaneletMap to a LaneletMapBin message. + autoware_map_msgs::msg::LaneletMapBin map_bin_msg; + lanelet::utils::conversion::toBinMsg(lanelet_map, &map_bin_msg); + + // 6) Set the frame_id in the header. + map_bin_msg.header.frame_id = "map"; + + return map_bin_msg; +} + +#endif // TEST_LANELET_UTILS_HPP_