refactor(behavior_path_planner): remove maximum drivable area visuali…

…zation (#5963)

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

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -25,8 +25,6 @@
     input_path_interval: 2.0
     output_path_interval: 2.0
 
-    visualize_maximum_drivable_area: true
-
     lane_following:
       drivable_area_right_bound_offset: 0.0
       drivable_area_left_bound_offset: 0.0

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -166,7 +166,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
     const BehaviorModuleOutput & output);
 
   // debug
-  rclcpp::Publisher<MarkerArray>::SharedPtr debug_maximum_drivable_area_publisher_;
   rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_turn_signal_info_publisher_;
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -73,11 +73,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   debug_avoidance_msg_array_publisher_ =
     create_publisher<AvoidanceDebugMsgArray>("~/debug/avoidance_debug_message_array", 1);
 
-  if (planner_data_->parameters.visualize_maximum_drivable_area) {
-    debug_maximum_drivable_area_publisher_ =
-      create_publisher<MarkerArray>("~/maximum_drivable_area", 1);
-  }
-
   debug_turn_signal_info_publisher_ = create_publisher<MarkerArray>("~/debug/turn_signal_info", 1);
 
   bound_publisher_ = create_publisher<MarkerArray>("~/debug/bound", 1);
@@ -264,7 +259,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.enable_cog_on_centerline = declare_parameter<bool>("enable_cog_on_centerline");
   p.input_path_interval = declare_parameter<double>("input_path_interval");
   p.output_path_interval = declare_parameter<double>("output_path_interval");
-  p.visualize_maximum_drivable_area = declare_parameter<bool>("visualize_maximum_drivable_area");
   p.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
   p.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
 
@@ -447,12 +441,6 @@ void BehaviorPathPlannerNode::run()
 
   planner_data_->prev_route_id = planner_data_->route_handler->getRouteUuid();
 
-  if (planner_data_->parameters.visualize_maximum_drivable_area) {
-    const auto maximum_drivable_area = marker_utils::createFurthestLineStringMarkerArray(
-      utils::getMaximumDrivableArea(planner_data_));
-    debug_maximum_drivable_area_publisher_->publish(maximum_drivable_area);
-  }
-
   lk_pd.unlock();  // release planner_data_
 
   planner_manager_->print();

planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp

@@ -81,8 +81,6 @@ MarkerArray createLaneletsAreaMarkerArray(
   const std::vector<lanelet::ConstLanelet> & lanelets, std::string && ns, const float & r,
   const float & g, const float & b);
 
-MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings);
-
 MarkerArray createPolygonMarkerArray(
   const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r,
   const float & g, const float & b, const float & w = 0.3);

planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp

@@ -76,9 +76,6 @@ struct BehaviorPathPlannerParameters
   double right_over_hang;
   double base_link2front;
   double base_link2rear;
-
-  // maximum drivable area visualization
-  bool visualize_maximum_drivable_area;
 };
 
 #endif  // BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp

@@ -47,9 +47,6 @@ void generateDrivableArea(
   PathWithLaneId & path, const double vehicle_length, const double offset,
   const bool is_driving_forward = true);
 
-lanelet::ConstLineStrings3d getMaximumDrivableArea(
-  const std::shared_ptr<const PlannerData> & planner_data);
-
 /**
  * @brief Expand the borders of the given lanelets
  * @param [in] drivable_lanes lanelets to expand

planning/behavior_path_planner_common/src/marker_utils/utils.cpp

@@ -236,90 +236,6 @@ MarkerArray createLaneletsAreaMarkerArray(
   return msg;
 }
 
-MarkerArray createFurthestLineStringMarkerArray(const lanelet::ConstLineStrings3d & linestrings)
-{
-  if (linestrings.empty()) {
-    return MarkerArray();
-  }
-
-  Marker marker = createDefaultMarker(
-    "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "shared_linestring_lanelets", 0L, Marker::LINE_STRIP,
-    createMarkerScale(0.3, 0.0, 0.0), createMarkerColor(0.996, 0.658, 0.466, 0.999));
-  marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
-
-  const auto reserve_size = linestrings.size() / 2;
-  lanelet::ConstLineStrings3d lefts;
-  lanelet::ConstLineStrings3d rights;
-  lefts.reserve(reserve_size);
-  rights.reserve(reserve_size);
-
-  size_t total_marker_reserve_size{0};
-  for (size_t idx = 1; idx < linestrings.size(); idx += 2) {
-    rights.emplace_back(linestrings.at(idx - 1));
-    lefts.emplace_back(linestrings.at(idx));
-
-    for (const auto & ls : linestrings.at(idx - 1).basicLineString()) {
-      total_marker_reserve_size += ls.size();
-    }
-    for (const auto & ls : linestrings.at(idx).basicLineString()) {
-      total_marker_reserve_size += ls.size();
-    }
-  }
-
-  if (!total_marker_reserve_size) {
-    marker.points.reserve(total_marker_reserve_size);
-  }
-
-  const auto & first_ls = lefts.front().basicLineString();
-  for (const auto & ls : first_ls) {
-    marker.points.push_back(createPoint(ls.x(), ls.y(), ls.z()));
-  }
-
-  for (auto idx = lefts.cbegin() + 1; idx != lefts.cend(); ++idx) {
-    Point front = createPoint(
-      idx->basicLineString().front().x(), idx->basicLineString().front().y(),
-      idx->basicLineString().front().z());
-    Point front_inverted = createPoint(
-      idx->invert().basicLineString().front().x(), idx->invert().basicLineString().front().y(),
-      idx->invert().basicLineString().front().z());
-
-    const auto & marker_back = marker.points.back();
-    const bool isFrontNear = tier4_autoware_utils::calcDistance2d(marker_back, front) <
-                             tier4_autoware_utils::calcDistance2d(marker_back, front_inverted);
-    const auto & left_ls = (isFrontNear) ? idx->basicLineString() : idx->invert().basicLineString();
-    for (const auto & left_l : left_ls) {
-      marker.points.push_back(createPoint(left_l.x(), left_l.y(), left_l.z()));
-    }
-  }
-
-  for (auto idx = rights.crbegin(); idx != rights.crend(); ++idx) {
-    Point front = createPoint(
-      idx->basicLineString().front().x(), idx->basicLineString().front().y(),
-      idx->basicLineString().front().z());
-    Point front_inverted = createPoint(
-      idx->invert().basicLineString().front().x(), idx->invert().basicLineString().front().y(),
-      idx->invert().basicLineString().front().z());
-
-    const auto & marker_back = marker.points.back();
-    const bool isFrontFurther = tier4_autoware_utils::calcDistance2d(marker_back, front) >
-                                tier4_autoware_utils::calcDistance2d(marker_back, front_inverted);
-    const auto & right_ls =
-      (isFrontFurther) ? idx->basicLineString() : idx->invert().basicLineString();
-    for (auto ls = right_ls.crbegin(); ls != right_ls.crend(); ++ls) {
-      marker.points.push_back(createPoint(ls->x(), ls->y(), ls->z()));
-    }
-  }
-
-  if (!marker.points.empty()) {
-    marker.points.push_back(marker.points.front());
-  }
-
-  MarkerArray msg;
-
-  msg.markers.push_back(marker);
-  return msg;
-}
-
 MarkerArray createPolygonMarkerArray(
   const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r,
   const float & g, const float & b, const float & w)

planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

@@ -1017,34 +1017,6 @@ void generateDrivableArea(
   path.right_bound = modified_right_bound;
 }
 
-// TODO(Azu) Some parts of is the same with generateCenterLinePath. Therefore it might be better if
-// we can refactor some of the code for better readability
-lanelet::ConstLineStrings3d getMaximumDrivableArea(
-  const std::shared_ptr<const PlannerData> & planner_data)
-{
-  const auto & p = planner_data->parameters;
-  const auto & route_handler = planner_data->route_handler;
-  const auto & ego_pose = planner_data->self_odometry->pose.pose;
-
-  lanelet::ConstLanelet current_lane;
-  if (!route_handler->getClosestLaneletWithinRoute(ego_pose, &current_lane)) {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("behavior_path_planner").get_child("utils"),
-      "failed to find closest lanelet within route!!!");
-    return {};
-  }
-
-  const auto current_lanes = route_handler->getLaneletSequence(
-    current_lane, ego_pose, p.backward_path_length, p.forward_path_length);
-  lanelet::ConstLineStrings3d linestring_shared;
-  for (const auto & lane : current_lanes) {
-    lanelet::ConstLineStrings3d furthest_line = route_handler->getFurthestLinestring(lane);
-    linestring_shared.insert(linestring_shared.end(), furthest_line.begin(), furthest_line.end());
-  }
-
-  return linestring_shared;
-}
-
 std::vector<DrivableLanes> expandLanelets(
   const std::vector<DrivableLanes> & drivable_lanes, const double left_bound_offset,
   const double right_bound_offset, const std::vector<std::string> & types_to_skip)