From 79d3affd639810c4c9e57c6da247af51051f0398 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Tue, 26 Dec 2023 12:03:35 +0900 Subject: [PATCH] refactor(bpp): remove maximum drivable area visualization Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/behavior_path_planner.param.yaml | 2 - .../behavior_path_planner_node.hpp | 1 - .../src/behavior_path_planner_node.cpp | 12 --- .../marker_utils/utils.hpp | 2 - .../parameters.hpp | 3 - .../static_drivable_area.hpp | 3 - .../src/marker_utils/utils.cpp | 84 ------------------- .../static_drivable_area.cpp | 28 ------- 8 files changed, 135 deletions(-) diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index b4d323de45cde..0c5dbc082c9b9 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/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 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index c47dc70213d1a..5d40f2a8614ed 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/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::SharedPtr debug_maximum_drivable_area_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_turn_signal_info_publisher_; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 93747fc6e9aa3..9fd397bb8a8b1 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/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("~/debug/avoidance_debug_message_array", 1); - if (planner_data_->parameters.visualize_maximum_drivable_area) { - debug_maximum_drivable_area_publisher_ = - create_publisher("~/maximum_drivable_area", 1); - } - debug_turn_signal_info_publisher_ = create_publisher("~/debug/turn_signal_info", 1); bound_publisher_ = create_publisher("~/debug/bound", 1); @@ -264,7 +259,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.enable_cog_on_centerline = declare_parameter("enable_cog_on_centerline"); p.input_path_interval = declare_parameter("input_path_interval"); p.output_path_interval = declare_parameter("output_path_interval"); - p.visualize_maximum_drivable_area = declare_parameter("visualize_maximum_drivable_area"); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("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(); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp index cfaa8de9b2fb9..1ce3dd3736276 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp @@ -81,8 +81,6 @@ MarkerArray createLaneletsAreaMarkerArray( const std::vector & 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); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index acda01dfc603c..811c89f61c066 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/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_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index 284b4aad20a6a..9053da45708a0 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/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 & planner_data); - /** * @brief Expand the borders of the given lanelets * @param [in] drivable_lanes lanelets to expand diff --git a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp index ec182ae5b909a..9e20b9c3f8714 100644 --- a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/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) diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 1d33190692d1a..ec147a7bb2771 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/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 & 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, ¤t_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 expandLanelets( const std::vector & drivable_lanes, const double left_bound_offset, const double right_bound_offset, const std::vector & types_to_skip)