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)