diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml index 90a6432f0b83a..dd8031b0438c6 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -14,4 +14,3 @@ drivable_area_margin: 6.0 refine_goal_search_radius_range: 7.5 intersection_search_distance: 30.0 - path_interval: 2.0 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 f0889cc638c02..e809e47b05228 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -17,6 +17,4 @@ refine_goal_search_radius_range: 7.5 intersection_search_distance: 30.0 - path_interval: 2.0 - visualize_drivable_area_for_shared_linestrings_lanelet: true 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 073f9015d4616..e5c1b89454f4e 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 @@ -50,7 +50,6 @@ #include #include -#include #include namespace behavior_path_planner @@ -137,8 +136,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief extract path from behavior tree output */ - std::pair getPath( - const BehaviorModuleOutput & bt_out); + PathWithLaneId::SharedPtr getPath(const BehaviorModuleOutput & bt_out); /** * @brief extract path candidate from behavior tree output diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 31394714a9f08..9db58a23b70b3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -37,8 +37,6 @@ struct BehaviorPathPlannerParameters double turn_light_on_threshold_dis_long; double turn_light_on_threshold_time; - double path_interval; - // vehicle info double wheel_base; double front_overhang; 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 c241ed0324353..bd392853bcb23 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -165,7 +165,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.turn_light_on_threshold_dis_lat = declare_parameter("turn_light_on_threshold_dis_lat", 0.3); p.turn_light_on_threshold_dis_long = declare_parameter("turn_light_on_threshold_dis_long", 10.0); p.turn_light_on_threshold_time = declare_parameter("turn_light_on_threshold_time", 3.0); - p.path_interval = declare_parameter("path_interval"); p.visualize_drivable_area_for_shared_linestrings_lanelet = declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true); @@ -478,10 +477,7 @@ void BehaviorPathPlannerNode::run() const auto output = bt_manager_->run(planner_data_); // path handling - const auto paths = getPath(output); - const auto path = paths.first; - const auto original_path = paths.second; - + const auto path = getPath(output); const auto path_candidate = getPathCandidate(output); planner_data_->prev_output_path = path; @@ -507,7 +503,7 @@ void BehaviorPathPlannerNode::run() hazard_signal.command = output.turn_signal_info.hazard_signal.command; } else { turn_signal = turn_signal_decider_.getTurnSignal( - *original_path, planner_data_->self_pose->pose, *(planner_data_->route_handler), + *path, planner_data_->self_pose->pose, *(planner_data_->route_handler), output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance); hazard_signal.command = HazardLightsCommand::DISABLE; } @@ -530,9 +526,7 @@ void BehaviorPathPlannerNode::run() RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n"); } -// output: spline interpolated path, original path -std::pair BehaviorPathPlannerNode::getPath( - const BehaviorModuleOutput & bt_output) +PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleOutput & bt_output) { // TODO(Horibe) do some error handling when path is not available. @@ -541,10 +535,7 @@ std::pair BehaviorPathPlan path->header.stamp = this->now(); RCLCPP_DEBUG( get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND"); - - const auto resampled_path = - util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval); - return std::make_pair(std::make_shared(resampled_path), path); + return path; } PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 34e5111c1a2cb..9a02ced1c41c4 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -90,18 +90,7 @@ double calcPathArcLength(const PathWithLaneId & path, size_t start, size_t end) PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval) { const auto base_points = calcPathArcLengthArray(path); - auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval); - - constexpr double epsilon = 0.01; - for (const auto & b : base_points) { - const auto is_almost_same_value = std::find_if( - sampling_points.begin(), sampling_points.end(), - [&](const auto v) { return std::abs(v - b) < epsilon; }); - if (is_almost_same_value == sampling_points.end()) { - sampling_points.push_back(b); - } - } - std::sort(sampling_points.begin(), sampling_points.end()); + const auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval); if (base_points.empty() || sampling_points.empty()) { return path; @@ -133,25 +122,22 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv // For LaneIds, Type, Twist // - // ------|----|----|----|----|----|----|----|--------> resampled - // [0] [1] [2] [3] [4] [5] [6] [7] + // ------|----|----|----|----|----|----|-------> resampled + // [0] [1] [2] [3] [4] [5] [6] // - // ------|---------------|----------|-------|---> base - // [0] [1] [2] [3] + // --------|---------------|----------|---------> base + // [0] [1] [2] // - // resampled[0] = base[0] - // resampled[1~3] = base[1] - // resampled[4~5] = base[2] - // resampled[6~7] = base[3] + // resampled[0~3] = base[0] + // resampled[4~5] = base[1] + // resampled[6] = base[2] // size_t base_idx{0}; for (size_t i = 0; i < resampled_path.points.size(); ++i) { - while (base_idx < base_points.size() - 1 && - ((sampling_points.at(i) > base_points.at(base_idx)) || - (sampling_points.at(i) - base_points.at(base_idx)) > 1e-3)) { + while (base_idx < base_points.size() - 1 && sampling_points.at(i) > base_points.at(base_idx)) { ++base_idx; } - size_t ref_idx = std::max(static_cast(base_idx), 0); + size_t ref_idx = std::max(static_cast(base_idx) - 1, 0); if (i == resampled_path.points.size() - 1) { ref_idx = base_points.size() - 1; // for last point } diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index c8600e25716b9..ad30db2917ecd 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -420,10 +420,10 @@ void BehaviorVelocityPlannerNode::onTrigger( const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - // const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); + const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); // check stop point - auto output_path_msg = filterStopPathPoint(filtered_path); + auto output_path_msg = filterStopPathPoint(interpolated_path_msg); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now();