refactor(behavior_path_plannner): commonize collision check markers (a…

…utowarefoundation#4798) * refactor(behavior_path_plannner): commonize collision check markers Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Update planning/behavior_path_planner/src/marker_utils/utils.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --------- Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
tier4 · Sep 12, 2023 · 1efcac7 · 1efcac7
1 parent 156174f
commit 1efcac7
Show file tree

Hide file tree

Showing 5 changed files with 13 additions and 159 deletions.
diff --git a/...ng/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/...ng/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp
@@ -30,12 +30,8 @@ using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
 // MarkerArray showAllValidLaneChangePath(
 //   const std::vector<LaneChangePath> & lanes, std::string && ns);
 using visualization_msgs::msg::MarkerArray;
-MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
 MarkerArray showAllValidLaneChangePath(
   const std::vector<LaneChangePath> & lanes, std::string && ns);
-MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
-MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns);
-MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, 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);

diff --git a/...nclude/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/...nclude/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp
@@ -180,20 +180,21 @@ struct SafetyCheckParams
 struct CollisionCheckDebug
 {
   std::string unsafe_reason;                ///< Reason indicating unsafe situation.
-  Pose current_pose{};                      ///< Ego vehicle's current pose.
   Twist current_twist{};                    ///< Ego vehicle's current velocity and rotation.
-  Twist object_twist{};                     ///< Detected object's velocity and rotation.
   Pose expected_ego_pose{};                 ///< Predicted future pose of ego vehicle.
+  Pose current_obj_pose{};                  ///< Detected object's current pose.
+  Twist object_twist{};                     ///< Detected object's velocity and rotation.
   Pose expected_obj_pose{};                 ///< Predicted future pose of object.
   double rss_longitudinal{0.0};             ///< Longitudinal RSS measure.
   double inter_vehicle_distance{0.0};       ///< Distance between ego vehicle and object.
   double extended_polygon_lon_offset{0.0};  ///< Longitudinal offset for extended polygon.
   double extended_polygon_lat_offset{0.0};  ///< Lateral offset for extended polygon.
   bool is_front{false};                     ///< True if object is in front of ego vehicle.
   bool is_safe{false};                      ///< True if situation is deemed safe.
-  std::vector<Pose> lerped_path;            ///< Interpolated ego vehicle path.
-  Polygon2d extended_ego_polygon{};         ///< Ego vehicle's extended collision polygon.
-  Polygon2d extended_obj_polygon{};         ///< Detected object's extended collision polygon.
+  std::vector<PoseWithVelocityStamped> ego_predicted_path;  ///< ego vehicle's predicted path.
+  std::vector<PoseWithVelocityAndPolygonStamped> obj_predicted_path;  ///< object's predicted path.
+  Polygon2d extended_ego_polygon{};  ///< Ego vehicle's extended collision polygon.
+  Polygon2d extended_obj_polygon{};  ///< Detected object's extended collision polygon.
 };
 using CollisionCheckDebugPair = std::pair<std::string, CollisionCheckDebug>;
 using CollisionCheckDebugMap =

diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp
@@ -36,40 +36,6 @@ using geometry_msgs::msg::Point;
 using tier4_autoware_utils::createDefaultMarker;
 using tier4_autoware_utils::createMarkerScale;
 
-MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
-{
-  Marker obj_marker = createDefaultMarker(
-    "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING,
-    createMarkerScale(0.5, 0.5, 0.5), colors::aqua());
-
-  MarkerArray marker_array;
-  int32_t id{0};
-
-  marker_array.markers.reserve(obj_debug_vec.size());
-
-  int idx{0};
-
-  for (const auto & [uuid, info] : obj_debug_vec) {
-    obj_marker.id = ++id;
-    obj_marker.pose = info.current_pose;
-
-    std::ostringstream ss;
-
-    ss << "Idx: " << ++idx << "\nReason: " << info.unsafe_reason
-       << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal
-       << "\nEgo to obj: " << info.inter_vehicle_distance
-       << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset
-       << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset
-       << "\nPosition: " << (info.is_front ? "front" : "back")
-       << "\nSafe: " << (info.is_safe ? "Yes" : "No");
-
-    obj_marker.text = ss.str();
-
-    marker_array.markers.push_back(obj_marker);
-  }
-  return marker_array;
-}
-
 MarkerArray showAllValidLaneChangePath(const std::vector<LaneChangePath> & lanes, std::string && ns)
 {
   if (lanes.empty()) {
@@ -103,121 +69,6 @@ MarkerArray showAllValidLaneChangePath(const std::vector<LaneChangePath> & lanes
   return marker_array;
 }
 
-MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
-{
-  MarkerArray marker_array;
-  int32_t id{0};
-  const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
-  marker_array.markers.reserve(obj_debug_vec.size());
-
-  for (const auto & [uuid, info] : obj_debug_vec) {
-    Marker marker = createDefaultMarker(
-      "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3),
-      colors::magenta());
-    marker.points.reserve(info.lerped_path.size());
-
-    for (const auto & point : info.lerped_path) {
-      marker.points.push_back(point.position);
-    }
-
-    marker_array.markers.push_back(marker);
-  }
-  return marker_array;
-}
-
-MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
-{
-  if (obj_debug_vec.empty()) {
-    return MarkerArray{};
-  }
-
-  constexpr float scale_val{0.2};
-  int32_t id{0};
-  const auto now = rclcpp::Clock{RCL_ROS_TIME}.now();
-  Marker ego_marker = createDefaultMarker(
-    "map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val),
-    colors::green());
-  Marker obj_marker = ego_marker;
-
-  auto text_marker = createDefaultMarker(
-    "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
-    createMarkerScale(1.5, 1.5, 1.5), colors::white());
-
-  MarkerArray marker_array;
-
-  const auto reserve_size = obj_debug_vec.size();
-
-  marker_array.markers.reserve(reserve_size * 4);
-
-  int32_t idx = {0};
-
-  for (const auto & [uuid, info] : obj_debug_vec) {
-    const auto color = info.is_safe ? colors::green() : colors::red();
-    const auto & ego_polygon = info.extended_ego_polygon.outer();
-    const auto poly_z = info.current_pose.position.z;  // temporally
-    ego_marker.id = ++id;
-    ego_marker.color = color;
-    ego_marker.points.reserve(ego_polygon.size());
-    for (const auto & p : ego_polygon) {
-      ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z));
-    }
-    marker_array.markers.push_back(ego_marker);
-
-    std::ostringstream ss;
-    text_marker.id = ego_marker.id;
-    ss << ++idx;
-    text_marker.text = ss.str();
-    text_marker.pose = info.expected_ego_pose;
-
-    marker_array.markers.push_back(text_marker);
-
-    const auto & obj_polygon = info.extended_obj_polygon.outer();
-    obj_marker.id = ++id;
-    obj_marker.color = color;
-    obj_marker.points.reserve(obj_polygon.size());
-    for (const auto & p : obj_polygon) {
-      obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z));
-    }
-    marker_array.markers.push_back(obj_marker);
-
-    text_marker.id = obj_marker.id;
-    text_marker.pose = info.expected_obj_pose;
-    marker_array.markers.push_back(text_marker);
-
-    ego_marker.points.clear();
-    obj_marker.points.clear();
-  }
-
-  return marker_array;
-}
-
-MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns)
-{
-  const auto colors = colors::colors_list();
-  const auto loop_size = std::min(colors.size(), obj_debug_vec.size());
-  MarkerArray marker_array;
-  int32_t id{0};
-  size_t idx{0};
-  const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
-  marker_array.markers.reserve(obj_debug_vec.size());
-
-  for (const auto & [uuid, info] : obj_debug_vec) {
-    const auto & color = colors.at(idx);
-    Marker marker = createDefaultMarker(
-      "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), color);
-    marker.points.reserve(2);
-    marker.points.push_back(info.expected_ego_pose.position);
-    marker.points.push_back(info.expected_obj_pose.position);
-    marker_array.markers.push_back(marker);
-    ++idx;
-    if (idx >= loop_size) {
-      break;
-    }
-  }
-
-  return marker_array;
-}
-
 MarkerArray createLaneChangingVirtualWallMarker(
   const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
   const rclcpp::Time & now, const std::string & ns)

diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp
@@ -14,6 +14,7 @@
 
 #include "behavior_path_planner/marker_utils/utils.hpp"
 
+#include "behavior_path_planner/marker_utils/colors.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
@@ -37,7 +38,7 @@ using visualization_msgs::msg::Marker;
 CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj)
 {
   CollisionCheckDebug debug;
-  debug.current_pose = obj.initial_pose.pose;
+  debug.current_obj_pose = obj.initial_pose.pose;
   debug.current_twist = obj.initial_twist.twist;
   return {tier4_autoware_utils::toHexString(obj.uuid), debug};
 }

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
@@ -323,6 +323,11 @@ void LaneChangeInterface::setObjectDebugVisualization() const
     add(showPolygon(debug_data, "ego_and_target_polygon_relation"));
   }
 
+  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"));
+  }
 }
 
 std::shared_ptr<LaneChangeDebugMsgArray> LaneChangeInterface::get_debug_msg_array() const