feat(lane_change): object filter visualization

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp

@@ -39,6 +39,7 @@ using behavior_path_planner::ShiftLineArray;
 using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap;
 using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair;
 using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
+using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Polygon;
 using geometry_msgs::msg::Pose;
@@ -103,6 +104,8 @@ MarkerArray createPredictedPathMarkerArray(
 
 MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 
+MarkerArray showPolygon(const TargetObjectsOnLane & filtered_objects, std::string && ns);
+
 MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 
 MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

@@ -154,6 +154,11 @@ class LaneChangeBase
     return object_debug_after_approval_;
   }
 
+  const LaneChangeTargetObjects & getDebugFilteredObjects() const
+  {
+    return debug_filtered_objects_;
+  }
+
   const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; }
 
   const Point & getEgoPosition() const { return getEgoPose().position; }
@@ -262,6 +267,7 @@ class LaneChangeBase
   mutable LaneChangePaths debug_valid_path_{};
   mutable CollisionCheckDebugMap object_debug_{};
   mutable CollisionCheckDebugMap object_debug_after_approval_{};
+  mutable LaneChangeTargetObjects debug_filtered_objects_{};
   mutable double object_debug_lifetime_{0.0};
   mutable StopWatch<std::chrono::milliseconds> stop_watch_;
 };

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp

@@ -115,6 +115,18 @@ struct TargetObjectsOnLane
   std::vector<ExtendedPredictedObject> on_left_lane{};      ///< Objects on the left lane.
   std::vector<ExtendedPredictedObject> on_shoulder_lane{};  ///< Objects on the shoulder lane.
   std::vector<ExtendedPredictedObject> on_other_lane{};     ///< Objects on other lanes.
+
+  [[nodiscard]] bool empty() const
+  {
+    return on_current_lane.empty() && on_right_lane.empty() && on_left_lane.empty() &&
+           on_shoulder_lane.empty() && on_other_lane.empty();
+  }
+
+  [[nodiscard]] bool size() const
+  {
+    return on_current_lane.empty() + on_right_lane.empty() + on_left_lane.empty() +
+           on_shoulder_lane.empty() + on_other_lane.empty();
+  }
 };
 
 /**

planning/behavior_path_planner/src/marker_utils/utils.cpp

@@ -31,6 +31,7 @@ namespace marker_utils
 using behavior_path_planner::ShiftLine;
 using behavior_path_planner::utils::calcPathArcLengthArray;
 using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
+using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane;
 using std_msgs::msg::ColorRGBA;
 using tier4_autoware_utils::calcOffsetPose;
 using tier4_autoware_utils::createDefaultMarker;
@@ -492,6 +493,57 @@ MarkerArray createPredictedPathMarkerArray(
   return msg;
 }
 
+MarkerArray showPolygon(const TargetObjectsOnLane & filtered_objects, std::string && ns)
+{
+  using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
+  if (filtered_objects.empty()) {
+    return MarkerArray{};
+  }
+
+  int32_t id{0};
+  const auto now = rclcpp::Clock{RCL_ROS_TIME}.now();
+
+  auto default_cube_marker =
+    [&](const auto & width, const auto & depth, const auto & color = colors::green()) {
+      return createDefaultMarker(
+        "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE,
+        createMarkerScale(width, depth, 1.0), color);
+    };
+
+  MarkerArray marker_array;
+  marker_array.markers.reserve(
+    filtered_objects.size());  // poly ego, text ego, poly obj, text obj, cube obj
+
+  const auto & current_lane_obj = filtered_objects.on_current_lane;
+  std::for_each(
+    current_lane_obj.begin(), current_lane_obj.end(), [&](const ExtendedPredictedObject & obj) {
+      const auto color = colors::yellow();
+
+      const auto insert_cube_marker = [&](const auto & pose) {
+        marker_array.markers.emplace_back();
+        auto & cube_marker = marker_array.markers.back();
+        cube_marker = default_cube_marker(1.0, 1.0, color);
+        cube_marker.pose = pose;
+      };
+      insert_cube_marker(obj.initial_pose.pose);
+    });
+
+  const auto & right_lane_obj = filtered_objects.on_right_lane;
+  std::for_each(
+    right_lane_obj.begin(), right_lane_obj.end(), [&](const ExtendedPredictedObject & obj) {
+      const auto color = colors::green();
+
+      const auto insert_cube_marker = [&](const auto & pose) {
+        marker_array.markers.emplace_back();
+        auto & cube_marker = marker_array.markers.back();
+        cube_marker = default_cube_marker(1.0, 1.0, color);
+        cube_marker.pose = pose;
+      };
+      insert_cube_marker(obj.initial_pose.pose);
+    });
+  return marker_array;
+}
+
 MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
 {
   if (obj_debug_vec.empty()) {
@@ -504,8 +556,10 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin
   constexpr float line_scale_val{0.2};
   const auto line_marker_scale = createMarkerScale(line_scale_val, line_scale_val, line_scale_val);
 
-  auto default_line_marker = [&](const auto & color = colors::green()) {
-    return createDefaultMarker("map", now, ns, ++id, Marker::LINE_STRIP, line_marker_scale, color);
+  auto default_line_marker = [&](bool is_safe, const auto & color = colors::green()) {
+    return createDefaultMarker(
+      "map", now, ns + (is_safe ? "_safe" : "_unsafe"), ++id, Marker::LINE_STRIP, line_marker_scale,
+      color);
   };
 
   constexpr float text_scale_val{1.5};
@@ -536,7 +590,7 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin
     const auto insert_polygon_marker = [&](const auto & polygon) {
       marker_array.markers.emplace_back();
       auto & polygon_marker = marker_array.markers.back();
-      polygon_marker = default_line_marker(color);
+      polygon_marker = default_line_marker(info.is_safe, color);
       polygon_marker.points.reserve(polygon.outer().size());
       for (const auto & p : polygon.outer()) {
         polygon_marker.points.push_back(createPoint(p.x(), p.y(), poly_z));

planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp

@@ -297,6 +297,15 @@ void LaneChangeInterface::setObjectDebugVisualization() const
   const auto debug_data = module_type_->getDebugData();
   const auto debug_after_approval = module_type_->getAfterApprovalDebugData();
   const auto debug_valid_path = module_type_->getDebugValidPath();
+  const auto before = module_type_->getDebugFilteredObjects();
+  const auto debug_object_filtered = std::invoke([&]() {
+    utils::path_safety_checker::TargetObjectsOnLane objs;
+    objs.on_current_lane.insert(
+      objs.on_current_lane.end(), before.current_lane.begin(), before.current_lane.end());
+    objs.on_right_lane.insert(
+      objs.on_right_lane.end(), before.target_lane.begin(), before.target_lane.end());
+    return objs;
+  });
 
   debug_marker_.markers.clear();
   const auto add = [this](const MarkerArray & added) {
@@ -308,12 +317,14 @@ void LaneChangeInterface::setObjectDebugVisualization() const
     add(showSafetyCheckInfo(debug_data, "object_debug_info"));
     add(showPredictedPath(debug_data, "ego_predicted_path"));
     add(showPolygon(debug_data, "ego_and_target_polygon_relation"));
+    add(showPolygon(debug_object_filtered, "object_filtered_before_approval"));
   }
 
   if (!debug_after_approval.empty()) {
     add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval"));
     add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval"));
     add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval"));
+    add(showPolygon(debug_object_filtered, "object_filtered_after_approval"));
   }
 }
 

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -727,8 +727,12 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
     if (
       target_backward_polygon &&
       boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) {
-      filtered_obj_indices.target_lane.push_back(i);
-      continue;
+      const auto lateral = tier4_autoware_utils::calcLateralDeviation(
+        current_pose, object.kinematics.initial_pose_with_covariance.pose.position);
+      if (std::abs(lateral) > common_parameters.vehicle_width) {
+        filtered_obj_indices.target_lane.push_back(i);
+        continue;
+      }
     }
 
     // check if the object intersects with current lanes
@@ -875,6 +879,7 @@ bool NormalLaneChange::getLaneChangePaths(
     utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_);
 
   const auto target_objects = getTargetObjects(current_lanes, target_lanes);
+  debug_filtered_objects_ = target_objects;
 
   candidate_paths->reserve(longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num);
 
@@ -1066,6 +1071,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   const auto & target_lanes = status_.target_lanes;
 
   const auto target_objects = getTargetObjects(current_lanes, target_lanes);
+  debug_filtered_objects_ = target_objects;
 
   CollisionCheckDebugMap debug_data;
   const auto safety_status = isLaneChangePathSafe(
@@ -1366,10 +1372,12 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     auto current_debug_data = marker_utils::createObjectDebug(obj);
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       obj, lane_change_parameters_->use_all_predicted_path);
+    auto is_safe{true};
     for (const auto & obj_path : obj_predicted_paths) {
-      if (!utils::path_safety_checker::checkCollision(
-            path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
-            current_debug_data.second)) {
+      is_safe = utils::path_safety_checker::checkCollision(
+        path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
+        current_debug_data.second);
+      if (!is_safe) {
         path_safety_status.is_safe = false;
         marker_utils::updateCollisionCheckDebugMap(
           debug_data, current_debug_data, path_safety_status.is_safe);
@@ -1380,8 +1388,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
             path, current_pose, common_parameters.vehicle_info, obj_polygon);
       }
     }
-    marker_utils::updateCollisionCheckDebugMap(
-      debug_data, current_debug_data, path_safety_status.is_safe);
+    marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe);
   }
 
   return path_safety_status;

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

@@ -117,8 +117,8 @@ Polygon2d createExtendedPolygon(
 }
 
 Polygon2d createExtendedPolygon(
-  const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin,
-  CollisionCheckDebug & debug)
+  const Pose & obj_pose, const Shape & shape, const double lon_length,
+  [[maybe_unused]] const double lat_margin, CollisionCheckDebug & debug)
 {
   const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, shape);
   if (obj_polygon.outer().empty()) {
@@ -140,8 +140,9 @@ Polygon2d createExtendedPolygon(
   }
 
   const double lon_offset = max_x + lon_length;
-  const double left_lat_offset = max_y + lat_margin;
-  const double right_lat_offset = min_y - lat_margin;
+  constexpr double percentage = 0.0;
+  const double left_lat_offset = max_y + lat_margin * percentage;
+  const double right_lat_offset = min_y - lat_margin * percentage;
 
   {
     debug.extended_polygon_lon_offset = lon_offset;