refactor(behavior_path_planner): renaming and comment collision check…

… debug (autowarefoundation#4749) * refactor(behavior_path_planner): renaming and comment collision check debug Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix spell check Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
tier4 · Sep 14, 2023 · 982526d · 982526d
1 parent ac9786f
commit 982526d
Show file tree

Hide file tree

Showing 5 changed files with 50 additions and 51 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp
@@ -51,24 +51,22 @@ using visualization_msgs::msg::MarkerArray;
 
 struct CollisionCheckDebug
 {
-  std::string failed_reason;
-  std::size_t lane_id{0};
-  Pose current_pose{};
-  Twist current_twist{};
-  Twist object_twist{};
-  Pose expected_ego_pose{};
-  Pose expected_obj_pose{};
-  Pose relative_to_ego{};
-  double rss_longitudinal{0.0};
-  double ego_to_obj_margin{0.0};
-  double longitudinal_offset{0.0};
-  double lateral_offset{0.0};
-  bool is_front{false};
-  bool allow_lane_change{false};
-  std::vector<Pose> lerped_path;
-  std::vector<PredictedPath> ego_predicted_path{};
-  Polygon2d ego_polygon{};
-  Polygon2d obj_polygon{};
+  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 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.
+  std::vector<PredictedPath> ego_predicted_path{};  ///< Predicted future path of ego vehicle.
+  Polygon2d extended_ego_polygon{};                 ///< Ego vehicle's extended collision polygon.
+  Polygon2d extended_obj_polygon{};  ///< Detected object's extended collision polygon.
 };
 using CollisionCheckDebugMap = std::unordered_map<std::string, CollisionCheckDebug>;
 

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
@@ -56,12 +56,13 @@ MarkerArray showObjectInfo(
 
     std::ostringstream ss;
 
-    ss << "Idx: " << ++idx << "\nReason: " << info.failed_reason
+    ss << "Idx: " << ++idx << "\nReason: " << info.unsafe_reason
        << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal
-       << "\nEgo to obj: " << info.ego_to_obj_margin << "\nLateral offset: " << info.lateral_offset
-       << "\nLongitudinal offset: " << info.longitudinal_offset
+       << "\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.allow_lane_change ? "Yes" : "No");
+       << "\nSafe: " << (info.is_safe ? "Yes" : "No");
 
     obj_marker.text = ss.str();
 
@@ -195,8 +196,8 @@ MarkerArray showPolygon(
   int32_t idx = {0};
 
   for (const auto & [uuid, info] : obj_debug_vec) {
-    const auto & color = info.allow_lane_change ? green_color : red_color;
-    const auto & ego_polygon = info.ego_polygon.outer();
+    const auto & color = info.is_safe ? green_color : red_color;
+    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 = createMarkerColor(color[0], color[1], color[2], 0.8);
@@ -214,7 +215,7 @@ MarkerArray showPolygon(
 
     marker_array.markers.push_back(text_marker);
 
-    const auto & obj_polygon = info.obj_polygon.outer();
+    const auto & obj_polygon = info.extended_obj_polygon.outer();
     obj_marker.id = ++id;
     obj_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8);
     obj_marker.points.reserve(obj_polygon.size());

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
@@ -334,11 +334,11 @@ std::shared_ptr<LaneChangeDebugMsgArray> LaneChangeInterface::get_debug_msg_arra
   for (const auto & [uuid, debug_data] : debug_data) {
     LaneChangeDebugMsg debug_msg;
     debug_msg.object_id = uuid;
-    debug_msg.allow_lane_change = debug_data.allow_lane_change;
+    debug_msg.allow_lane_change = debug_data.is_safe;
     debug_msg.is_front = debug_data.is_front;
-    debug_msg.relative_distance = debug_data.relative_to_ego;
-    debug_msg.failed_reason = debug_data.failed_reason;
-    debug_msg.velocity = debug_data.object_twist.linear.x;
+    debug_msg.failed_reason = debug_data.unsafe_reason;
+    debug_msg.velocity =
+      std::hypot(debug_data.object_twist.linear.x, debug_data.object_twist.linear.y);
     debug_msg_array.lane_change_info.push_back(debug_msg);
   }
   lane_change_debug_msg_array_ = debug_msg_array;

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -1258,17 +1258,17 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     return std::make_pair(tier4_autoware_utils::toHexString(obj.uuid), debug);
   };
 
-  const auto updateDebugInfo =
-    [&debug_data](std::pair<std::string, CollisionCheckDebug> & obj, bool is_allowed) {
-      const auto & key = obj.first;
-      auto & element = obj.second;
-      element.allow_lane_change = is_allowed;
-      if (debug_data.find(key) != debug_data.end()) {
-        debug_data[key] = element;
-      } else {
-        debug_data.insert(obj);
-      }
-    };
+  const auto updateDebugInfo = [&debug_data](
+                                 std::pair<std::string, CollisionCheckDebug> & obj, bool is_safe) {
+    const auto & key = obj.first;
+    auto & element = obj.second;
+    element.is_safe = is_safe;
+    if (debug_data.find(key) != debug_data.end()) {
+      debug_data[key] = element;
+    } else {
+      debug_data.insert(obj);
+    }
+  };
 
   for (const auto & obj : collision_check_objects) {
     auto current_debug_data = assignDebugData(obj);

diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp
@@ -81,8 +81,8 @@ Polygon2d createExtendedPolygon(
   const double lat_offset = width / 2.0 + lat_margin;
 
   {
-    debug.longitudinal_offset = lon_offset;
-    debug.lateral_offset = lat_offset;
+    debug.extended_polygon_lon_offset = lon_offset;
+    debug.extended_polygon_lat_offset = lat_offset;
   }
 
   const auto p1 = tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, lat_offset, 0.0);
@@ -132,8 +132,8 @@ Polygon2d createExtendedPolygon(
   const double right_lat_offset = min_y - lat_margin;
 
   {
-    debug.longitudinal_offset = lon_offset;
-    debug.lateral_offset = (left_lat_offset + right_lat_offset) / 2;
+    debug.extended_polygon_lon_offset = lon_offset;
+    debug.extended_polygon_lat_offset = (left_lat_offset + right_lat_offset) / 2;
   }
 
   const auto p1 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, left_lat_offset, 0.0);
@@ -267,13 +267,13 @@ bool checkCollision(
       debug.lerped_path.push_back(ego_pose);
       debug.expected_ego_pose = ego_pose;
       debug.expected_obj_pose = obj_pose;
-      debug.ego_polygon = ego_polygon;
-      debug.obj_polygon = obj_polygon;
+      debug.extended_ego_polygon = ego_polygon;
+      debug.extended_obj_polygon = obj_polygon;
     }
 
     // check overlap
     if (boost::geometry::overlaps(ego_polygon, obj_polygon)) {
-      debug.failed_reason = "overlap_polygon";
+      debug.unsafe_reason = "overlap_polygon";
       return false;
     }
 
@@ -304,15 +304,15 @@ bool checkCollision(
 
     {
       debug.rss_longitudinal = rss_dist;
-      debug.ego_to_obj_margin = min_lon_length;
-      debug.ego_polygon = extended_ego_polygon;
-      debug.obj_polygon = extended_obj_polygon;
+      debug.inter_vehicle_distance = min_lon_length;
+      debug.extended_ego_polygon = extended_ego_polygon;
+      debug.extended_obj_polygon = extended_obj_polygon;
       debug.is_front = is_object_front;
     }
 
     // check overlap with extended polygon
     if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) {
-      debug.failed_reason = "overlap_extended_polygon";
+      debug.unsafe_reason = "overlap_extended_polygon";
       return false;
     }
   }