refactor(bpp): simplify ExtendedPredictedObject and add new member va…

…riables (autowarefoundation#8889)

* simplify ExtendedPredictedObject and add new member variables

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* replace self polygon to initial polygon

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* comment

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* add comments to dist of ego

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -445,14 +445,14 @@ void NormalLaneChange::insertStopPoint(
 
     for (const auto & object : target_objects.current_lane) {
       // check if stationary
-      const auto obj_v = std::abs(object.initial_twist.twist.linear.x);
+      const auto obj_v = std::abs(object.initial_twist.linear.x);
       if (obj_v > lane_change_parameters_->stopped_object_velocity_threshold) {
         continue;
       }
 
       // calculate distance from path front to the stationary object polygon on the ego lane.
       const auto polygon =
-        autoware::universe_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer();
+        autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape).outer();
       for (const auto & polygon_p : polygon) {
         const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d());
         const auto lateral_fp = autoware::motion_utils::calcLateralOffset(path.points, p_fp);
@@ -499,21 +499,21 @@ void NormalLaneChange::insertStopPoint(
     const bool has_blocking_target_lane_obj = std::any_of(
       target_objects.target_lane_leading.begin(), target_objects.target_lane_leading.end(),
       [&](const auto & o) {
-        const auto v = std::abs(o.initial_twist.twist.linear.x);
+        const auto v = std::abs(o.initial_twist.linear.x);
         if (v > lane_change_parameters_->stopped_object_velocity_threshold) {
           return false;
         }
 
         // target_objects includes objects out of target lanes, so filter them out
         if (!boost::geometry::intersects(
-              autoware::universe_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(),
+              autoware::universe_utils::toPolygon2d(o.initial_pose, o.shape).outer(),
               lanelet::utils::combineLaneletsShape(get_target_lanes())
                 .polygon2d()
                 .basicPolygon())) {
           return false;
         }
 
-        const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose);
+        const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose);
         return stopping_distance_for_obj < distance_to_target_lane_obj &&
                distance_to_target_lane_obj < distance_to_ego_lane_obj;
       });
@@ -2198,10 +2198,10 @@ bool NormalLaneChange::has_collision_with_decel_patterns(
         utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution);
 
       return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) {
-        const auto selected_rss_param = (obj.initial_twist.twist.linear.x <=
-                                         lane_change_parameters_->stopped_object_velocity_threshold)
-                                          ? lane_change_parameters_->rss_params_for_parked
-                                          : rss_param;
+        const auto selected_rss_param =
+          (obj.initial_twist.linear.x <= lane_change_parameters_->stopped_object_velocity_threshold)
+            ? lane_change_parameters_->rss_params_for_parked
+            : rss_param;
         return is_collided(
           lane_change_path.path, obj, ego_predicted_path, selected_rss_param, debug_data);
       });
@@ -2295,10 +2295,10 @@ bool NormalLaneChange::isVehicleStuck(
   const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length;
 
   for (const auto & object : lane_change_debug_.filtered_objects.current_lane) {
-    const auto & p = object.initial_pose.pose;  // TODO(Horibe): consider footprint point
+    const auto & p = object.initial_pose;  // TODO(Horibe): consider footprint point
 
     // Note: it needs chattering prevention.
-    if (std::abs(object.initial_twist.twist.linear.x) > 0.3) {  // check if stationary
+    if (std::abs(object.initial_twist.linear.x) > 0.3) {  // check if stationary
       continue;
     }
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp

@@ -734,12 +734,12 @@ bool isParkedObject(
   using lanelet::geometry::toArcCoordinates;
 
   const double object_vel_norm =
-    std::hypot(object.initial_twist.twist.linear.x, object.initial_twist.twist.linear.y);
+    std::hypot(object.initial_twist.linear.x, object.initial_twist.linear.y);
   if (object_vel_norm > static_object_velocity_threshold) {
     return false;
   }
 
-  const auto & object_pose = object.initial_pose.pose;
+  const auto & object_pose = object.initial_pose;
   const auto object_closest_index =
     autoware::motion_utils::findNearestIndex(path.points, object_pose.position);
   const auto object_closest_pose = path.points.at(object_closest_index).point.pose;
@@ -806,7 +806,7 @@ bool isParkedObject(
 {
   using lanelet::geometry::distance2d;
 
-  const auto & obj_pose = object.initial_pose.pose;
+  const auto & obj_pose = object.initial_pose;
   const auto & obj_shape = object.shape;
   const auto obj_poly = autoware::universe_utils::toPolygon2d(obj_pose, obj_shape);
   const auto obj_point = obj_pose.position;
@@ -867,7 +867,7 @@ bool passed_parked_objects(
   const auto & leading_obj = objects.at(*leading_obj_idx);
   auto debug = utils::path_safety_checker::createObjectDebug(leading_obj);
   const auto leading_obj_poly =
-    autoware::universe_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape);
+    autoware::universe_utils::toPolygon2d(leading_obj.initial_pose, leading_obj.shape);
   if (leading_obj_poly.outer().empty()) {
     return true;
   }
@@ -899,7 +899,7 @@ bool passed_parked_objects(
 
   const auto current_pose = common_data_ptr->get_ego_pose();
   const auto dist_ego_to_obj = motion_utils::calcSignedArcLength(
-    current_lane_path.points, current_pose.position, leading_obj.initial_pose.pose.position);
+    current_lane_path.points, current_pose.position, leading_obj.initial_pose.position);
 
   if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) {
     return true;
@@ -928,12 +928,11 @@ std::optional<size_t> getLeadingStaticObjectIdx(
   std::optional<size_t> leading_obj_idx = std::nullopt;
   for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) {
     const auto & obj = objects.at(obj_idx);
-    const auto & obj_pose = obj.initial_pose.pose;
+    const auto & obj_pose = obj.initial_pose;
 
     // ignore non-static object
     // TODO(shimizu): parametrize threshold
-    const double obj_vel_norm =
-      std::hypot(obj.initial_twist.twist.linear.x, obj.initial_twist.twist.linear.y);
+    const double obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y);
     if (obj_vel_norm > 1.0) {
       continue;
     }
@@ -989,8 +988,8 @@ ExtendedPredictedObject transform(
   const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration;
   const auto & velocity_threshold = lane_change_parameters.stopped_object_velocity_threshold;
   const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration;
-  const double obj_vel_norm = std::hypot(
-    extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y);
+  const double obj_vel_norm =
+    std::hypot(extended_object.initial_twist.linear.x, extended_object.initial_twist.linear.y);
 
   extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size());
   for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) {

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp

@@ -16,14 +16,15 @@
 #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_  // NOLINT
 
 #include <autoware/universe_utils/geometry/boost_geometry.hpp>
+#include <autoware/universe_utils/geometry/boost_polygon_utils.hpp>
 
 #include <autoware_perception_msgs/msg/predicted_object.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 
 #include <boost/uuid/uuid_hash.hpp>
 
-#include <cmath>
+#include <algorithm>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -33,7 +34,9 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker
 {
 
 using autoware::universe_utils::Polygon2d;
+using autoware_perception_msgs::msg::ObjectClassification;
 using autoware_perception_msgs::msg::PredictedObject;
+using autoware_perception_msgs::msg::Shape;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 
@@ -60,8 +63,8 @@ struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped
   Polygon2d poly;
 
   PoseWithVelocityAndPolygonStamped(
-    const double time, const Pose & pose, const double velocity, const Polygon2d & poly)
-  : PoseWithVelocityStamped(time, pose, velocity), poly(poly)
+    const double time, const Pose & pose, const double velocity, Polygon2d poly)
+  : PoseWithVelocityStamped(time, pose, velocity), poly(std::move(poly))
   {
   }
 };
@@ -75,22 +78,30 @@ struct PredictedPathWithPolygon
 struct ExtendedPredictedObject
 {
   unique_identifier_msgs::msg::UUID uuid;
-  geometry_msgs::msg::PoseWithCovariance initial_pose;
-  geometry_msgs::msg::TwistWithCovariance initial_twist;
-  geometry_msgs::msg::AccelWithCovariance initial_acceleration;
-  autoware_perception_msgs::msg::Shape shape;
-  std::vector<autoware_perception_msgs::msg::ObjectClassification> classification;
+  Pose initial_pose;
+  Twist initial_twist;
+  Shape shape;
+  ObjectClassification classification;
+  Polygon2d initial_polygon;
   std::vector<PredictedPathWithPolygon> predicted_paths;
+  double dist_from_ego{0.0};  ///< Distance from ego to obj, can be arc length or euclidean.
 
   ExtendedPredictedObject() = default;
   explicit ExtendedPredictedObject(const PredictedObject & object)
   : uuid(object.object_id),
-    initial_pose(object.kinematics.initial_pose_with_covariance),
-    initial_twist(object.kinematics.initial_twist_with_covariance),
-    initial_acceleration(object.kinematics.initial_acceleration_with_covariance),
-    shape(object.shape),
-    classification(object.classification)
+    initial_pose(object.kinematics.initial_pose_with_covariance.pose),
+    initial_twist(object.kinematics.initial_twist_with_covariance.twist),
+    shape(object.shape)
   {
+    classification.label = std::invoke([&]() {
+      auto max_elem = std::max_element(
+        object.classification.begin(), object.classification.end(),
+        [](const auto & a, const auto & b) { return a.probability < b.probability; });
+
+      return (max_elem != object.classification.end()) ? max_elem->label
+                                                       : ObjectClassification::UNKNOWN;
+    });
+    initial_polygon = autoware::universe_utils::toPolygon2d(initial_pose, shape);
   }
 };
 using ExtendedPredictedObjects = std::vector<ExtendedPredictedObject>;

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp

@@ -608,7 +608,7 @@ MarkerArray showFilteredObjects(
         cube_marker = default_cube_marker(1.0, 1.0, color);
         cube_marker.pose = pose;
       };
-      insert_cube_marker(obj.initial_pose.pose);
+      insert_cube_marker(obj.initial_pose);
     });
 
   return marker_array;

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp

@@ -327,7 +327,7 @@ ExtendedPredictedObject transform(
 {
   ExtendedPredictedObject extended_object(object);
 
-  const auto obj_velocity = extended_object.initial_twist.twist.linear.x;
+  const auto obj_velocity = extended_object.initial_twist.linear.x;
 
   extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size());
   for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) {

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

@@ -590,7 +590,7 @@ std::vector<Polygon2d> getCollidedPolygons(
   {
     debug.ego_predicted_path = predicted_ego_path;
     debug.obj_predicted_path = target_object_path.path;
-    debug.current_obj_pose = target_object.initial_pose.pose;
+    debug.current_obj_pose = target_object.initial_pose;
   }
 
   std::vector<Polygon2d> collided_polygons{};
@@ -712,11 +712,10 @@ bool checkPolygonsIntersects(
 CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj)
 {
   CollisionCheckDebug debug;
-  debug.current_obj_pose = obj.initial_pose.pose;
-  debug.extended_obj_polygon =
-    autoware::universe_utils::toPolygon2d(obj.initial_pose.pose, obj.shape);
+  debug.current_obj_pose = obj.initial_pose;
+  debug.extended_obj_polygon = autoware::universe_utils::toPolygon2d(obj.initial_pose, obj.shape);
   debug.obj_shape = obj.shape;
-  debug.current_twist = obj.initial_twist.twist;
+  debug.current_twist = obj.initial_twist;
   return {autoware::universe_utils::toBoostUUID(obj.uuid), debug};
 }
 

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -537,7 +537,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose &
       target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(),
       [&](const auto & o) {
         const auto arc_length = autoware::motion_utils::calcSignedArcLength(
-          centerline_path.points, ego_pose.position, o.initial_pose.pose.position);
+          centerline_path.points, ego_pose.position, o.initial_pose.position);
         if (arc_length > 0.0) return;
         if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return;
         arc_length_to_closet_object = arc_length;

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

@@ -838,20 +838,20 @@ bool StaticObstacleAvoidanceModule::isSafePath(
     auto current_debug_data = utils::path_safety_checker::createObjectDebug(object);
 
     const auto obj_polygon =
-      autoware::universe_utils::toPolygon2d(object.initial_pose.pose, object.shape);
+      autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape);
 
     const auto is_object_front =
       utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info);
 
-    const auto & object_twist = object.initial_twist.twist;
+    const auto & object_twist = object.initial_twist;
     const auto v_norm = std::hypot(object_twist.linear.x, object_twist.linear.y);
-    const auto object_type = utils::getHighestProbLabel(object.classification);
-    const auto object_parameter = parameters_->object_parameters.at(object_type);
+    const auto object_type = object.classification;
+    const auto object_parameter = parameters_->object_parameters.at(object_type.label);
     const auto is_object_moving = v_norm > object_parameter.moving_speed_threshold;
 
     const auto is_object_oncoming =
       is_object_moving &&
-      utils::path_safety_checker::isTargetObjectOncoming(getEgoPose(), object.initial_pose.pose);
+      utils::path_safety_checker::isTargetObjectOncoming(getEgoPose(), object.initial_pose);
 
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       object, parameters_->check_all_predicted_path);