From 51ffc39d89e3638295fa4f73e310ac50ba0e2e7d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 20 Feb 2023 17:37:18 +0900 Subject: [PATCH] fix(behavior_path_planner): move findEgoIndex & findEgoSegmentIndex to planner data (#2912) Signed-off-by: satoshi-ota --- .../behavior_path_planner_node.hpp | 18 ------------------ .../behavior_path_planner/data_manager.hpp | 17 +++++++++++++++++ .../avoidance/avoidance_module.hpp | 2 +- .../scene_module/scene_module_interface.hpp | 18 ------------------ .../src/behavior_path_planner_node.cpp | 2 +- .../avoidance/avoidance_module.cpp | 6 +++--- .../lane_following/lane_following_module.cpp | 2 +- .../pull_over/pull_over_module.cpp | 6 +++--- .../side_shift/side_shift_module.cpp | 4 ++-- 9 files changed, 28 insertions(+), 47 deletions(-) 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 031b2aef1638e..45eab080400dc 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 @@ -213,24 +213,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node */ Path convertToPath( const std::shared_ptr & path_candidate_ptr, const bool is_ready); - - template - size_t findEgoIndex(const std::vector & points) const - { - const auto & p = planner_data_; - return motion_utils::findFirstNearestIndexWithSoftConstraints( - points, p->self_odometry->pose.pose, p->parameters.ego_nearest_dist_threshold, - p->parameters.ego_nearest_yaw_threshold); - } - - template - size_t findEgoSegmentIndex(const std::vector & points) const - { - const auto & p = planner_data_; - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->self_odometry->pose.pose, p->parameters.ego_nearest_dist_threshold, - p->parameters.ego_nearest_yaw_threshold); - } }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 925883fe1ba16..f0e23741fa749 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -32,6 +32,7 @@ #include #include +#include namespace behavior_path_planner { @@ -74,6 +75,22 @@ struct PlannerData std::shared_ptr route_handler{std::make_shared()}; BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; + + template + size_t findEgoIndex(const std::vector & points) const + { + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, self_odometry->pose.pose, parameters.ego_nearest_dist_threshold, + parameters.ego_nearest_yaw_threshold); + } + + template + size_t findEgoSegmentIndex(const std::vector & points) const + { + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, self_odometry->pose.pose, parameters.ego_nearest_dist_threshold, + parameters.ego_nearest_yaw_threshold); + } }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 1b1163b1c1b43..a02ea4963abc0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -339,7 +339,7 @@ class AvoidanceModule : public SceneModuleInterface void postProcess(PathShifter & path_shifter) const { - const size_t nearest_idx = findEgoIndex(path_shifter.getReferencePath().points); + const size_t nearest_idx = planner_data_->findEgoIndex(path_shifter.getReferencePath().points); path_shifter.removeBehindShiftLineAndSetBaseOffset(nearest_idx); } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index ab4aaa7ac72b7..3a7a379227400 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -306,24 +306,6 @@ class SceneModuleInterface return uuid; } - template - size_t findEgoIndex(const std::vector & points) const - { - const auto & p = planner_data_; - return motion_utils::findFirstNearestIndexWithSoftConstraints( - points, p->self_odometry->pose.pose, p->parameters.ego_nearest_dist_threshold, - p->parameters.ego_nearest_yaw_threshold); - } - - template - size_t findEgoSegmentIndex(const std::vector & points) const - { - const auto & p = planner_data_; - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->self_odometry->pose.pose, p->parameters.ego_nearest_dist_threshold, - p->parameters.ego_nearest_yaw_threshold); - } - public: BT::NodeStatus current_state_; }; 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 53ce524acd311..11f89c06e0630 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -798,7 +798,7 @@ void BehaviorPathPlannerNode::run() // publish drivable bounds publish_bounds(*path); - const size_t target_idx = findEgoIndex(path->points); + const size_t target_idx = planner_data->findEgoIndex(path->points); util::clipPathLength(*path, target_idx, planner_data_->parameters); if (!path->points.empty()) { diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 7478aa7a90b34..b55a90748a2d8 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2182,7 +2182,7 @@ bool AvoidanceModule::isSafePath( auto path_with_current_velocity = shifted_path.path; path_with_current_velocity = util::resamplePathWithSpline(path_with_current_velocity, 0.5); - const size_t ego_idx = findEgoIndex(path_with_current_velocity.points); + const size_t ego_idx = planner_data_->findEgoIndex(path_with_current_velocity.points); util::clipPathLength(path_with_current_velocity, ego_idx, forward_check_distance, 0.0); constexpr double MIN_EGO_VEL_IN_PREDICTION = 1.38; // 5km/h @@ -2964,7 +2964,7 @@ BehaviorModuleOutput AvoidanceModule::plan() output.path = std::make_shared(avoidance_path.path); - const size_t ego_idx = findEgoIndex(output.path->points); + const size_t ego_idx = planner_data_->findEgoIndex(output.path->points); util::clipPathLength(*output.path, ego_idx, planner_data_->parameters); // Drivable area generation. @@ -3009,7 +3009,7 @@ CandidateOutput AvoidanceModule::planCandidate() const ""); } - const size_t ego_idx = findEgoIndex(shifted_path.path.points); + const size_t ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); util::clipPathLength(shifted_path.path, ego_idx, planner_data_->parameters); output.path_candidate = shifted_path.path; diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index 4052c56ef0c00..a63e507693480 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -133,7 +133,7 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const p.forward_path_length, p); // clip backward length - const size_t current_seg_idx = findEgoSegmentIndex(reference_path.points); + const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(reference_path.points); util::clipPathLength( reference_path, current_seg_idx, p.forward_path_length, p.backward_path_length); const auto drivable_lanelets = getLaneletsFromPath(reference_path, route_handler); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 4f78de15c88d7..8842944c5d34f 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -485,7 +485,7 @@ BehaviorModuleOutput PullOverModule::plan() // generate drivable area for each partial path for (auto & path : status_.pull_over_path.partial_paths) { - const size_t ego_idx = findEgoIndex(path.points); + const size_t ego_idx = planner_data_->findEgoIndex(path.points); util::clipPathLength(path, ego_idx, planner_data_->parameters); const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); const auto expanded_lanes = util::expandLanelets( @@ -661,7 +661,7 @@ PathWithLaneId PullOverModule::generateStopPath() : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_pose); // if stop pose is closer than min_stop_distance, stop as soon as possible - const size_t ego_idx = findEgoIndex(reference_path.points); + const size_t ego_idx = planner_data_->findEgoIndex(reference_path.points); const size_t stop_idx = findFirstNearestSegmentIndexWithSoftConstraints( reference_path.points, stop_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); @@ -722,7 +722,7 @@ PathWithLaneId PullOverModule::generateEmergencyStopPath() } // set deceleration velocity - const size_t ego_idx = findEgoIndex(stop_path.points); + const size_t ego_idx = planner_data_->findEgoIndex(stop_path.points); for (auto & point : stop_path.points) { auto & p = point.point; const size_t target_idx = findFirstNearestSegmentIndexWithSoftConstraints( diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index a7897618e14e8..f84bdd6ba2b83 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -194,7 +194,7 @@ void SideShiftModule::updateData() current_lanelets_ = route_handler->getLaneletSequence( current_lane, reference_pose, p.backward_path_length, p.forward_path_length); - const size_t nearest_idx = findEgoIndex(path_shifter_.getReferencePath().points); + const size_t nearest_idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); path_shifter_.removeBehindShiftLineAndSetBaseOffset(nearest_idx); } @@ -337,7 +337,7 @@ ShiftLine SideShiftModule::calcShiftLine() const return dist_to_end; }(); - const size_t nearest_idx = findEgoIndex(reference_path_->points); + const size_t nearest_idx = planner_data_->findEgoIndex(reference_path_->points); ShiftLine shift_line; shift_line.end_shift_length = requested_lateral_offset_; shift_line.start_idx = util::getIdxByArclength(*reference_path_, nearest_idx, dist_to_start);