feat(behavior_path_planner): lane change revised visualization (#1140)

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

change force lane change path color

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

visualize ego polygon with respect to object using same color

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Add valid path's visualization

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

fix conflicts

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

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

@@ -16,4 +16,4 @@
       enable_collision_check_at_prepare_phase: true
       use_predicted_path_outside_lanelet: true
       use_all_predicted_path: true
-      publish_debug_marker: true
+      publish_debug_marker: false

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -148,6 +148,10 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   // debug
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_drivable_area_lanelets_publisher_;
   rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
+  /**
+   * @brief check path if it is unsafe or forced
+   */
+  bool isForcedCandidatePath() const;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp

@@ -15,10 +15,12 @@
 #define BEHAVIOR_PATH_PLANNER__DEBUG_UTILITIES_HPP_
 
 #include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
+#include "tier4_autoware_utils/tier4_autoware_utils.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>
@@ -31,16 +33,52 @@
 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{0};
+  Pose current_pose{};
+  Twist current_twist{};
+  Pose expected_ego_pose{};
+  Pose expected_obj_pose{};
+  Pose relative_to_ego{};
+  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{};
+};
+
+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,43 @@
+// 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 showAllValidLaneChangePath(
+  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 showPolygon(
+  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"
 
@@ -25,12 +26,14 @@
 #include <tf2/utils.h>
 
 #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
 {
@@ -49,6 +52,7 @@ struct LaneChangeParameters
   bool enable_collision_check_at_prepare_phase;
   bool use_predicted_path_outside_lanelet;
   bool use_all_predicted_path;
+  bool publish_debug_marker;
 };
 
 struct LaneChangeStatus
@@ -166,6 +170,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> debug_valid_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_bt_node_interface.hpp

@@ -29,6 +29,7 @@ struct SceneModuleStatus
   explicit SceneModuleStatus(const std::string & n) : module_name(n) {}
   std::string module_name;  // TODO(Horibe) should be const
   bool is_requested{false};
+  bool is_execution_ready{false};
   bool is_waiting_approval{false};
   BT::NodeStatus status{BT::NodeStatus::IDLE};
 };

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/pull_out/pull_out_path.hpp"
 
 #include <opencv2/opencv.hpp>
@@ -72,6 +73,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
 {
@@ -313,6 +315,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);
 
@@ -347,7 +351,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);
@@ -357,15 +361,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 behavior_path_planner::util
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -329,7 +329,8 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.enable_abort_lane_change = dp("enable_abort_lane_change", true);
   p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true);
   p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true);
-  p.use_all_predicted_path = dp("use_all_predicted_path", false);
+  p.use_all_predicted_path = dp("use_all_predicted_path", true);
+  p.publish_debug_marker = dp("publish_debug_marker", false);
 
   // validation of parameters
   if (p.lane_change_sampling_num < 1) {
@@ -552,18 +553,19 @@ void BehaviorPathPlannerNode::run()
 
   // path handling
   const auto path = getPath(output, planner_data);
-  const auto path_candidate = getPathCandidate(output, planner_data);
 
   // update planner data
   planner_data_->prev_output_path = path;
   mutex_pd_.unlock();
 
   PathWithLaneId clipped_path;
-  if (skipSmoothGoalConnection(bt_manager_->getModulesStatus())) {
+  const auto module_status_ptr_vec = bt_manager_->getModulesStatus();
+  if (skipSmoothGoalConnection(module_status_ptr_vec)) {
     clipped_path = *path;
   } else {
     clipped_path = modifyPathForSmoothGoalConnection(*path);
   }
+
   clipPathLength(clipped_path);
   if (!clipped_path.points.empty()) {
     path_publisher_->publish(clipped_path);
@@ -572,6 +574,7 @@ void BehaviorPathPlannerNode::run()
       get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish.");
   }
 
+  const auto path_candidate = getPathCandidate(output, planner_data);
   path_candidate_publisher_->publish(util::toPath(*path_candidate));
 
   // for turn signal
@@ -627,6 +630,13 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(
 {
   auto path_candidate =
     bt_output.path_candidate ? bt_output.path_candidate : std::make_shared<PathWithLaneId>();
+
+  if (isForcedCandidatePath()) {
+    for (auto & path_point : path_candidate->points) {
+      path_point.point.longitudinal_velocity_mps = 1.0;
+    }
+  }
+
   path_candidate->header = planner_data->route_handler->getRouteHeader();
   path_candidate->header.stamp = this->now();
   RCLCPP_DEBUG(
@@ -635,6 +645,26 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(
   return path_candidate;
 }
 
+bool BehaviorPathPlannerNode::isForcedCandidatePath() const
+{
+  const auto & module_status_ptr_vec = bt_manager_->getModulesStatus();
+  for (const auto & module_status_ptr : module_status_ptr_vec) {
+    if (!module_status_ptr) {
+      continue;
+    }
+    if (module_status_ptr->module_name != "LaneChange") {
+      continue;
+    }
+    const auto & is_waiting_approval = module_status_ptr->is_waiting_approval;
+    const auto & is_execution_ready = module_status_ptr->is_execution_ready;
+    if (is_waiting_approval && !is_execution_ready) {
+      return true;
+    }
+    break;
+  }
+  return false;
+}
+
 bool BehaviorPathPlannerNode::skipSmoothGoalConnection(
   const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses) const
 {