From 47e517a04cbb05552de17512f858ae19246f9e0a Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Thu, 7 Jul 2022 03:32:25 +0900 Subject: [PATCH] feat(object_filter): add detected object filter (#1221) * Add detected object filter Signed-off-by: Shunsuke Miura * Refactor class name Signed-off-by: Shunsuke Miura * Add readme Signed-off-by: Shunsuke Miura * ADd lanelet filter option Signed-off-by: Shunsuke Miura * change default parameter Signed-off-by: Shunsuke Miura * refactor Signed-off-by: Shunsuke Miura * Update readme Signed-off-by: Shunsuke Miura * change detection launch Signed-off-by: Shunsuke Miura * ADd unknown only option Signed-off-by: Shunsuke Miura * Update launcher Signed-off-by: Shunsuke Miura * Fix bug Signed-off-by: Shunsuke Miura * Move object filter into detected_object_validator Signed-off-by: Shunsuke Miura * ci(pre-commit): autofix * Add config parameter yaml for position filter Signed-off-by: Shunsuke Miura * Add config for each class Signed-off-by: Shunsuke Miura * ci(pre-commit): autofix * Fix config Signed-off-by: Shunsuke Miura * Use shape instead of position Signed-off-by: Shunsuke Miura * Update read me Signed-off-by: Shunsuke Miura * Use disjoint instead of intersects Signed-off-by: Shunsuke Miura * ci(pre-commit): autofix * Fix typo, remove debug code. Signed-off-by: Shunsuke Miura * Use shared_ptr Signed-off-by: Shunsuke Miura Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- ...ra_lidar_fusion_based_detection.launch.xml | 10 +- .../lidar_based_detection.launch.xml | 8 + .../detected_object_validation/CMakeLists.txt | 23 ++ .../detected_object_validation/README.md | 2 + .../config/object_lanelet_filter.param.yaml | 11 + .../config/object_position_filter.param.yaml | 16 ++ .../object_lanelet_filter.hpp | 82 +++++++ .../object_position_filter.hpp | 68 ++++++ .../launch/object_lanelet_filter.launch.xml | 14 ++ .../launch/object_position_filter.launch.xml | 12 + .../object-lanelet-filter.md | 50 +++++ .../object-position-filter.md | 53 +++++ .../detected_object_validation/package.xml | 3 + .../src/object_lanelet_filter.cpp | 207 ++++++++++++++++++ .../src/object_position_filter.cpp | 87 ++++++++ 15 files changed, 645 insertions(+), 1 deletion(-) create mode 100644 perception/detected_object_validation/config/object_lanelet_filter.param.yaml create mode 100644 perception/detected_object_validation/config/object_position_filter.param.yaml create mode 100644 perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp create mode 100644 perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp create mode 100644 perception/detected_object_validation/launch/object_lanelet_filter.launch.xml create mode 100644 perception/detected_object_validation/launch/object_position_filter.launch.xml create mode 100644 perception/detected_object_validation/object-lanelet-filter.md create mode 100644 perception/detected_object_validation/object-position-filter.md create mode 100644 perception/detected_object_validation/src/object_lanelet_filter.cpp create mode 100644 perception/detected_object_validation/src/object_position_filter.cpp diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index ca0915753816e..46ddadad521cd 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -177,6 +177,14 @@ - + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 98ad89b603637..e65f8162e2d49 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -99,6 +99,14 @@ + + + + + + + + diff --git a/perception/detected_object_validation/CMakeLists.txt b/perception/detected_object_validation/CMakeLists.txt index 3aada4eafa396..7ea3ef5b56bce 100644 --- a/perception/detected_object_validation/CMakeLists.txt +++ b/perception/detected_object_validation/CMakeLists.txt @@ -14,6 +14,9 @@ find_package(PCL REQUIRED QUIET COMPONENTS common search filters segmentation) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) +### Find Boost Dependencies +find_package(Boost REQUIRED) + include_directories( include SYSTEM @@ -21,6 +24,7 @@ include_directories( ${PCL_COMMON_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} ) # Generate occupancy grid based validator exe file @@ -51,11 +55,30 @@ target_link_libraries(obstacle_pointcloud_based_validator Eigen3::Eigen ) +ament_auto_add_library(object_lanelet_filter SHARED + src/object_lanelet_filter.cpp +) + +ament_auto_add_library(object_position_filter SHARED + src/object_position_filter.cpp +) + rclcpp_components_register_node(obstacle_pointcloud_based_validator PLUGIN "obstacle_pointcloud_based_validator::ObstaclePointCloudBasedValidator" EXECUTABLE obstacle_pointcloud_based_validator_node ) +rclcpp_components_register_node(object_lanelet_filter + PLUGIN "object_lanelet_filter::ObjectLaneletFilterNode" + EXECUTABLE object_lanelet_filter_node +) + +rclcpp_components_register_node(object_position_filter + PLUGIN "object_position_filter::ObjectPositionFilterNode" + EXECUTABLE object_position_filter_node +) + ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/detected_object_validation/README.md b/perception/detected_object_validation/README.md index 2e7ae38d52a2a..12fad4ad9759f 100644 --- a/perception/detected_object_validation/README.md +++ b/perception/detected_object_validation/README.md @@ -8,3 +8,5 @@ The purpose of this package is to eliminate obvious false positives of DetectedO - [Obstacle pointcloud based validator](obstacle-pointcloud-based-validator.md) - [Occupancy grid based validator](occupancy-grid-based-validator.md) +- [Object lanelet filter](object-lanelet-filter.md) +- [Object position filter](object-position-filter.md) diff --git a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml new file mode 100644 index 0000000000000..dfdee95642fed --- /dev/null +++ b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + filter_target_label: + UNKNOWN : true + CAR : false + TRUCK : false + BUS : false + TRAILER : false + MOTORCYCLE : false + BICYCLE : false + PEDESTRIAN : false diff --git a/perception/detected_object_validation/config/object_position_filter.param.yaml b/perception/detected_object_validation/config/object_position_filter.param.yaml new file mode 100644 index 0000000000000..70afd9d31be94 --- /dev/null +++ b/perception/detected_object_validation/config/object_position_filter.param.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + filter_target_label: + UNKNOWN : true + CAR : false + TRUCK : false + BUS : false + TRAILER : false + MOTORCYCLE : false + BICYCLE : false + PEDESTRIAN : false + + upper_bound_x: 100.0 + lower_bound_x: 0.0 + upper_bound_y: 10.0 + lower_bound_y: -10.0 diff --git a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp new file mode 100644 index 0000000000000..f4f3cd48c1187 --- /dev/null +++ b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp @@ -0,0 +1,82 @@ +// Copyright 2022 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 DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ +#define DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +namespace object_lanelet_filter +{ +using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::MultiPoint2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + +struct Filter_target_label +{ + bool UNKNOWN; + bool CAR; + bool TRUCK; + bool BUS; + bool TRAILER; + bool MOTORCYCLE; + bool BICYCLE; + bool PEDESTRIAN; +}; + +class ObjectLaneletFilterNode : public rclcpp::Node +{ +public: + explicit ObjectLaneletFilterNode(const rclcpp::NodeOptions & node_options); + +private: + void objectCallback(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr); + void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr); + + rclcpp::Publisher::SharedPtr object_pub_; + rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr object_sub_; + + lanelet::LaneletMapPtr lanelet_map_ptr_; + lanelet::ConstLanelets road_lanelets_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + Filter_target_label filter_target_; + + bool transformDetectedObjects( + const autoware_auto_perception_msgs::msg::DetectedObjects &, const std::string &, + const tf2_ros::Buffer &, autoware_auto_perception_msgs::msg::DetectedObjects &); + LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &); + lanelet::ConstLanelets getIntersectedLanelets( + const LinearRing2d &, const lanelet::ConstLanelets &); + bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &); +}; + +} // namespace object_lanelet_filter + +#endif // DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_ diff --git a/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp new file mode 100644 index 0000000000000..59326f4e21395 --- /dev/null +++ b/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp @@ -0,0 +1,68 @@ +// Copyright 2022 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 DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ +#define DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +namespace object_position_filter +{ +struct Filter_target_label +{ + bool UNKNOWN; + bool CAR; + bool TRUCK; + bool BUS; + bool TRAILER; + bool MOTORCYCLE; + bool BICYCLE; + bool PEDESTRIAN; +}; + +class ObjectPositionFilterNode : public rclcpp::Node +{ +public: + explicit ObjectPositionFilterNode(const rclcpp::NodeOptions & node_options); + +private: + void objectCallback(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr); + + rclcpp::Publisher::SharedPtr object_pub_; + rclcpp::Subscription::SharedPtr object_sub_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + float upper_bound_x_; + float upper_bound_y_; + float lower_bound_x_; + float lower_bound_y_; + Filter_target_label filter_target_; +}; + +} // namespace object_position_filter + +#endif // DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_ diff --git a/perception/detected_object_validation/launch/object_lanelet_filter.launch.xml b/perception/detected_object_validation/launch/object_lanelet_filter.launch.xml new file mode 100644 index 0000000000000..2cccc4f3bb526 --- /dev/null +++ b/perception/detected_object_validation/launch/object_lanelet_filter.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/perception/detected_object_validation/launch/object_position_filter.launch.xml b/perception/detected_object_validation/launch/object_position_filter.launch.xml new file mode 100644 index 0000000000000..49aae282aa3c3 --- /dev/null +++ b/perception/detected_object_validation/launch/object_position_filter.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/perception/detected_object_validation/object-lanelet-filter.md b/perception/detected_object_validation/object-lanelet-filter.md new file mode 100644 index 0000000000000..2f0438b616401 --- /dev/null +++ b/perception/detected_object_validation/object-lanelet-filter.md @@ -0,0 +1,50 @@ +# object_lanelet_filter + +## Purpose + +The `object_lanelet_filter` is a node that filters detected object by using vector map. +The objects only inside of the vector map will be published. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------ | ----------------------------------------------------- | ---------------------- | +| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | +| `input/object` | `autoware_auto_perception_msgs::msg::DetectedObjects` | input detected objects | + +### Output + +| Name | Type | Description | +| --------------- | ----------------------------------------------------- | ------------------------- | +| `output/object` | `autoware_auto_perception_msgs::msg::DetectedObjects` | filtered detected objects | + +## Parameters + +### Core Parameters + +| Name | Type | Default Value | Description | +| -------------------------------- | ---- | ------------- | ----------------------------------------- | +| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. | +| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. | +| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. | +| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. | +| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. | +| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. | +| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. | +| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. | + +## Assumptions / Known limits + +Filtering is performed based on the shape polygon of the object. + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/perception/detected_object_validation/object-position-filter.md b/perception/detected_object_validation/object-position-filter.md new file mode 100644 index 0000000000000..a47379a778f44 --- /dev/null +++ b/perception/detected_object_validation/object-position-filter.md @@ -0,0 +1,53 @@ +# object_position_filter + +## Purpose + +The `object_position_filter` is a node that filters detected object based on x,y values. +The objects only inside of the x, y bound will be published. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------- | ----------------------------------------------------- | ---------------------- | +| `input/object` | `autoware_auto_perception_msgs::msg::DetectedObjects` | input detected objects | + +### Output + +| Name | Type | Description | +| --------------- | ----------------------------------------------------- | ------------------------- | +| `output/object` | `autoware_auto_perception_msgs::msg::DetectedObjects` | filtered detected objects | + +## Parameters + +### Core Parameters + +| Name | Type | Default Value | Description | +| -------------------------------- | ----- | ------------- | --------------------------------------------------------------- | +| `filter_target_label.UNKNOWN` | bool | false | If true, unknown objects are filtered. | +| `filter_target_label.CAR` | bool | false | If true, car objects are filtered. | +| `filter_target_label.TRUCK` | bool | false | If true, truck objects are filtered. | +| `filter_target_label.BUS` | bool | false | If true, bus objects are filtered. | +| `filter_target_label.TRAILER` | bool | false | If true, trailer objects are filtered. | +| `filter_target_label.MOTORCYCLE` | bool | false | If true, motorcycle objects are filtered. | +| `filter_target_label.BICYCLE` | bool | false | If true, bicycle objects are filtered. | +| `filter_target_label.PEDESTRIAN` | bool | false | If true, pedestrian objects are filtered. | +| `upper_bound_x` | float | 100.00 | Bound for filtering. Only used if filter_by_xy_position is true | +| `lower_bound_x` | float | 0.00 | Bound for filtering. Only used if filter_by_xy_position is true | +| `upper_bound_y` | float | 50.00 | Bound for filtering. Only used if filter_by_xy_position is true | +| `lower_bound_y` | float | -50.00 | Bound for filtering. Only used if filter_by_xy_position is true | + +## Assumptions / Known limits + +Filtering is performed based on the center position of the object. + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index de7c820a1a153..afba244eb3cb7 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -12,7 +12,10 @@ autoware_cmake + autoware_auto_mapping_msgs autoware_auto_perception_msgs + geometry_msgs + lanelet2_extension message_filters nav_msgs pcl_conversions diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp new file mode 100644 index 0000000000000..4d5f794324195 --- /dev/null +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -0,0 +1,207 @@ +// Copyright 2022 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_filter/object_lanelet_filter.hpp" + +#include + +#include +#include +#include + +#include + +boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + /*target*/ target_frame_id, /*src*/ source_frame_id, time, + rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("object_lanelet_filter"), ex.what()); + return boost::none; + } +} +namespace object_lanelet_filter +{ +ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & node_options) +: Node("object_lanelet_filter_node", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) +{ + using std::placeholders::_1; + + // Set parameters + filter_target_.UNKNOWN = declare_parameter("filter_target_label.UNKNOWN", false); + filter_target_.CAR = declare_parameter("filter_target_label.CAR", false); + filter_target_.TRUCK = declare_parameter("filter_target_label.TRUCK", false); + filter_target_.BUS = declare_parameter("filter_target_label.BUS", false); + filter_target_.TRAILER = declare_parameter("filter_target_label.TRAILER", false); + 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 publisher/subscriber + map_sub_ = this->create_subscription( + "input/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&ObjectLaneletFilterNode::mapCallback, this, _1)); + object_sub_ = this->create_subscription( + "input/object", rclcpp::QoS{1}, std::bind(&ObjectLaneletFilterNode::objectCallback, this, _1)); + object_pub_ = this->create_publisher( + "output/object", rclcpp::QoS{1}); +} + +void ObjectLaneletFilterNode::mapCallback( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr map_msg) +{ + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*map_msg, lanelet_map_ptr_); + const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); + road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); +} + +void ObjectLaneletFilterNode::objectCallback( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_msg) +{ + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + + // Guard + if (object_pub_->get_subscription_count() < 1) return; + + autoware_auto_perception_msgs::msg::DetectedObjects output_object_msg; + output_object_msg.header = input_msg->header; + + if (!lanelet_map_ptr_) { + RCLCPP_ERROR(get_logger(), "No vector map received."); + return; + } + autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; + if (!transformDetectedObjects(*input_msg, "map", tf_buffer_, transformed_objects)) { + RCLCPP_ERROR(get_logger(), "Failed transform to map."); + return; + } + + // calculate convex hull + const auto convex_hull = getConvexHull(transformed_objects); + // get intersected lanelets + lanelet::ConstLanelets intersected_lanelets = getIntersectedLanelets(convex_hull, road_lanelets_); + + int index = 0; + for (const auto & object : transformed_objects.objects) { + const auto & footprint = object.shape.footprint; + const auto & position = object.kinematics.pose_with_covariance.pose.position; + const auto & label = object.classification.front().label; + if ( + (label == Label::UNKNOWN && filter_target_.UNKNOWN) || + (label == Label::CAR && filter_target_.CAR) || + (label == Label::TRUCK && filter_target_.TRUCK) || + (label == Label::BUS && filter_target_.BUS) || + (label == Label::TRAILER && filter_target_.TRAILER) || + (label == Label::MOTORCYCLE && filter_target_.MOTORCYCLE) || + (label == Label::BICYCLE && filter_target_.BICYCLE) || + (label == Label::PEDESTRIAN && filter_target_.PEDESTRIAN)) { + Polygon2d polygon; + for (const auto & point : footprint.points) { + polygon.outer().emplace_back(point.x + position.x, point.y + position.y); + } + polygon.outer().push_back(polygon.outer().front()); + if (isPolygonOverlapLanelets(polygon, intersected_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; + } + object_pub_->publish(output_object_msg); +} + +bool ObjectLaneletFilterNode::transformDetectedObjects( + const autoware_auto_perception_msgs::msg::DetectedObjects & input_msg, + const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + autoware_auto_perception_msgs::msg::DetectedObjects & output_msg) +{ + output_msg = input_msg; + + /* transform to world coordinate */ + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + { + const auto ros_target2objects_world = + getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); + } + for (auto & object : output_msg.objects) { + tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose); + } + } + return true; +} + +LinearRing2d ObjectLaneletFilterNode::getConvexHull( + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects) +{ + MultiPoint2d candidate_points; + for (const auto & object : detected_objects.objects) { + const auto & pos = object.kinematics.pose_with_covariance.pose.position; + for (const auto & p : object.shape.footprint.points) { + candidate_points.emplace_back(p.x + pos.x, p.y + pos.y); + } + } + + LinearRing2d convex_hull; + boost::geometry::convex_hull(candidate_points, convex_hull); + + return convex_hull; +} + +lanelet::ConstLanelets ObjectLaneletFilterNode::getIntersectedLanelets( + const LinearRing2d & convex_hull, const lanelet::ConstLanelets & road_lanelets) +{ + lanelet::ConstLanelets intersected_lanelets; + for (const auto & road_lanelet : road_lanelets) { + if (boost::geometry::intersects(convex_hull, road_lanelet.polygon2d().basicPolygon())) { + intersected_lanelets.emplace_back(road_lanelet); + } + } + return intersected_lanelets; +} + +bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( + const Polygon2d & polygon, const lanelet::ConstLanelets & intersected_lanelets) +{ + for (const auto & lanelet : intersected_lanelets) { + if (!boost::geometry::disjoint(polygon, lanelet.polygon2d().basicPolygon())) { + return true; + } + } + return false; +} + +} // namespace object_lanelet_filter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(object_lanelet_filter::ObjectLaneletFilterNode) diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp new file mode 100644 index 0000000000000..dee7d613cafe3 --- /dev/null +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -0,0 +1,87 @@ +// Copyright 2022 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_filter/object_position_filter.hpp" + +#include + +namespace object_position_filter +{ +ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & node_options) +: Node("object_position_filter_node", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) +{ + using std::placeholders::_1; + + // Set parameters + upper_bound_x_ = declare_parameter("upper_bound_x", 100.0); + upper_bound_y_ = declare_parameter("upper_bound_y", 50.0); + lower_bound_x_ = declare_parameter("lower_bound_x", 0.0); + lower_bound_y_ = declare_parameter("lower_bound_y", -50.0); + filter_target_.UNKNOWN = declare_parameter("filter_target_label.UNKNOWN", false); + filter_target_.CAR = declare_parameter("filter_target_label.CAR", false); + filter_target_.TRUCK = declare_parameter("filter_target_label.TRUCK", false); + filter_target_.BUS = declare_parameter("filter_target_label.BUS", false); + filter_target_.TRAILER = declare_parameter("filter_target_label.TRAILER", false); + 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 publisher/subscriber + object_sub_ = this->create_subscription( + "input/object", rclcpp::QoS{1}, std::bind(&ObjectPositionFilterNode::objectCallback, this, _1)); + object_pub_ = this->create_publisher( + "output/object", rclcpp::QoS{1}); +} + +void ObjectPositionFilterNode::objectCallback( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_msg) +{ + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + // Guard + if (object_pub_->get_subscription_count() < 1) return; + + autoware_auto_perception_msgs::msg::DetectedObjects output_object_msg; + output_object_msg.header = input_msg->header; + + for (const auto & object : input_msg->objects) { + const auto & position = object.kinematics.pose_with_covariance.pose.position; + const auto & label = object.classification.front().label; + if ( + (label == Label::UNKNOWN && filter_target_.UNKNOWN) || + (label == Label::CAR && filter_target_.CAR) || + (label == Label::TRUCK && filter_target_.TRUCK) || + (label == Label::BUS && filter_target_.BUS) || + (label == Label::TRAILER && filter_target_.TRAILER) || + (label == Label::MOTORCYCLE && filter_target_.MOTORCYCLE) || + (label == Label::BICYCLE && filter_target_.BICYCLE) || + (label == Label::PEDESTRIAN && filter_target_.PEDESTRIAN)) { + if ( + position.x > lower_bound_x_ && position.x < upper_bound_x_ && position.y > lower_bound_y_ && + position.y < upper_bound_y_) { + output_object_msg.objects.emplace_back(object); + } + } else { + output_object_msg.objects.emplace_back(object); + } + } + + object_pub_->publish(output_object_msg); +} + +} // namespace object_position_filter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(object_position_filter::ObjectPositionFilterNode)