feat(behavior_path_planner): lane change revised visualization

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

planning/behavior_path_planner/CMakeLists.txt

@@ -22,6 +22,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/scene_module/lane_following/lane_following_module.cpp
   src/scene_module/lane_change/lane_change_module.cpp
   src/scene_module/lane_change/util.cpp
+  src/scene_module/lane_change/debug.cpp
   src/scene_module/pull_over/pull_over_module.cpp
   src/scene_module/pull_over/util.cpp
   src/scene_module/pull_out/pull_out_module.cpp

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -13,3 +13,4 @@
       use_predicted_path_outside_lanelet: false
       use_all_predicted_path: false
       lane_change_search_distance: 30.0
+      publish_debug_marker: true

planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp

@@ -16,11 +16,12 @@
 
 #include "behavior_path_planner/path_utilities.hpp"
 #include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
-#include "behavior_path_planner/utilities.hpp"
+#include "tier4_autoware_utils/geometry/geometry.hpp"
 
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_path.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/polygon.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
@@ -35,16 +36,51 @@
 namespace marker_utils
 {
 using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using behavior_path_planner::ShiftPointArray;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Polygon;
 using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::Twist;
 using geometry_msgs::msg::Vector3;
 using std_msgs::msg::ColorRGBA;
+using tier4_autoware_utils::Polygon2d;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
+struct CollisionCheckDebug
+{
+  std::string failed_reason;
+  std::size_t lane_id;
+  Pose current_pose;
+  Twist current_twist;
+  Pose expected_ego_pose;
+  Pose expected_obj_pose;
+  Pose relative_to_ego;
+  bool is_front;
+  bool allow_lane_change;
+  std::vector<Pose> lerped_path;
+  std::vector<PredictedPath> ego_predicted_path;
+  Polygon2d ego_polygon;
+};
+
+constexpr std::array<std::array<float, 3>, 10> colorsList()
+{
+  constexpr std::array<float, 3> red = {1., 0., 0.};
+  constexpr std::array<float, 3> green = {0., 1., 0.};
+  constexpr std::array<float, 3> blue = {0., 0., 1.};
+  constexpr std::array<float, 3> yellow = {1., 1., 0.};
+  constexpr std::array<float, 3> aqua = {0., 1., 1.};
+  constexpr std::array<float, 3> magenta = {1., 0., 1.};
+  constexpr std::array<float, 3> medium_orchid = {0.729, 0.333, 0.827};
+  constexpr std::array<float, 3> light_pink = {1, 0.713, 0.756};
+  constexpr std::array<float, 3> light_yellow = {1, 1, 0.878};
+  constexpr std::array<float, 3> light_steel_blue = {0.690, 0.768, 0.870};
+  return {red,     green,         blue,       yellow,       aqua,
+          magenta, medium_orchid, light_pink, light_yellow, light_steel_blue};
+}
+
 inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); }
 
 MarkerArray createPoseMarkerArray(

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

@@ -0,0 +1,42 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_
+
+#include "behavior_path_planner/debug_utilities.hpp"
+#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace marker_utils::lane_change_markers
+{
+using behavior_path_planner::LaneChangePath;
+using marker_utils::CollisionCheckDebug;
+using visualization_msgs::msg::MarkerArray;
+MarkerArray showObjectInfo(
+  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
+MarkerArray showAllLaneChangeLanes(const std::vector<LaneChangePath> & lanes, std::string && ns);
+MarkerArray showLerpedPose(
+  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
+MarkerArray showEgoPredictedPaths(
+  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
+MarkerArray showEgoPolygon(
+  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
+MarkerArray showPolygonPose(
+  const std::unordered_map<std::string, CollisionCheckDebug> & obj_debug_vec, std::string && ns);
+}  // namespace marker_utils::lane_change_markers
+#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__DEBUG_HPP_

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

@@ -15,6 +15,7 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_
 
+#include "behavior_path_planner/scene_module/lane_change/debug.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/utilities.hpp"
@@ -31,12 +32,14 @@
 
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
 namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using marker_utils::CollisionCheckDebug;
 
 struct LaneChangeParameters
 {
@@ -56,6 +59,7 @@ struct LaneChangeParameters
   bool use_predicted_path_outside_lanelet;
   bool use_all_predicted_path;
   double lane_change_search_distance;
+  double publish_debug_marker;
 };
 
 struct LaneChangeStatus
@@ -173,6 +177,10 @@ class LaneChangeModule : public SceneModuleInterface
   bool isCurrentSpeedLow() const;
   bool isAbortConditionSatisfied() const;
   bool hasFinishedLaneChange() const;
+
+  void setObjectDebugVisualization() const;
+  mutable std::unordered_map<std::string, CollisionCheckDebug> object_debug_;
+  mutable std::vector<LaneChangePath> candidate_path_;
 };
 }  // namespace behavior_path_planner
 

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

@@ -29,6 +29,8 @@
 #include <lanelet2_core/primitives/Primitive.h>
 
 #include <memory>
+#include <string>
+#include <unordered_map>
 #include <vector>
 
 namespace behavior_path_planner::lane_change_utils
@@ -65,13 +67,16 @@ bool selectSafePath(
   const lanelet::ConstLanelets & target_lanes,
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
   const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters,
-  const LaneChangeParameters & ros_parameters, LaneChangePath * selected_path);
+  const behavior_path_planner::LaneChangeParameters & ros_parameters,
+  LaneChangePath * selected_path,
+  std::unordered_map<std::string, CollisionCheckDebug> & debug_data);
 bool isLaneChangePathSafe(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
   const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters,
-  const LaneChangeParameters & lane_change_parameters, const bool use_buffer = true,
+  const behavior_path_planner::LaneChangeParameters & lane_change_parameters,
+  std::unordered_map<std::string, CollisionCheckDebug> & debug_data, const bool use_buffer = true,
   const double acceleration = 0.0);
 bool hasEnoughDistance(
   const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -241,7 +241,7 @@ class SceneModuleInterface
   rclcpp::Logger logger_;
 
 protected:
-  MarkerArray debug_marker_;
+  mutable MarkerArray debug_marker_;
   rclcpp::Clock::SharedPtr clock_;
   mutable AvoidanceDebugMsgArray::SharedPtr debug_avoidance_msg_array_ptr_{};
 

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -16,6 +16,7 @@
 #define BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_
 
 #include "behavior_path_planner/data_manager.hpp"
+#include "behavior_path_planner/debug_utilities.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
 #include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp"
 
@@ -84,6 +85,7 @@ using tier4_autoware_utils::Point2d;
 using tier4_autoware_utils::Polygon2d;
 namespace bg = boost::geometry;
 using geometry_msgs::msg::Pose;
+using marker_utils::CollisionCheckDebug;
 
 struct FrenetCoordinate3d
 {
@@ -320,6 +322,8 @@ Polygon2d convertCylindricalObjectToGeometryPolygon(
 
 Polygon2d convertPolygonObjectToGeometryPolygon(const Pose & current_pose, const Shape & obj_shape);
 
+std::string getUuidStr(const PredictedObject & obj);
+
 std::vector<PredictedPath> getPredictedPathFromObj(
   const PredictedObject & obj, const bool & is_use_all_predicted_path);
 
@@ -354,7 +358,7 @@ bool isLongitudinalDistanceEnough(
 bool hasEnoughDistance(
   const Pose & expected_ego_pose, const Twist & ego_current_twist,
   const Pose & expected_object_pose, const Twist & object_current_twist,
-  const BehaviorPathPlannerParameters & param);
+  const BehaviorPathPlannerParameters & param, CollisionCheckDebug & debug);
 
 bool isLateralDistanceEnough(
   const double & relative_lateral_distance, const double & lateral_distance_threshold);
@@ -364,15 +368,15 @@ bool isSafeInLaneletCollisionCheck(
   const PredictedPath & ego_predicted_path, const double & ego_vehicle_length,
   const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time,
   const double & check_time_resolution, const PredictedObject & target_object,
-  const PredictedPath & target_object_path,
-  const BehaviorPathPlannerParameters & common_parameters);
+  const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters,
+  CollisionCheckDebug & debug);
 
 bool isSafeInFreeSpaceCollisionCheck(
   const Pose & ego_current_pose, const Twist & ego_current_twist,
   const PredictedPath & ego_predicted_path, const double & ego_vehicle_length,
   const double & ego_vehicle_width, const double & check_start_time, const double & check_end_time,
   const double & check_time_resolution, const PredictedObject & target_object,
-  const BehaviorPathPlannerParameters & common_parameters);
+  const BehaviorPathPlannerParameters & common_parameters, CollisionCheckDebug & debug);
 }  // namespace util
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -330,6 +330,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
     dp("abort_lane_change_angle_thresh", tier4_autoware_utils::deg2rad(10.0));
   p.abort_lane_change_distance_thresh = dp("abort_lane_change_distance_thresh", 0.3);
   p.lane_change_search_distance = dp("lane_change_search_distance", 30.0);
+  p.publish_debug_marker = dp("publish_debug_marker", false);
 
   // validation of parameters
   if (p.lane_change_sampling_num < 1) {

planning/behavior_path_planner/src/debug_utilities.cpp

@@ -14,6 +14,8 @@
 
 #include "behavior_path_planner/debug_utilities.hpp"
 
+#include "behavior_path_planner/utilities.hpp"
+
 namespace marker_utils
 {
 using behavior_path_planner::ShiftPoint;