feat(lane_change): object filter visualization (autowarefoundation#5082)

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

planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp

@@ -17,6 +17,7 @@
 
 #include "behavior_path_planner/marker_utils/utils.hpp"
 #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp"
+#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 
 #include <string>
 #include <unordered_map>
@@ -25,11 +26,16 @@
 namespace marker_utils::lane_change_markers
 {
 using behavior_path_planner::LaneChangePath;
+using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
 using visualization_msgs::msg::MarkerArray;
 MarkerArray showAllValidLaneChangePath(
   const std::vector<LaneChangePath> & lanes, std::string && ns);
 MarkerArray createLaneChangingVirtualWallMarker(
   const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
   const rclcpp::Time & now, const std::string & ns);
+MarkerArray showFilteredObjects(
+  const ExtendedPredictedObjects & current_lane_objects,
+  const ExtendedPredictedObjects & target_lane_objects,
+  const ExtendedPredictedObjects & other_lane_objects, const std::string & ns);
 }  // namespace marker_utils::lane_change_markers
 #endif  // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_

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

@@ -29,6 +29,7 @@
 
 #include <lanelet2_core/geometry/Lanelet.h>
 
+#include <cstdint>
 #include <string>
 #include <vector>
 
@@ -42,6 +43,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::ExtendedPredictedObjects;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Polygon;
 using geometry_msgs::msg::Pose;
@@ -109,6 +111,10 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin
 MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 
 MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
+
+MarkerArray showFilteredObjects(
+  const ExtendedPredictedObjects & predicted_objects, const std::string & ns,
+  const ColorRGBA & color, int32_t id);
 }  // namespace marker_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_

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

@@ -77,6 +77,7 @@ struct ExtendedPredictedObject
   autoware_auto_perception_msgs::msg::Shape shape;
   std::vector<PredictedPathWithPolygon> predicted_paths;
 };
+using ExtendedPredictedObjects = std::vector<ExtendedPredictedObject>;
 
 /**
  * @brief Specifies which object class should be checked.

planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp

@@ -98,4 +98,28 @@ MarkerArray createLaneChangingVirtualWallMarker(
   return marker_array;
 }
 
+MarkerArray showFilteredObjects(
+  const ExtendedPredictedObjects & current_lane_objects,
+  const ExtendedPredictedObjects & target_lane_objects,
+  const ExtendedPredictedObjects & other_lane_objects, const std::string & ns)
+{
+  int32_t update_id = 0;
+  auto current_marker =
+    marker_utils::showFilteredObjects(current_lane_objects, ns, colors::yellow(), update_id);
+  update_id += static_cast<int32_t>(current_marker.markers.size());
+  auto target_marker =
+    marker_utils::showFilteredObjects(target_lane_objects, ns, colors::aqua(), update_id);
+  update_id += static_cast<int32_t>(target_marker.markers.size());
+  auto other_marker =
+    marker_utils::showFilteredObjects(other_lane_objects, ns, colors::medium_orchid(), update_id);
+
+  MarkerArray marker_array;
+  marker_array.markers.insert(
+    marker_array.markers.end(), current_marker.markers.begin(), current_marker.markers.end());
+  marker_array.markers.insert(
+    marker_array.markers.end(), target_marker.markers.begin(), target_marker.markers.end());
+  marker_array.markers.insert(
+    marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end());
+  return marker_array;
+}
 }  // namespace marker_utils::lane_change_markers

planning/behavior_path_planner/src/marker_utils/utils.cpp

@@ -21,6 +21,8 @@
 
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
+#include <cstdint>
+
 namespace marker_utils
 {
 using behavior_path_planner::ShiftLine;
@@ -645,4 +647,40 @@ MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, st
   return marker_array;
 }
 
+MarkerArray showFilteredObjects(
+  const ExtendedPredictedObjects & predicted_objects, const std::string & ns,
+  const ColorRGBA & color, int32_t id)
+{
+  using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
+  if (predicted_objects.empty()) {
+    return MarkerArray{};
+  }
+
+  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(
+    predicted_objects.size());  // poly ego, text ego, poly obj, text obj, cube obj
+
+  std::for_each(
+    predicted_objects.begin(), predicted_objects.end(), [&](const ExtendedPredictedObject & obj) {
+      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;
+}
+
 }  // namespace marker_utils

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

@@ -15,6 +15,7 @@
 #include "behavior_path_planner/scene_module/lane_change/interface.hpp"
 
 #include "behavior_path_planner/marker_utils/lane_change/debug.hpp"
+#include "behavior_path_planner/marker_utils/utils.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 #include "behavior_path_planner/utils/lane_change/utils.hpp"
@@ -305,17 +306,22 @@ void LaneChangeInterface::setObjectDebugVisualization() const
   using marker_utils::showPredictedPath;
   using marker_utils::showSafetyCheckInfo;
   using marker_utils::lane_change_markers::showAllValidLaneChangePath;
+  using marker_utils::lane_change_markers::showFilteredObjects;
 
   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 debug_filtered_objects = module_type_->getDebugFilteredObjects();
 
   debug_marker_.markers.clear();
   const auto add = [this](const MarkerArray & added) {
     tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
   };
 
   add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths"));
+  add(showFilteredObjects(
+    debug_filtered_objects.current_lane, debug_filtered_objects.target_lane,
+    debug_filtered_objects.other_lane, "object_filtered"));
   if (!debug_data.empty()) {
     add(showSafetyCheckInfo(debug_data, "object_debug_info"));
     add(showPredictedPath(debug_data, "ego_predicted_path"));

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

@@ -934,6 +934,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;
 
   const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes);
 
@@ -1150,6 +1151,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(