feat(detected_object_validation): add BOUNDING_BOX interface to lanel…

…et filter (autowarefoundation#3957)

* feature(detected_object_validation): add BOUNDING_BOX interface to lanelet filter

Signed-off-by: scepter914 <scepter914@gmail.com>

* update README

Signed-off-by: scepter914 <scepter914@gmail.com>

* avoid reuse msg data

Signed-off-by: scepter914 <scepter914@gmail.com>

---------

Signed-off-by: scepter914 <scepter914@gmail.com>

perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp

@@ -63,6 +63,8 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   lanelet::ConstLanelets getIntersectedLanelets(
     const LinearRing2d &, const lanelet::ConstLanelets &);
   bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &);
+  geometry_msgs::msg::Polygon setFootprint(
+    const autoware_auto_perception_msgs::msg::DetectedObject &);
 };
 
 }  // namespace object_lanelet_filter

perception/detected_object_validation/object-lanelet-filter.md

@@ -2,7 +2,7 @@
 
 ## Purpose
 
-The `object_lanelet_filter` is a node that filters detected object by using vector map.  
+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
@@ -39,7 +39,7 @@ The objects only inside of the vector map will be published.
 
 ## Assumptions / Known limits
 
-Filtering is performed based on the shape polygon of the object.
+The lanelet filter is performed based on the shape polygon and bounding box of the objects.
 
 ## (Optional) Error detection and handling
 

perception/detected_object_validation/src/object_lanelet_filter.cpp

@@ -89,7 +89,7 @@ void ObjectLaneletFilterNode::objectCallback(
 
   int index = 0;
   for (const auto & object : transformed_objects.objects) {
-    const auto & footprint = object.shape.footprint;
+    const auto footprint = setFootprint(object);
     const auto & label = object.classification.front().label;
     if (filter_target_.isTarget(label)) {
       Polygon2d polygon;
@@ -110,13 +110,39 @@ void ObjectLaneletFilterNode::objectCallback(
   object_pub_->publish(output_object_msg);
 }
 
+geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint(
+  const autoware_auto_perception_msgs::msg::DetectedObject & detected_object)
+{
+  geometry_msgs::msg::Polygon footprint;
+  if (detected_object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
+    const auto object_size = detected_object.shape.dimensions;
+    const double x_front = object_size.x / 2.0;
+    const double x_rear = -object_size.x / 2.0;
+    const double y_left = object_size.y / 2.0;
+    const double y_right = -object_size.y / 2.0;
+
+    footprint.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(x_front).y(y_left).z(0.0));
+    footprint.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(x_front).y(y_right).z(0.0));
+    footprint.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(x_rear).y(y_right).z(0.0));
+    footprint.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(x_rear).y(y_left).z(0.0));
+  } else {
+    footprint = detected_object.shape.footprint;
+  }
+  return footprint;
+}
+
 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) {
+    const auto footprint = setFootprint(object);
+    for (const auto & p : footprint.points) {
       candidate_points.emplace_back(p.x + pos.x, p.y + pos.y);
     }
   }