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 7413beb3c2574..f3815ef1cc6e7 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 @@ -35,6 +35,7 @@ #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/util/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/util/lane_change/lane_change_module_data.hpp" +#include "behavior_path_planner/util/lane_following/module_data.hpp" #include "behavior_path_planner/util/pull_out/pull_out_parameters.hpp" #include "behavior_path_planner/util/pull_over/pull_over_parameters.hpp" #include "behavior_path_planner/util/side_shift/side_shift_parameters.hpp" @@ -153,16 +154,17 @@ class BehaviorPathPlannerNode : public rclcpp::Node // parameters std::shared_ptr avoidance_param_ptr_; std::shared_ptr lane_change_param_ptr_; + std::shared_ptr lane_following_param_ptr_; BehaviorPathPlannerParameters getCommonParam(); #ifdef USE_OLD_ARCHITECTURE BehaviorTreeManagerParam getBehaviorTreeManagerParam(); - LaneFollowingParameters getLaneFollowingParam(); #endif AvoidanceParameters getAvoidanceParam(); LaneChangeParameters getLaneChangeParam(); + LaneFollowingParameters getLaneFollowingParam(); SideShiftParameters getSideShiftParam(); PullOverParameters getPullOverParam(); PullOutParameters getPullOutParam(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 1baa547e27f07..870c83f7208a7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" +#include "behavior_path_planner/util/lane_following/module_data.hpp" #include @@ -54,7 +55,9 @@ struct SceneModuleStatus class PlannerManager { public: - PlannerManager(rclcpp::Node & node, const bool verbose); + PlannerManager( + rclcpp::Node & node, const std::shared_ptr & parameters, + const bool verbose); BehaviorModuleOutput run(const std::shared_ptr & data); @@ -187,6 +190,8 @@ class PlannerManager boost::optional root_lanelet_{boost::none}; + std::shared_ptr parameters_; + std::vector manager_ptrs_; std::vector approved_module_ptrs_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp index 36d42322d1aa6..5889381dbb54e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_following/lane_following_module.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_FOLLOWING__LANE_FOLLOWING_MODULE_HPP_ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" +#include "behavior_path_planner/util/lane_following/module_data.hpp" #include @@ -29,20 +30,12 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; -struct LaneFollowingParameters -{ - double lane_change_prepare_duration; - // drivable area expansion - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip{}; -}; - class LaneFollowingModule : public SceneModuleInterface { public: LaneFollowingModule( - const std::string & name, rclcpp::Node & node, const LaneFollowingParameters & parameters); + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters); bool isExecutionRequested() const override; bool isExecutionReady() const override; @@ -52,14 +45,14 @@ class LaneFollowingModule : public SceneModuleInterface void onEntry() override; void onExit() override; - void setParameters(const LaneFollowingParameters & parameters); + void setParameters(const std::shared_ptr & parameters); void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override { } private: - LaneFollowingParameters parameters_; + std::shared_ptr parameters_; PathWithLaneId getReferencePath() const; void initParam(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_following/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_following/module_data.hpp new file mode 100644 index 0000000000000..096660090c60d --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_following/module_data.hpp @@ -0,0 +1,35 @@ +// Copyright 2023 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__UTIL__LANE_FOLLOWING__MODULE_DATA_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTIL__LANE_FOLLOWING__MODULE_DATA_HPP_ + +#include +#include + +namespace behavior_path_planner +{ + +struct LaneFollowingParameters +{ + double lane_change_prepare_duration; + // drivable area expansion + double drivable_area_right_bound_offset; + double drivable_area_left_bound_offset; + std::vector drivable_area_types_to_skip{}; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTIL__LANE_FOLLOWING__MODULE_DATA_HPP_ 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 c876d6b40f2b6..e158c71e71bcd 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -118,6 +118,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod { avoidance_param_ptr_ = std::make_shared(getAvoidanceParam()); lane_change_param_ptr_ = std::make_shared(getLaneChangeParam()); + lane_following_param_ptr_ = std::make_shared(getLaneFollowingParam()); } m_set_param_res = this->add_on_set_parameters_callback( @@ -144,7 +145,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod bt_manager_->registerSceneModule(avoidance_module); auto lane_following_module = - std::make_shared("LaneFollowing", *this, getLaneFollowingParam()); + std::make_shared("LaneFollowing", *this, lane_following_param_ptr_); bt_manager_->registerSceneModule(lane_following_module); auto ext_request_lane_change_right_module = @@ -193,7 +194,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod mutex_bt_.lock(); const auto & p = planner_data_->parameters; - planner_manager_ = std::make_shared(*this, p.verbose); + planner_manager_ = + std::make_shared(*this, lane_following_param_ptr_, p.verbose); mutex_bt_.unlock(); } @@ -487,7 +489,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() return p; } -#ifdef USE_OLD_ARCHITECTURE LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam() { LaneFollowingParameters p{}; @@ -501,7 +502,6 @@ LaneFollowingParameters BehaviorPathPlannerNode::getLaneFollowingParam() declare_parameter("lane_following.lane_change_prepare_duration", 2.0); return p; } -#endif LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() { diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 32b246c618f18..a90e1cef71cf8 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -27,8 +27,11 @@ namespace behavior_path_planner { -PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose) -: logger_(node.get_logger().get_child("planner_manager")), +PlannerManager::PlannerManager( + rclcpp::Node & node, const std::shared_ptr & parameters, + const bool verbose) +: parameters_{parameters}, + logger_(node.get_logger().get_child("planner_manager")), clock_(*node.get_clock()), verbose_{verbose} { @@ -355,12 +358,15 @@ BehaviorModuleOutput PlannerManager::getReferencePath( const double lane_change_buffer = util::calcLaneChangeBuffer(p, num_lane_change); reference_path = util::setDecelerationVelocity( - *route_handler, reference_path, current_lanes, 2.0, lane_change_buffer); + *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration, + lane_change_buffer); } const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); - const auto expanded_lanes = util::expandLanelets(shorten_lanes, 0.0, 0.0); + const auto expanded_lanes = util::expandLanelets( + shorten_lanes, parameters_->drivable_area_left_bound_offset, + parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); util::generateDrivableArea(reference_path, expanded_lanes, p.vehicle_length, data); 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 a63e507693480..4cdca4dd09e53 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 @@ -25,7 +25,8 @@ namespace behavior_path_planner { LaneFollowingModule::LaneFollowingModule( - const std::string & name, rclcpp::Node & node, const LaneFollowingParameters & parameters) + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters) : SceneModuleInterface{name, node}, parameters_{parameters} { initParam(); @@ -69,7 +70,7 @@ void LaneFollowingModule::onExit() RCLCPP_DEBUG(getLogger(), "LANE_FOLLOWING onExit"); } -void LaneFollowingModule::setParameters(const LaneFollowingParameters & parameters) +void LaneFollowingModule::setParameters(const std::shared_ptr & parameters) { parameters_ = parameters; } @@ -146,15 +147,15 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const const double lane_change_buffer = util::calcLaneChangeBuffer(p, num_lane_change); reference_path = util::setDecelerationVelocity( - *route_handler, reference_path, current_lanes, parameters_.lane_change_prepare_duration, + *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration, lane_change_buffer); } const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); const auto expanded_lanes = util::expandLanelets( - shorten_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset, parameters_.drivable_area_types_to_skip); + shorten_lanes, parameters_->drivable_area_left_bound_offset, + parameters_->drivable_area_right_bound_offset, parameters_->drivable_area_types_to_skip); util::generateDrivableArea(reference_path, expanded_lanes, p.vehicle_length, planner_data_);