diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 01ccc632b0133..ea86022e7134d 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -154,6 +154,8 @@ perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhon perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp +planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner_common/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index e2ffff2195645..ba18b7dadd599 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -155,7 +155,6 @@ class EKFLocalizer : public rclcpp::Node const HyperParameters params_; - double ekf_rate_; double ekf_dt_; /* process noise variance for discrete model */ diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index 873060c75fcfc..e88a59ffdfab9 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -30,6 +30,7 @@ #include #include +#include struct EKFDiagnosticInfo { @@ -76,12 +77,15 @@ class EKFModule std::array getCurrentPoseCovariance() const; std::array getCurrentTwistCovariance() const; + size_t find_closest_delay_time_index(double target_value) const; + void accumulate_delay_time(const double dt); + void predictWithDelay(const double dt); bool measurementUpdatePose( - const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr, + const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info); bool measurementUpdateTwist( - const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr, + const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info); geometry_msgs::msg::PoseWithCovarianceStamped compensatePoseWithZDelay( const PoseWithCovariance & pose, const double delay_time); @@ -91,6 +95,7 @@ class EKFModule std::shared_ptr warning_; const int dim_x_; + std::vector accumulated_delay_times_; const HyperParameters params_; }; diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index f6c664eb26451..e1eafdc6f7948 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -17,10 +17,9 @@ #include -std::string poseDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt); +std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold); std::string twistDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt); + const double delay_time, const double delay_time_threshold); std::string poseDelayTimeWarningMessage(const double delay_time); std::string twistDelayTimeWarningMessage(const double delay_time); std::string mahalanobisWarningMessage(const double distance, const double max_distance); diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 5dd78a00f2fbd..a3d2f52a4058c 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -44,7 +44,6 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti : rclcpp::Node(node_name, node_options), warning_(std::make_shared(this)), params_(this), - ekf_rate_(params_.ekf_rate), ekf_dt_(params_.ekf_dt), pose_queue_(params_.pose_smoothing_steps), twist_queue_(params_.twist_smoothing_steps) @@ -109,9 +108,22 @@ void EKFLocalizer::updatePredictFrequency() if (get_clock()->now() < *last_predict_time_) { warning_->warn("Detected jump back in time"); } else { - ekf_rate_ = 1.0 / (get_clock()->now() - *last_predict_time_).seconds(); - DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_); - ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); + /* Measure dt */ + ekf_dt_ = (get_clock()->now() - *last_predict_time_).seconds(); + DEBUG_INFO( + get_logger(), "[EKF] update ekf_dt_ to %f seconds (= %f hz)", ekf_dt_, 1 / ekf_dt_); + + if (ekf_dt_ > 10.0) { + ekf_dt_ = 10.0; + RCLCPP_WARN( + get_logger(), "Large ekf_dt_ detected!! (%f sec) Capped to 10.0 seconds", ekf_dt_); + } else if (ekf_dt_ > params_.pose_smoothing_steps / params_.ekf_rate) { + RCLCPP_WARN( + get_logger(), "EKF period may be too slow to finish pose smoothing!! (%f sec) ", ekf_dt_); + } + + /* Register dt and accumulate time delay */ + ekf_module_->accumulate_delay_time(ekf_dt_); /* Update discrete proc_cov*/ proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0); @@ -165,7 +177,7 @@ void EKFLocalizer::timerCallback() const size_t n = pose_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto pose = pose_queue_.pop_increment_age(); - bool is_updated = ekf_module_->measurementUpdatePose(*pose, ekf_dt_, t_curr, pose_diag_info_); + bool is_updated = ekf_module_->measurementUpdatePose(*pose, t_curr, pose_diag_info_); if (is_updated) { pose_is_updated = true; @@ -200,8 +212,7 @@ void EKFLocalizer::timerCallback() const size_t n = twist_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto twist = twist_queue_.pop_increment_age(); - bool is_updated = - ekf_module_->measurementUpdateTwist(*twist, ekf_dt_, t_curr, twist_diag_info_); + bool is_updated = ekf_module_->measurementUpdateTwist(*twist, t_curr, twist_diag_info_); if (is_updated) { twist_is_updated = true; } diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 0a5e3c98c96c8..10926b04507a0 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -38,6 +38,7 @@ EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters params) : warning_(std::move(warning)), dim_x_(6), // x, y, yaw, yaw_bias, vx, wz + accumulated_delay_times_(params.extend_state_step, 1.0E15), params_(params) { Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); @@ -131,6 +132,48 @@ double EKFModule::getYawBias() const return kalman_filter_.getLatestX()(IDX::YAWB); } +size_t EKFModule::find_closest_delay_time_index(double target_value) const +{ + // If target_value is too large, return last index + 1 + if (target_value > accumulated_delay_times_.back()) { + return accumulated_delay_times_.size(); + } + + auto lower = std::lower_bound( + accumulated_delay_times_.begin(), accumulated_delay_times_.end(), target_value); + + // If the lower bound is the first element, return its index. + // If the lower bound is beyond the last element, return the last index. + // If else, take the closest element. + if (lower == accumulated_delay_times_.begin()) { + return 0; + } else if (lower == accumulated_delay_times_.end()) { + return accumulated_delay_times_.size() - 1; + } else { + // Compare the target with the lower bound and the previous element. + auto prev = lower - 1; + bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value); + + // Return the index of the closer element. + return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev) + : std::distance(accumulated_delay_times_.begin(), lower); + } +} + +void EKFModule::accumulate_delay_time(const double dt) +{ + // Shift the delay times to the right. + std::copy_backward( + accumulated_delay_times_.begin(), accumulated_delay_times_.end() - 1, + accumulated_delay_times_.end()); + + // Add the new delay time to all elements. + accumulated_delay_times_.front() = 0.0; + for (auto & delay_time : accumulated_delay_times_) { + delay_time += dt; + } +} + void EKFModule::predictWithDelay(const double dt) { const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); @@ -147,8 +190,7 @@ void EKFModule::predictWithDelay(const double dt) } bool EKFModule::measurementUpdatePose( - const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr, - EKFDiagnosticInfo & pose_diag_info) + const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info) { if (pose.header.frame_id != params_.pose_frame_id) { warning_->warnThrottle( @@ -170,14 +212,15 @@ bool EKFModule::measurementUpdatePose( delay_time = std::max(delay_time, 0.0); - const int delay_step = std::roundf(delay_time / dt); + const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time); - pose_diag_info.delay_time_threshold = params_.extend_state_step * dt; + pose_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { pose_diag_info.is_passed_delay_gate = false; warning_->warnThrottle( - poseDelayStepWarningMessage(delay_time, params_.extend_state_step, dt), 2000); + poseDelayStepWarningMessage(pose_diag_info.delay_time, pose_diag_info.delay_time_threshold), + 2000); return false; } @@ -243,7 +286,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDela } bool EKFModule::measurementUpdateTwist( - const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr, + const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info) { if (twist.header.frame_id != "base_link") { @@ -262,14 +305,16 @@ bool EKFModule::measurementUpdateTwist( } delay_time = std::max(delay_time, 0.0); - const int delay_step = std::roundf(delay_time / dt); + const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time); - twist_diag_info.delay_time_threshold = params_.extend_state_step * dt; + twist_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { twist_diag_info.is_passed_delay_gate = false; warning_->warnThrottle( - twistDelayStepWarningMessage(delay_time, params_.extend_state_step, dt), 2000); + twistDelayStepWarningMessage( + twist_diag_info.delay_time, twist_diag_info.delay_time_threshold), + 2000); return false; } diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp index 9a7bbf47a94eb..c69f30b2d6601 100644 --- a/localization/ekf_localizer/src/warning_message.cpp +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -18,22 +18,20 @@ #include -std::string poseDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt) +std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) { const std::string s = "Pose delay exceeds the compensation limit, ignored. " - "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]"; - return fmt::format(s, delay_time, extend_state_step * ekf_dt); + "delay: {:.3f}[s], limit: {:.3f}[s]"; + return fmt::format(s, delay_time, delay_time_threshold); } -std::string twistDelayStepWarningMessage( - const double delay_time, const int extend_state_step, const double ekf_dt) +std::string twistDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) { const std::string s = "Twist delay exceeds the compensation limit, ignored. " - "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]"; - return fmt::format(s, delay_time, extend_state_step * ekf_dt); + "delay: {:.3f}[s], limit: {:.3f}[s]"; + return fmt::format(s, delay_time, delay_time_threshold); } std::string poseDelayTimeWarningMessage(const double delay_time) diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index b4a05afa844dc..2069969e0ae5e 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -21,17 +21,17 @@ TEST(PoseDelayStepWarningMessage, SmokeTest) { EXPECT_STREQ( - poseDelayStepWarningMessage(6.0, 2, 2.0).c_str(), + poseDelayStepWarningMessage(6.0, 4.0).c_str(), "Pose delay exceeds the compensation limit, ignored. " - "delay: 6.000[s], limit = extend_state_step * ekf_dt : 4.000[s]"); + "delay: 6.000[s], limit: 4.000[s]"); } TEST(TwistDelayStepWarningMessage, SmokeTest) { EXPECT_STREQ( - twistDelayStepWarningMessage(10.0, 3, 2.0).c_str(), + twistDelayStepWarningMessage(10.0, 6.0).c_str(), "Twist delay exceeds the compensation limit, ignored. " - "delay: 10.000[s], limit = extend_state_step * ekf_dt : 6.000[s]"); + "delay: 10.000[s], limit: 6.000[s]"); } TEST(PoseDelayTimeWarningMessage, SmokeTest) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt b/planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt new file mode 100644 index 0000000000000..bca909c7cef7e --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_avoidance_by_lane_change_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/manager.cpp + src/interface.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml rename to planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp new file mode 100644 index 0000000000000..e177244930da6 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp @@ -0,0 +1,33 @@ +// Copyright 2022 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_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ + +#include "behavior_path_avoidance_module/data_structs.hpp" + +#include + +namespace behavior_path_planner +{ +struct AvoidanceByLCParameters : public AvoidanceParameters +{ + // execute only when the target object longitudinal distance is larger than this param. + double execute_object_longitudinal_margin{0.0}; + + // execute only when lane change end point is before the object. + bool execute_only_when_lane_change_finish_before_object{false}; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp new file mode 100644 index 0000000000000..0bc08ccd400ca --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp @@ -0,0 +1,48 @@ +// 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_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_ + +#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" +#include "behavior_path_avoidance_by_lane_change_module/scene.hpp" +#include "behavior_path_planner/scene_module/lane_change/interface.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner +{ +class AvoidanceByLaneChangeInterface : public LaneChangeInterface +{ +public: + AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); + + bool isExecutionRequested() const override; + +protected: + void updateRTCStatus(const double start_distance, const double finish_distance) override; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp new file mode 100644 index 0000000000000..b1beae9c6fc03 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp @@ -0,0 +1,52 @@ +// 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_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_ + +#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" +#include "behavior_path_avoidance_by_lane_change_module/interface.hpp" +#include "behavior_path_planner/scene_module/lane_change/manager.hpp" + +#include + +#include +#include +#include +#include + +namespace behavior_path_planner +{ +using route_handler::Direction; + +class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager +{ +public: + AvoidanceByLaneChangeModuleManager() + : LaneChangeModuleManager( + "avoidance_by_lc", route_handler::Direction::NONE, + LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) + { + } + + void init(rclcpp::Node * node) override; + + std::unique_ptr createNewSceneModuleInstance() override; + +private: + std::shared_ptr avoidance_parameters_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp similarity index 81% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp rename to planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp index 67fd2c1ba0c77..1e667c207c64f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp @@ -12,18 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ +#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" #include "behavior_path_planner/scene_module/lane_change/normal.hpp" #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using geometry_msgs::msg::Pose; -using geometry_msgs::msg::Twist; using AvoidanceDebugData = DebugData; class AvoidanceByLaneChange : public NormalLaneChange @@ -56,4 +54,4 @@ class AvoidanceByLaneChange : public NormalLaneChange }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_avoidance_by_lane_change_module/package.xml new file mode 100644 index 0000000000000..da51dd9f235dc --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/package.xml @@ -0,0 +1,39 @@ + + + + behavior_path_avoidance_by_lane_change_module + 0.1.0 + The behavior_path_avoidance_by_lane_change_module package + + Satoshi Ota + Zulfaqar Azmi + Fumiya Watanabe + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_avoidance_module + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_avoidance_by_lane_change_module/plugins.xml b/planning/behavior_path_avoidance_by_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..092d54c096ae9 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp new file mode 100644 index 0000000000000..2e451461abab7 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -0,0 +1,62 @@ +// 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. + +#include "behavior_path_avoidance_by_lane_change_module/interface.hpp" + +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" + +#include +#include +#include +#include +#include + +namespace behavior_path_planner +{ +AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: LaneChangeInterface{ + name, + node, + parameters, + rtc_interface_ptr_map, + objects_of_interest_marker_interface_ptr_map, + std::make_unique(parameters, avoidance_by_lane_change_parameters)} +{ +} + +bool AvoidanceByLaneChangeInterface::isExecutionRequested() const +{ + return module_type_->specialRequiredCheck() && module_type_->isLaneChangeRequired(); +} + +void AvoidanceByLaneChangeInterface::updateRTCStatus( + const double start_distance, const double finish_distance) +{ + const auto direction = std::invoke([&]() -> std::string { + const auto dir = module_type_->getDirection(); + return (dir == Direction::LEFT) ? "left" : "right"; + }); + + rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( + uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp new file mode 100644 index 0000000000000..61b0a36ae0fb0 --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -0,0 +1,176 @@ +// 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. + +#include "behavior_path_avoidance_by_lane_change_module/manager.hpp" + +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner +{ + +void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + using tier4_autoware_utils::getOrDeclareParameter; + + // init manager interface + initInterface(node, {"left", "right"}); + + // init lane change manager + LaneChangeModuleManager::init(node); + + AvoidanceByLCParameters p{}; + // unique parameters + { + const std::string ns = "avoidance_by_lane_change."; + p.execute_object_longitudinal_margin = + getOrDeclareParameter(*node, ns + "execute_object_longitudinal_margin"); + p.execute_only_when_lane_change_finish_before_object = + getOrDeclareParameter(*node, ns + "execute_only_when_lane_change_finish_before_object"); + } + + // general params + { + const std::string ns = "avoidance."; + p.resample_interval_for_planning = + getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); + p.resample_interval_for_output = + getOrDeclareParameter(*node, ns + "resample_interval_for_output"); + } + + // target object + { + const auto get_object_param = [&](std::string && ns) { + ObjectParameter param{}; + param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); + param.moving_speed_threshold = + getOrDeclareParameter(*node, ns + "moving_speed_threshold"); + param.moving_time_threshold = + getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); + param.envelope_buffer_margin = + getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = + getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); + param.safety_buffer_lateral = + getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + return param; + }; + + const std::string ns = "avoidance_by_lane_change.target_object."; + p.object_parameters.emplace( + ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); + p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); + p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); + p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); + p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); + p.object_parameters.emplace( + ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); + p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); + p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); + + p.lower_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); + p.upper_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); + } + + // target filtering + { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_avoidance_target = + getOrDeclareParameter(*node, ns); + }; + + const std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + + p.object_check_goal_distance = + getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.threshold_distance_object_is_on_center = + getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); + p.object_check_shiftable_ratio = + getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); + p.object_check_min_road_shoulder_width = + getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); + p.object_last_seen_threshold = + getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); + } + + { + const std::string ns = "avoidance.target_filtering.force_avoidance."; + p.enable_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "enable"); + p.threshold_time_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "time_threshold"); + p.object_ignore_section_traffic_light_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + } + + { + const std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); + } + + // safety check + { + const std::string ns = "avoidance.safety_check."; + p.hysteresis_factor_expand_rate = + getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); + } + + avoidance_parameters_ = std::make_shared(p); +} + +std::unique_ptr +AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); +} + +} // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::AvoidanceByLaneChangeModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp similarity index 97% rename from planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp rename to planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index befd458153e6e..bfa873d9518ef 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -12,15 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" +#include "behavior_path_avoidance_by_lane_change_module/scene.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" -#include -#include - #include #include diff --git a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..2264669fdcdbe --- /dev/null +++ b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,129 @@ +// 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. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::AvoidanceByLaneChangeModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_planner") + + "/config/lane_change/lane_change.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + + "/config/avoidance.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_avoidance_by_lane_change_module") + + "/config/avoidance_by_lc.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_avoidance_module/CMakeLists.txt b/planning/behavior_path_avoidance_module/CMakeLists.txt new file mode 100644 index 0000000000000..c94591f9bbd30 --- /dev/null +++ b/planning/behavior_path_avoidance_module/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_avoidance_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/debug.cpp + src/utils.cpp + src/manager.cpp + src/shift_line_generator.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_utils.cpp + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/avoidance/avoidance.param.yaml rename to planning/behavior_path_avoidance_module/config/avoidance.param.yaml diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp similarity index 98% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 68c64ff9b7112..8c45230959c6c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -622,4 +622,4 @@ struct DebugData } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp similarity index 90% rename from planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp index 12b54a5031041..2bf3158da9ab6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ -#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include @@ -78,4 +78,4 @@ std::string toStrInfo(const behavior_path_planner::ShiftLine & sp); std::string toStrInfo(const behavior_path_planner::AvoidLine & ap); -#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 54a38b828e987..6717cefefc9f6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__HELPER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__HELPER_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include @@ -331,4 +331,4 @@ class AvoidanceHelper }; } // namespace behavior_path_planner::helper::avoidance -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__HELPER_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp index 0fe8bd7efa17b..72cfbe6f0a1ed 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -51,4 +51,4 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index b01cb62f22372..6a66b485d1c59 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/helper.hpp" -#include "behavior_path_planner/utils/avoidance/shift_line_generator.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/helper.hpp" +#include "behavior_path_avoidance_module/shift_line_generator.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" @@ -450,4 +450,4 @@ class AvoidanceModule : public SceneModuleInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp similarity index 95% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp index 054a6e15ef6bf..edfe9ab9663ce 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__SHIFT_LINE_GENERATOR_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__SHIFT_LINE_GENERATOR_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/helper.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/helper.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -248,4 +248,4 @@ class ShiftLineGenerator } // namespace behavior_path_planner::utils::avoidance -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__SHIFT_LINE_GENERATOR_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp rename to planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index e7611208962f0..c954ab9022853 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -180,4 +180,4 @@ double calcDistanceToAvoidStartLine( const std::shared_ptr & parameters); } // namespace behavior_path_planner::utils::avoidance -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_avoidance_module/package.xml b/planning/behavior_path_avoidance_module/package.xml new file mode 100644 index 0000000000000..642f83497acfb --- /dev/null +++ b/planning/behavior_path_avoidance_module/package.xml @@ -0,0 +1,56 @@ + + + + behavior_path_avoidance_module + 0.1.0 + The behavior_path_avoidance_module package + + Takamasa Horibe + Satoshi Ota + Fumiya Watanabe + Kyoichi Sugahara + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_tf2 + autoware_perception_msgs + behavior_path_planner + behavior_path_planner_common + geometry_msgs + lanelet2_extension + libboost-dev + magic_enum + motion_utils + objects_of_interest_marker_interface + pluginlib + rclcpp + route_handler + rtc_interface + sensor_msgs + signal_processing + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_avoidance_module/plugins.xml b/planning/behavior_path_avoidance_module/plugins.xml new file mode 100644 index 0000000000000..f25677dad1e9a --- /dev/null +++ b/planning/behavior_path_avoidance_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp similarity index 99% rename from planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp rename to planning/behavior_path_avoidance_module/src/debug.cpp index 7587c3bb3663e..3ae210e0b1d78 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/marker_utils/avoidance/debug.hpp" +#include "behavior_path_avoidance_module/debug.hpp" #include "behavior_path_planner_common/utils/utils.hpp" diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp similarity index 99% rename from planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp rename to planning/behavior_path_avoidance_module/src/manager.cpp index e17804f10deaa..f4997d2af12f1 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/avoidance/manager.hpp" +#include "behavior_path_avoidance_module/manager.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp similarity index 99% rename from planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp rename to planning/behavior_path_avoidance_module/src/scene.cpp index df93df75410e1..9b0c85f60ba29 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" +#include "behavior_path_avoidance_module/scene.hpp" -#include "behavior_path_planner/marker_utils/avoidance/debug.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/debug.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" diff --git a/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp similarity index 99% rename from planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp rename to planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index c7fbaa931737c..108412876d47f 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/avoidance/shift_line_generator.hpp" +#include "behavior_path_avoidance_module/shift_line_generator.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include @@ -966,6 +966,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( AvoidLineArray ret = shift_lines; constexpr double ep = 1.0e-3; + constexpr double RETURN_SHIFT_THRESHOLD = 0.1; const bool has_candidate_point = !ret.empty(); const bool has_registered_point = last_.has_value(); @@ -977,14 +978,15 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( return ret; } - // If the return-to-center shift points are already registered, do nothing. - if (!has_registered_point && std::fabs(base_offset_) < ep) { - return ret; - } - - constexpr double RETURN_SHIFT_THRESHOLD = 0.1; - if (std::abs(last_.value().end_shift_length) < RETURN_SHIFT_THRESHOLD) { - return ret; + if (last_.has_value()) { + if (std::abs(last_.value().end_shift_length) < RETURN_SHIFT_THRESHOLD) { + return ret; + } + } else { + // If the return-to-center shift points are already registered, do nothing. + if (std::abs(base_offset_) < ep) { + return ret; + } } // From here, the return-to-center is not registered. But perhaps the candidate is diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp similarity index 99% rename from planning/behavior_path_planner/src/utils/avoidance/utils.cpp rename to planning/behavior_path_avoidance_module/src/utils.cpp index 6fd1943369cb9..b469e9070baad 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -14,8 +14,8 @@ #include "behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" diff --git a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..83514127b0a8e --- /dev/null +++ b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,126 @@ +// 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. + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager.hpp" +#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include + +#include +#include + +using behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + + std::vector module_names; + module_names.emplace_back("behavior_path_planner::AvoidanceModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_planner") + + "/config/lane_change/lane_change.param.yaml", + ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + + "/config/avoidance.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_planner/test/test_avoidance_utils.cpp b/planning/behavior_path_avoidance_module/test/test_utils.cpp similarity index 92% rename from planning/behavior_path_planner/test/test_avoidance_utils.cpp rename to planning/behavior_path_avoidance_module/test/test_utils.cpp index fc2bea1a5b288..e5c6fd2e72092 100644 --- a/planning/behavior_path_planner/test/test_avoidance_utils.cpp +++ b/planning/behavior_path_avoidance_module/test/test_utils.cpp @@ -11,8 +11,9 @@ // 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. -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" + +#include "behavior_path_avoidance_module/data_structs.hpp" +#include "behavior_path_avoidance_module/utils.hpp" #include #include diff --git a/planning/behavior_path_goal_planner_module/CMakeLists.txt b/planning/behavior_path_goal_planner_module/CMakeLists.txt new file mode 100644 index 0000000000000..c91289bf29e2c --- /dev/null +++ b/planning/behavior_path_goal_planner_module/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_goal_planner_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/default_fixed_goal_planner.cpp + src/freespace_pull_over.cpp + src/geometric_pull_over.cpp + src/goal_searcher.cpp + src/shift_pull_over.cpp + src/util.cpp + src/goal_planner_module.cpp + src/manager.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml rename to planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp similarity index 80% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp index 1595503a225f8..4da625a7a9087 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ -#include "behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp" +#include "behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" #include @@ -42,4 +42,4 @@ class DefaultFixedGoalPlanner : public FixedGoalPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp similarity index 86% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index 5fe3427749844..0926d1010b5c3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ #include "behavior_path_planner_common/data_manager.hpp" @@ -50,4 +50,4 @@ class FixedGoalPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp index bb5d7b7555cfc..5ce6d83795069 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FREESPACE_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FREESPACE_PULL_OVER_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ -#include "behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp" +#include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" #include #include @@ -51,4 +51,4 @@ class FreespacePullOver : public PullOverPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FREESPACE_PULL_OVER_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp similarity index 86% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp index d96b01582a2d1..c5ba04ba7f52e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GEOMETRIC_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GEOMETRIC_PULL_OVER_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp" +#include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include @@ -69,4 +69,4 @@ class GeometricPullOver : public PullOverPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GEOMETRIC_PULL_OVER_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp similarity index 92% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index a9bc31983ac6c..7b5b9b1a68102 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ - -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp" -#include "behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp" -#include "behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp" -#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" -#include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" -#include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ + +#include "behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "behavior_path_goal_planner_module/freespace_pull_over.hpp" +#include "behavior_path_goal_planner_module/geometric_pull_over.hpp" +#include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "behavior_path_goal_planner_module/goal_searcher.hpp" +#include "behavior_path_goal_planner_module/shift_pull_over.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -282,6 +282,19 @@ class GoalPlannerModule : public SceneModuleInterface bool isExecutionReady() const override; void processOnExit() override; void updateData() override; + + void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & goal_planner_params); + + void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & goal_planner_params); + + void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & goal_planner_params); + void postProcess() override; void setParameters(const std::shared_ptr & parameters); void acceptVisitor( @@ -291,7 +304,7 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; } private: - /*　 + /* * state transitions and plan function used in each state * * +--------------------------+ @@ -494,4 +507,4 @@ class GoalPlannerModule : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp similarity index 92% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp index c5adf3a2ad652..21eb22046bff6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -13,10 +13,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -124,4 +124,4 @@ struct GoalPlannerParameters }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp similarity index 88% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp index 2fc0acf1c5086..2d9e2c6cf3700 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ -#include "behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp" +#include "behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include @@ -55,4 +55,4 @@ class GoalSearcher : public GoalSearcherBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp similarity index 86% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp index 7d1c99a69b1d5..93472ab6c0703 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ -#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include @@ -69,4 +69,4 @@ class GoalSearcherBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/manager.hpp similarity index 84% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/manager.hpp index a166a677c7388..aefd4cac3ee35 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" +#include "behavior_path_goal_planner_module/goal_planner_module.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -55,4 +55,4 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp similarity index 92% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index a0bfd8e883d80..ddd7c93998654 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ -#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" @@ -135,4 +135,4 @@ class PullOverPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp similarity index 88% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp index 9247ec566fbd8..cc8686f4a9fef 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__SHIFT_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__SHIFT_PULL_OVER_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ -#include "behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp" +#include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include @@ -61,4 +61,4 @@ class ShiftPullOver : public PullOverPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__SHIFT_PULL_OVER_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp similarity index 91% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp rename to planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index 25c0551d896b3..791c35f3afca6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ +#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ +#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ -#include "behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp" +#include "behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -71,4 +71,4 @@ MarkerArray createNumObjectsToAvoidTextsMarkerArray( } // namespace goal_planner_utils } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ +#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_goal_planner_module/package.xml b/planning/behavior_path_goal_planner_module/package.xml new file mode 100644 index 0000000000000..213c0093b08d9 --- /dev/null +++ b/planning/behavior_path_goal_planner_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_path_goal_planner_module + 0.1.0 + The behavior_path_goal_planner_module package + + Kosuke Takeuchi + Kyoichi Sugahara + Satoshi OTA + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_goal_planner_module/plugins.xml b/planning/behavior_path_goal_planner_module/plugins.xml new file mode 100644 index 0000000000000..6c3b557e35e7a --- /dev/null +++ b/planning/behavior_path_goal_planner_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp similarity index 96% rename from planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp rename to planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index 9ae50def1d358..f028b8aff8b98 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp" +#include "behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp similarity index 94% rename from planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp rename to planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp index d5252028f880b..c9af9ee369bd5 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp" +#include "behavior_path_goal_planner_module/freespace_pull_over.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include "behavior_path_goal_planner_module/util.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include @@ -116,7 +116,7 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) std::vector> pairs_terminal_velocity_and_accel{}; pairs_terminal_velocity_and_accel.resize(partial_paths.size()); - utils::start_goal_planner_common::modifyVelocityByDirection( + utils::parking_departure::modifyVelocityByDirection( partial_paths, pairs_terminal_velocity_and_accel, velocity_, 0); // Check if driving forward for each path, return empty if not diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp similarity index 95% rename from planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp rename to planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp index 55d8bc6cfda3f..31c1d3ef77b0e 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp" +#include "behavior_path_goal_planner_module/geometric_pull_over.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include #include diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp similarity index 97% rename from planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp rename to planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index e8905f64e9617..98f5664153bfd 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" +#include "behavior_path_goal_planner_module/goal_planner_module.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -39,7 +39,7 @@ #include #include -using behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance; +using behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; @@ -120,8 +120,7 @@ GoalPlannerModule::GoalPlannerModule( // Initialize safety checker if (parameters_->safety_check_params.enable_safety_check) { initializeSafetyCheckParameters(); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - goal_planner_data_.collision_check); + utils::parking_departure::initializeCollisionCheckDebugMap(goal_planner_data_.collision_check); } prev_data_.reset(); @@ -242,6 +241,30 @@ void GoalPlannerModule::onFreespaceParkingTimer() } } +void GoalPlannerModule::updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & goal_planner_params) +{ + ego_predicted_path_params = + std::make_shared(goal_planner_params->ego_predicted_path_params); +} + +void GoalPlannerModule::updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & goal_planner_params) +{ + safety_check_params = + std::make_shared(goal_planner_params->safety_check_params); +} + +void GoalPlannerModule::updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & goal_planner_params) +{ + objects_filtering_params = + std::make_shared(goal_planner_params->objects_filtering_params); +} + void GoalPlannerModule::updateData() { if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) { @@ -303,11 +326,9 @@ void GoalPlannerModule::initializeOccupancyGridMap() void GoalPlannerModule::initializeSafetyCheckParameters() { - utils::start_goal_planner_common::updateEgoPredictedPathParams( - ego_predicted_path_params_, parameters_); - utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); - utils::start_goal_planner_common::updateObjectsFilteringParams( - objects_filtering_params_, parameters_); + updateEgoPredictedPathParams(ego_predicted_path_params_, parameters_); + updateSafetyCheckParams(safety_check_params_, parameters_); + updateObjectsFilteringParams(objects_filtering_params_, parameters_); } void GoalPlannerModule::processOnExit() @@ -775,7 +796,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output if (prev_data_.safety_status.is_safe || !prev_data_.stop_path_after_approval) { auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto stop_path = - behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( + behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop); if (stop_path) { @@ -852,7 +873,6 @@ void GoalPlannerModule::updateSteeringFactor( return SteeringFactor::STRAIGHT; }); - // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( pose, distance, PlanningBehavior::GOAL_PLANNER, steering_factor_direction, type, ""); } @@ -1761,13 +1781,13 @@ std::pair GoalPlannerModule::isSafePath() const parameters_->forward_goal_search_length); const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); const std::pair terminal_velocity_and_accel = - utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + utils::parking_departure::getPairsTerminalVelocityAndAccel( thread_safe_data_.get_pull_over_path()->pairs_terminal_velocity_and_accel, thread_safe_data_.get_pull_over_path()->path_idx); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); - utils::start_goal_planner_common::updatePathProperty( + utils::parking_departure::updatePathProperty( ego_predicted_path_params_, terminal_velocity_and_accel); // TODO(Sugahara): shoule judge is_object_front properly const bool is_object_front = true; @@ -1786,7 +1806,7 @@ std::pair GoalPlannerModule::isSafePath() const const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); - utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( + utils::parking_departure::updateSafetyCheckTargetObjectsData( goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); const double hysteresis_factor = @@ -1812,7 +1832,7 @@ std::pair GoalPlannerModule::isSafePath() const throw std::domain_error("[pull_over] invalid safety check method"); }); - /*　　 + /* *　　 ==== is_safe *　　 ---- current_is_safe *　　 is_safe @@ -1937,8 +1957,7 @@ void GoalPlannerModule::setDebugData() } add(showPredictedPath(goal_planner_data_.collision_check, "ego_predicted_path")); add(showPolygon(goal_planner_data_.collision_check, "ego_and_target_polygon_relation")); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - goal_planner_data_.collision_check); + utils::parking_departure::initializeCollisionCheckDebugMap(goal_planner_data_.collision_check); // visualize safety status maker { diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp similarity index 99% rename from planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp rename to planning/behavior_path_goal_planner_module/src/goal_searcher.cpp index ee8dae231f587..b087827a9c539 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" +#include "behavior_path_goal_planner_module/goal_searcher.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp similarity index 99% rename from planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp rename to planning/behavior_path_goal_planner_module/src/manager.cpp index 0916b5e8ebaef..3079361253c31 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/goal_planner/manager.hpp" +#include "behavior_path_goal_planner_module/manager.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp similarity index 98% rename from planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp rename to planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp index 9ff452b131efa..712da5aa03a4e 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" +#include "behavior_path_goal_planner_module/shift_pull_over.hpp" -#include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp similarity index 99% rename from planning/behavior_path_planner/src/utils/goal_planner/util.cpp rename to planning/behavior_path_goal_planner_module/src/util.cpp index 1cd985d566f73..a01f5680dcc13 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_goal_planner_module/util.hpp" #include #include diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 2ea43bf5a4ef1..75f9e7f7af187 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -12,39 +12,17 @@ pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/planner_manager.cpp src/behavior_path_planner_node.cpp - src/scene_module/avoidance/avoidance_module.cpp - src/scene_module/avoidance/manager.cpp src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp src/scene_module/dynamic_avoidance/manager.cpp - src/scene_module/start_planner/start_planner_module.cpp - src/scene_module/start_planner/manager.cpp - src/scene_module/goal_planner/goal_planner_module.cpp - src/scene_module/goal_planner/manager.cpp src/scene_module/side_shift/side_shift_module.cpp src/scene_module/side_shift/manager.cpp src/scene_module/lane_change/interface.cpp src/scene_module/lane_change/normal.cpp src/scene_module/lane_change/external_request.cpp - src/scene_module/lane_change/avoidance_by_lane_change.cpp src/scene_module/lane_change/manager.cpp - src/utils/start_goal_planner_common/utils.cpp - src/utils/avoidance/shift_line_generator.cpp - src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp - src/utils/goal_planner/util.cpp - src/utils/goal_planner/shift_pull_over.cpp - src/utils/goal_planner/geometric_pull_over.cpp - src/utils/goal_planner/freespace_pull_over.cpp - src/utils/goal_planner/goal_searcher.cpp - src/utils/goal_planner/default_fixed_goal_planner.cpp - src/utils/start_planner/util.cpp - src/utils/start_planner/shift_pull_out.cpp - src/utils/start_planner/geometric_pull_out.cpp - src/utils/start_planner/freespace_pull_out.cpp - src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp - src/marker_utils/avoidance/debug.cpp src/marker_utils/lane_change/debug.cpp ) @@ -71,14 +49,6 @@ if(BUILD_TESTING) ${PROJECT_NAME}_lib ) - ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_avoidance_module - test/test_avoidance_utils.cpp - ) - - target_link_libraries(test_${CMAKE_PROJECT_NAME}_avoidance_module - ${PROJECT_NAME}_lib - ) - ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_lane_change_module test/test_lane_change_utils.cpp ) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index 2e3cbd992e27f..b7cef007d6d9f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -15,7 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ -#include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" #include "behavior_path_planner/scene_module/lane_change/base_class.hpp" #include "behavior_path_planner/scene_module/lane_change/external_request.hpp" #include "behavior_path_planner/scene_module/lane_change/normal.hpp" @@ -145,23 +144,6 @@ class LaneChangeInterface : public SceneModuleInterface bool is_abort_approval_requested_{false}; }; - -class AvoidanceByLaneChangeInterface : public LaneChangeInterface -{ -public: - AvoidanceByLaneChangeInterface( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::shared_ptr & avoidance_by_lane_change_parameters, - const std::unordered_map> & rtc_interface_ptr_map, - std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); - - bool isExecutionRequested() const override; - -protected: - void updateRTCStatus(const double start_distance, const double finish_distance) override; -}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp index ed476adcbd834..024c6685b28c4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp @@ -94,24 +94,6 @@ class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManage { } }; - -class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager -{ -public: - AvoidanceByLaneChangeModuleManager() - : LaneChangeModuleManager( - "avoidance_by_lc", route_handler::Direction::NONE, - LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) - { - } - - void init(rclcpp::Node * node) override; - - std::unique_ptr createNewSceneModuleInstance() override; - -private: - std::shared_ptr avoidance_parameters_; -}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index f11d48a24c79c..6f34bc79fe5f1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -14,8 +14,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include @@ -147,15 +147,6 @@ enum class LaneChangeModuleType { EXTERNAL_REQUEST, AVOIDANCE_BY_LANE_CHANGE, }; - -struct AvoidanceByLCParameters : public AvoidanceParameters -{ - // execute only when the target object longitudinal distance is larger than this param. - double execute_object_longitudinal_margin{0.0}; - - // execute only when lane change end point is before the object. - bool execute_only_when_lane_change_finish_before_object{false}; -}; } // namespace behavior_path_planner namespace behavior_path_planner::data::lane_change diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 5ca86ee5a47b6..0ca7badd0b3f4 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -7,28 +7,19 @@ Zulfaqar Azmi - - Kosuke Takeuchi - - Kosuke Takeuchi Fumiya Watanabe Kyoichi Sugahara - - Takamasa Horibe - Satoshi Ota - - Zulfaqar Azmi - Satoshi Ota - Mamoru Sobue - - - Takayuki Murooka - Taiki Tanaka Tomoya Kimura Shumpei Wakabayashi + Tomohito Ando + Takamasa Horibe + Satoshi Ota + Kosuke Takeuchi + Kyoichi Sugahara + Takayuki Murooka Apache License 2.0 diff --git a/planning/behavior_path_planner/plugins.xml b/planning/behavior_path_planner/plugins.xml index 561246fcf75e4..fed2e1c9af3c3 100644 --- a/planning/behavior_path_planner/plugins.xml +++ b/planning/behavior_path_planner/plugins.xml @@ -1,12 +1,8 @@ - - - - diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 413350e3eb23b..cec9d5013d929 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -372,7 +372,6 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o const auto finish_distance = motion_utils::calcSignedArcLength( output.path->points, current_position, status.lane_change_path.info.shift_line.end.position); - // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end}, {start_distance, finish_distance}, PlanningBehavior::LANE_CHANGE, steering_factor_direction, @@ -474,38 +473,4 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( // not in the vicinity of the end of the path. return original return original_turn_signal_info; } - -AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr & parameters, - const std::shared_ptr & avoidance_by_lane_change_parameters, - const std::unordered_map> & rtc_interface_ptr_map, - std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: LaneChangeInterface{ - name, - node, - parameters, - rtc_interface_ptr_map, - objects_of_interest_marker_interface_ptr_map, - std::make_unique(parameters, avoidance_by_lane_change_parameters)} -{ -} - -bool AvoidanceByLaneChangeInterface::isExecutionRequested() const -{ - return module_type_->specialRequiredCheck() && module_type_->isLaneChangeRequired(); -} - -void AvoidanceByLaneChangeInterface::updateRTCStatus( - const double start_distance, const double finish_distance) -{ - const auto direction = std::invoke([&]() -> std::string { - const auto dir = module_type_->getDirection(); - return (dir == Direction::LEFT) ? "left" : "right"; - }); - - rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( - uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); -} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 29b7cad533a66..ed40476723dba 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -246,148 +246,6 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(*node, ns + "execute_object_longitudinal_margin"); - p.execute_only_when_lane_change_finish_before_object = - getOrDeclareParameter(*node, ns + "execute_only_when_lane_change_finish_before_object"); - } - - // general params - { - const std::string ns = "avoidance."; - p.resample_interval_for_planning = - getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); - p.resample_interval_for_output = - getOrDeclareParameter(*node, ns + "resample_interval_for_output"); - } - - // target object - { - const auto get_object_param = [&](std::string && ns) { - ObjectParameter param{}; - param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); - param.moving_speed_threshold = - getOrDeclareParameter(*node, ns + "moving_speed_threshold"); - param.moving_time_threshold = - getOrDeclareParameter(*node, ns + "moving_time_threshold"); - param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); - param.envelope_buffer_margin = - getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); - return param; - }; - - const std::string ns = "avoidance_by_lane_change.target_object."; - p.object_parameters.emplace( - ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); - p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); - p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); - p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); - p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); - p.object_parameters.emplace( - ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); - p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); - p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); - - p.lower_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); - p.upper_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); - } - - // target filtering - { - const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { - if (p.object_parameters.count(object_type) == 0) { - return; - } - p.object_parameters.at(object_type).is_avoidance_target = - getOrDeclareParameter(*node, ns); - }; - - const std::string ns = "avoidance.target_filtering."; - set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); - set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); - set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); - set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); - set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); - set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); - set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); - set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); - - p.object_check_goal_distance = - getOrDeclareParameter(*node, ns + "object_check_goal_distance"); - p.threshold_distance_object_is_on_center = - getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); - p.object_check_shiftable_ratio = - getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); - p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); - p.object_last_seen_threshold = - getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); - } - - { - const std::string ns = "avoidance.target_filtering.force_avoidance."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); - p.object_ignore_section_traffic_light_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); - p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); - } - - { - const std::string ns = "avoidance.target_filtering.detection_area."; - p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); - p.object_check_min_forward_distance = - getOrDeclareParameter(*node, ns + "min_forward_distance"); - p.object_check_max_forward_distance = - getOrDeclareParameter(*node, ns + "max_forward_distance"); - p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "backward_distance"); - } - - // safety check - { - const std::string ns = "avoidance.safety_check."; - p.hysteresis_factor_expand_rate = - getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); - } - - avoidance_parameters_ = std::make_shared(p); -} - -std::unique_ptr -AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() -{ - return std::make_unique( - name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); -} - } // namespace behavior_path_planner #include @@ -403,6 +261,3 @@ PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS( behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, behavior_path_planner::SceneModuleManagerInterface) -PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::AvoidanceByLaneChangeModuleManager, - behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index 9235e8e8e3de6..0e253c0c73985 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -49,16 +49,12 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_planner"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::AvoidanceModuleManager"); module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); module_names.emplace_back("behavior_path_planner::SideShiftModuleManager"); - module_names.emplace_back("behavior_path_planner::StartPlannerModuleManager"); - module_names.emplace_back("behavior_path_planner::GoalPlannerModuleManager"); module_names.emplace_back("behavior_path_planner::LaneChangeRightModuleManager"); module_names.emplace_back("behavior_path_planner::LaneChangeLeftModuleManager"); module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); - module_names.emplace_back("behavior_path_planner::AvoidanceByLaneChangeModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); @@ -72,12 +68,8 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_planner_dir + "/config/avoidance/avoidance.param.yaml", behavior_path_planner_dir + "/config/dynamic_avoidance/dynamic_avoidance.param.yaml", behavior_path_planner_dir + "/config/lane_change/lane_change.param.yaml", - behavior_path_planner_dir + "/config/start_planner/start_planner.param.yaml", - behavior_path_planner_dir + "/config/goal_planner/goal_planner.param.yaml", - behavior_path_planner_dir + "/config/avoidance_by_lc/avoidance_by_lc.param.yaml", behavior_path_planner_dir + "/config/side_shift/side_shift.param.yaml"}); return std::make_shared(node_options); diff --git a/planning/behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner_common/CMakeLists.txt index 5567f04d5726b..5dbf81280f9b1 100644 --- a/planning/behavior_path_planner_common/CMakeLists.txt +++ b/planning/behavior_path_planner_common/CMakeLists.txt @@ -21,6 +21,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/drivable_area_expansion/drivable_area_expansion.cpp src/utils/drivable_area_expansion/map_utils.cpp src/utils/drivable_area_expansion/footprints.cpp + src/utils/parking_departure/geometric_parallel_parking.cpp + src/utils/parking_departure/utils.cpp src/marker_utils/utils.cpp ) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp similarity index 84% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index 84d84e0628cad..c270a1500431c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -44,4 +44,4 @@ struct StartGoalPlannerData } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp similarity index 95% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 7f5eee7a12aaa..4d1f1fdb959c0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/parameters.hpp" @@ -150,4 +150,4 @@ class GeometricParallelParking }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp similarity index 63% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp index 16be48b5acc6d..1d4245bfec0e4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp @@ -12,14 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ -#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -28,10 +25,9 @@ #include #include -namespace behavior_path_planner::utils::start_goal_planner_common +namespace behavior_path_planner::utils::parking_departure { -using behavior_path_planner::StartPlannerParameters; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; @@ -59,30 +55,6 @@ void modifyVelocityByDirection( std::vector> & terminal_vel_acc_pairs, const double target_velocity, const double acceleration); -void updateEgoPredictedPathParams( - std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & start_planner_params); - -void updateEgoPredictedPathParams( - std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & goal_planner_params); - -void updateSafetyCheckParams( - std::shared_ptr & safety_check_params, - const std::shared_ptr & start_planner_params); - -void updateSafetyCheckParams( - std::shared_ptr & safety_check_params, - const std::shared_ptr & goal_planner_params); - -void updateObjectsFilteringParams( - std::shared_ptr & objects_filtering_params, - const std::shared_ptr & start_planner_params); - -void updateObjectsFilteringParams( - std::shared_ptr & objects_filtering_params, - const std::shared_ptr & goal_planner_params); - void updatePathProperty( std::shared_ptr & ego_predicted_path_params, const std::pair & pairs_terminal_velocity_and_accel); @@ -103,6 +75,14 @@ std::optional generateFeasibleStopPath( geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, const double maximum_jerk); -} // namespace behavior_path_planner::utils::start_goal_planner_common +/** + * @brief calculate end arc length to generate reference path considering the goal position + * @return a pair of s_end and terminal_is_goal + */ +std::pair calcEndArcLength( + const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, + const Pose & goal_pose); + +} // namespace behavior_path_planner::utils::parking_departure -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 018e6bd2a3d6e..f66f425f1b7fa 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -134,6 +134,19 @@ void filterObjectsByPosition( const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance); +/** + * @brief Filter objects based on their distance from a reference point. + * + * This function filters out objects that are outside a specified radius from a reference point. + * + * @param objects The predicted objects to filter. + * @param reference_point The reference point (e.g., current pose of the ego vehicle). + * @param search_radius The radius within which to retain objects. + */ +void filterObjectsWithinRadius( + PredictedObjects & objects, const geometry_msgs::msg::Point & reference_point, + const double search_radius); + /** * @brief Filters the provided objects based on their classification. * diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp similarity index 98% rename from planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp rename to planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index e6dbd2679db3c..7ba4f114328a7 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -281,7 +281,7 @@ bool GeometricParallelParking::planPullOut( // get road center line path from pull_out end to goal, and combine after the second arc path const double s_start = getArcCoordinates(road_lanes, *end_pose).length; - const auto path_end_info = start_planner_utils::calcEndArcLength( + const auto path_end_info = utils::parking_departure::calcEndArcLength( s_start, planner_data_->parameters.forward_path_length, road_lanes, goal_pose); const double s_end = path_end_info.first; const bool path_terminal_is_goal = path_end_info.second; diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp similarity index 72% rename from planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp rename to planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp index cbc6d6a4a0b8d..58fd392988ad9 100644 --- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp @@ -12,9 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -namespace behavior_path_planner::utils::start_goal_planner_common +#include "behavior_path_planner_common/utils/utils.hpp" + +#include + +namespace behavior_path_planner::utils::parking_departure { using motion_utils::calcDecelDistWithJerkAndAccConstraints; @@ -80,53 +84,6 @@ void modifyVelocityByDirection( } } -void updateEgoPredictedPathParams( - std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & start_planner_params) -{ - ego_predicted_path_params = - std::make_shared(start_planner_params->ego_predicted_path_params); -} -void updateEgoPredictedPathParams( - std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & goal_planner_params) -{ - ego_predicted_path_params = - std::make_shared(goal_planner_params->ego_predicted_path_params); -} - -void updateSafetyCheckParams( - std::shared_ptr & safety_check_params, - const std::shared_ptr & start_planner_params) -{ - safety_check_params = - std::make_shared(start_planner_params->safety_check_params); -} - -void updateSafetyCheckParams( - std::shared_ptr & safety_check_params, - const std::shared_ptr & goal_planner_params) -{ - safety_check_params = - std::make_shared(goal_planner_params->safety_check_params); -} - -void updateObjectsFilteringParams( - std::shared_ptr & objects_filtering_params, - const std::shared_ptr & start_planner_params) -{ - objects_filtering_params = - std::make_shared(start_planner_params->objects_filtering_params); -} - -void updateObjectsFilteringParams( - std::shared_ptr & objects_filtering_params, - const std::shared_ptr & goal_planner_params) -{ - objects_filtering_params = - std::make_shared(goal_planner_params->objects_filtering_params); -} - void updatePathProperty( std::shared_ptr & ego_predicted_path_params, const std::pair & pairs_terminal_velocity_and_accel) @@ -178,7 +135,7 @@ std::optional generateFeasibleStopPath( // try to insert stop point in current_path after approval // but if can't stop with constraints(maximum deceleration, maximum jerk), don't insert stop point const auto min_stop_distance = - behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance( + behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance( planner_data, maximum_deceleration, maximum_jerk, 0.0); if (!min_stop_distance) { @@ -198,4 +155,25 @@ std::optional generateFeasibleStopPath( return current_path; } -} // namespace behavior_path_planner::utils::start_goal_planner_common +std::pair calcEndArcLength( + const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, + const Pose & goal_pose) +{ + const double s_forward_length = s_start + forward_path_length; + // use forward length if the goal pose is not in the lanelets + if (!utils::isInLanelets(goal_pose, road_lanes)) { + return {s_forward_length, false}; + } + + const double s_goal = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; + + // If the goal is behind the start or beyond the forward length, use forward length. + if (s_goal < s_start || s_goal >= s_forward_length) { + return {s_forward_length, false}; + } + + // path end is goal + return {s_goal, true}; +} + +} // namespace behavior_path_planner::utils::parking_departure diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 84c02b3b325de..188c50993e4ae 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -44,6 +44,15 @@ bool position_filter( return (backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance); } + +bool is_within_circle( + const geometry_msgs::msg::Point & object_pos, const geometry_msgs::msg::Point & reference_point, + const double search_radius) +{ + const double dist = + std::hypot(reference_point.x - object_pos.x, reference_point.y - object_pos.y); + return dist < search_radius; +} } // namespace behavior_path_planner::utils::path_safety_checker::filter namespace behavior_path_planner::utils::path_safety_checker @@ -128,6 +137,18 @@ void filterObjectsByPosition( filterObjects(objects, filter); } +void filterObjectsWithinRadius( + PredictedObjects & objects, const geometry_msgs::msg::Point & reference_point, + const double search_radius) +{ + const auto filter = [&](const auto & object) { + return filter::is_within_circle( + object.kinematics.initial_pose_with_covariance.pose.position, reference_point, search_radius); + }; + + filterObjects(objects, filter); +} + void filterObjectsByClass( PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) { diff --git a/planning/behavior_path_start_planner_module/CMakeLists.txt b/planning/behavior_path_start_planner_module/CMakeLists.txt new file mode 100644 index 0000000000000..6bb2f76cb9842 --- /dev/null +++ b/planning/behavior_path_start_planner_module/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_start_planner_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/start_planner_module.cpp + src/manager.cpp + src/freespace_pull_out.cpp + src/geometric_pull_out.cpp + src/shift_pull_out.cpp + src/util.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml similarity index 91% rename from planning/behavior_path_planner/config/start_planner/start_planner.param.yaml rename to planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 4bcb847d1280c..514d61e225ecd 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -138,6 +138,20 @@ backward_path_length: 30.0 forward_path_length: 100.0 + surround_moving_obstacle_check: + search_radius: 10.0 + th_moving_obstacle_velocity: 1.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # debug debug: print_debug_info: false diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp index 3a154dfd83353..f38279d172009 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ -#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp" +#include "behavior_path_start_planner_module/pull_out_planner_base.hpp" #include #include @@ -49,4 +49,4 @@ class FreespacePullOut : public PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp similarity index 70% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp index 4f46350d6c2b1..164868b47535e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__GEOMETRIC_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__GEOMETRIC_PULL_OUT_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "behavior_path_start_planner_module/pull_out_path.hpp" +#include "behavior_path_start_planner_module/pull_out_planner_base.hpp" #include @@ -36,4 +36,4 @@ class GeometricPullOut : public PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__GEOMETRIC_PULL_OUT_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp index 10fcbf81f8d45..3fb64baaa2bba 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/start_planner/start_planner_module.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "behavior_path_start_planner_module/start_planner_module.hpp" #include @@ -53,4 +53,4 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__MANAGER_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp similarity index 84% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp index 9c9c6fe83e288..68fcb25cac88e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PATH_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PATH_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -34,4 +34,4 @@ struct PullOutPath Pose end_pose{}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PATH_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index 60cd2731eb92f..de3d376fa4b3d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PLANNER_BASE_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" +#include "behavior_path_start_planner_module/pull_out_path.hpp" +#include "behavior_path_start_planner_module/start_planner_parameters.hpp" #include #include @@ -66,4 +66,4 @@ class PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PLANNER_BASE_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp similarity index 84% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index 7a868e6c2a234..9012504ed2e7a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__SHIFT_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__SHIFT_PULL_OUT_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp" +#include "behavior_path_start_planner_module/pull_out_path.hpp" +#include "behavior_path_start_planner_module/pull_out_planner_base.hpp" #include @@ -57,4 +57,4 @@ class ShiftPullOut : public PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__SHIFT_PULL_OUT_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp similarity index 84% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index e3249d7ffd311..a143f34ead649 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__START_PLANNER_MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__START_PLANNER_MODULE_HPP_ - -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" -#include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" -#include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" -#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ + #include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_start_planner_module/freespace_pull_out.hpp" +#include "behavior_path_start_planner_module/geometric_pull_out.hpp" +#include "behavior_path_start_planner_module/pull_out_path.hpp" +#include "behavior_path_start_planner_module/shift_pull_out.hpp" +#include "behavior_path_start_planner_module/start_planner_parameters.hpp" #include #include @@ -100,6 +100,18 @@ class StartPlannerModule : public SceneModuleInterface void processOnExit() override; void updateData() override; + void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params); + + void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params); + + void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params); + void setParameters(const std::shared_ptr & parameters) { parameters_ = parameters; @@ -137,6 +149,17 @@ class StartPlannerModule : public SceneModuleInterface void initializeSafetyCheckParameters(); bool requiresDynamicObjectsCollisionDetection() const; + + /** + * @brief Check if there are no moving objects around within a certain radius. + * + * This function filters the dynamic objects within a certain radius and then filters them by + * their velocity. If there are no moving objects around, it returns true. Otherwise, it returns + * false. + * + * @return True if there are no moving objects around. False otherwise. + */ + bool noMovingObjectsAround() const; bool receivedNewRoute() const; bool isModuleRunning() const; @@ -233,4 +256,4 @@ class StartPlannerModule : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__START_PLANNER_MODULE_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp similarity index 86% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp index 46b72cacac651..783aace0983ca 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp @@ -13,10 +13,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ -#include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -93,9 +93,15 @@ struct StartPlannerParameters utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params{}; utils::path_safety_checker::SafetyCheckParams safety_check_params{}; + // surround moving obstacle check + double search_radius{0.0}; + double th_moving_obstacle_velocity{0.0}; + behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck + surround_moving_obstacles_type_to_check{}; + bool print_debug_info{false}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp similarity index 81% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp index 5fcb5fd5668b1..147632329d926 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" @@ -50,13 +50,6 @@ lanelet::ConstLanelets getPullOutLanes( const std::shared_ptr & planner_data, const double backward_length); Pose getBackedPose( const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance); -/** - * @brief calculate end arc length to generate reference path considering the goal position - * @return a pair of s_end and terminal_is_goal - */ -std::pair calcEndArcLength( - const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, - const Pose & goal_pose); } // namespace behavior_path_planner::start_planner_utils -#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_start_planner_module/package.xml b/planning/behavior_path_start_planner_module/package.xml new file mode 100644 index 0000000000000..cbfdba0d07a57 --- /dev/null +++ b/planning/behavior_path_start_planner_module/package.xml @@ -0,0 +1,38 @@ + + + + behavior_path_start_planner_module + 0.1.0 + The behavior_path_start_planner_module package + + Kosuke Takeuchi + Kyoichi Sugahara + Satoshi OTA + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + behavior_path_planner + behavior_path_planner_common + motion_utils + pluginlib + rclcpp + rtc_interface + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_start_planner_module/plugins.xml b/planning/behavior_path_start_planner_module/plugins.xml new file mode 100644 index 0000000000000..fde8b673be5bc --- /dev/null +++ b/planning/behavior_path_start_planner_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp similarity index 93% rename from planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp rename to planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp index 32a36e1d70509..dd068c3c26cf8 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" +#include "behavior_path_start_planner_module/freespace_pull_out.hpp" -#include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_start_planner_module/util.hpp" #include @@ -90,8 +91,8 @@ std::optional FreespacePullOut::plan(const Pose & start_pose, const constexpr double offset_from_end_pose = 1.0; const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose); const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0); - const auto path_end_info = - start_planner_utils::calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + const auto path_end_info = behavior_path_planner::utils::parking_departure::calcEndArcLength( + s_start, forward_path_length, road_lanes, goal_pose); const double s_end = path_end_info.first; const bool path_terminal_is_goal = path_end_info.second; diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp similarity index 97% rename from planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp rename to planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp index fefa6b440c096..7d6dd60398e8e 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" +#include "behavior_path_start_planner_module/geometric_pull_out.hpp" -#include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_start_planner_module/util.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp similarity index 91% rename from planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp rename to planning/behavior_path_start_planner_module/src/manager.cpp index e379fcfa03a80..58d0fbee7921b 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/start_planner/manager.hpp" +#include "behavior_path_start_planner_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -273,6 +273,35 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); } + // surround moving obstacle check + std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; + { + p.search_radius = + node->declare_parameter(surround_moving_obstacle_check_ns + "search_radius"); + p.th_moving_obstacle_velocity = node->declare_parameter( + surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity"); + // ObjectTypesToCheck + std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; + { + p.surround_moving_obstacles_type_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.surround_moving_obstacles_type_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.surround_moving_obstacles_type_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.surround_moving_obstacles_type_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.surround_moving_obstacles_type_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.surround_moving_obstacles_type_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.surround_moving_obstacles_type_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.surround_moving_obstacles_type_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + } + // debug std::string debug_ns = ns + "debug."; { diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp similarity index 97% rename from planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp rename to planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index dd1d3d3c4cca4..73952ca02f558 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" +#include "behavior_path_start_planner_module/shift_pull_out.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" -#include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" @@ -187,8 +187,8 @@ std::vector ShiftPullOut::calcPullOutPaths( // generate road lane reference path const auto arc_position_start = getArcCoordinates(road_lanes, start_pose); const double s_start = std::max(arc_position_start.length - backward_path_length, 0.0); - const auto path_end_info = - start_planner_utils::calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + const auto path_end_info = behavior_path_planner::utils::parking_departure::calcEndArcLength( + s_start, forward_path_length, road_lanes, goal_pose); const double s_end = path_end_info.first; const bool path_terminal_is_goal = path_end_info.second; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp similarity index 94% rename from planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp rename to planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 7bbee7fe0a10c..0d3892cde8023 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/start_planner/start_planner_module.hpp" +#include "behavior_path_start_planner_module/start_planner_module.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" -#include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -36,7 +36,7 @@ #include #include -using behavior_path_planner::utils::start_goal_planner_common::initializeCollisionCheckDebugMap; +using behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; @@ -130,6 +130,30 @@ void StartPlannerModule::initVariables() initializeCollisionCheckDebugMap(start_planner_data_.collision_check); } +void StartPlannerModule::updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params) +{ + ego_predicted_path_params = + std::make_shared(start_planner_params->ego_predicted_path_params); +} + +void StartPlannerModule::updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params) +{ + safety_check_params = + std::make_shared(start_planner_params->safety_check_params); +} + +void StartPlannerModule::updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params) +{ + objects_filtering_params = + std::make_shared(start_planner_params->objects_filtering_params); +} + void StartPlannerModule::updateData() { if (receivedNewRoute()) { @@ -146,6 +170,8 @@ void StartPlannerModule::updateData() if (requiresDynamicObjectsCollisionDetection()) { status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects(); + } else { + status_.is_safe_dynamic_objects = true; } } @@ -179,6 +205,21 @@ bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const return parameters_->safety_check_params.enable_safety_check && status_.driving_forward; } +bool StartPlannerModule::noMovingObjectsAround() const +{ + auto dynamic_objects = *(planner_data_->dynamic_object); + utils::path_safety_checker::filterObjectsWithinRadius( + dynamic_objects, planner_data_->self_odometry->pose.pose.position, parameters_->search_radius); + utils::path_safety_checker::filterObjectsByClass( + dynamic_objects, parameters_->surround_moving_obstacles_type_to_check); + const auto filtered_objects = utils::path_safety_checker::filterObjectsByVelocity( + dynamic_objects, parameters_->th_moving_obstacle_velocity, false); + if (!filtered_objects.objects.empty()) { + DEBUG_PRINT("Moving objects exist in the safety check area"); + } + return filtered_objects.objects.empty(); +} + bool StartPlannerModule::hasCollisionWithDynamicObjects() const { // TODO(Sugahara): update path, params for predicted path and so on in this function to avoid @@ -271,20 +312,24 @@ bool StartPlannerModule::isStopped() bool StartPlannerModule::isExecutionReady() const { bool is_safe = true; - // Evaluate safety. The situation is not safe if any of the following conditions are met: // 1. pull out path has not been found - // 2. waiting for approval and there is a collision with dynamic objects + // 2. there is a moving objects around ego + // 3. waiting for approval and there is a collision with dynamic objects if (!status_.found_pull_out_path) { is_safe = false; + } else if (isWaitingApproval()) { + if (!noMovingObjectsAround()) { + is_safe = false; + } else if (requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects()) { + is_safe = false; + } } - if (requiresDynamicObjectsCollisionDetection()) { - is_safe = !hasCollisionWithDynamicObjects(); + if (!is_safe) { + stop_pose_ = planner_data_->self_odometry->pose.pose; } - if (!is_safe) stop_pose_ = planner_data_->self_odometry->pose.pose; - return is_safe; } @@ -329,7 +374,7 @@ BehaviorModuleOutput StartPlannerModule::plan() if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.has_stop_point) { auto current_path = getCurrentPath(); const auto stop_path = - behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( + behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop); @@ -382,7 +427,6 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); - // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction, @@ -392,7 +436,6 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); - // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); @@ -410,11 +453,9 @@ CandidateOutput StartPlannerModule::planCandidate() const void StartPlannerModule::initializeSafetyCheckParameters() { - utils::start_goal_planner_common::updateEgoPredictedPathParams( - ego_predicted_path_params_, parameters_); - utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); - utils::start_goal_planner_common::updateObjectsFilteringParams( - objects_filtering_params_, parameters_); + updateEgoPredictedPathParams(ego_predicted_path_params_, parameters_); + updateSafetyCheckParams(safety_check_params_, parameters_); + updateObjectsFilteringParams(objects_filtering_params_, parameters_); } PathWithLaneId StartPlannerModule::getFullPath() const @@ -1074,13 +1115,13 @@ bool StartPlannerModule::isSafePath() const // for ego predicted path const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_out_path.points); const std::pair terminal_velocity_and_accel = - utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + utils::parking_departure::getPairsTerminalVelocityAndAccel( status_.pull_out_path.pairs_terminal_velocity_and_accel, status_.current_path_idx); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for start_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); - utils::start_goal_planner_common::updatePathProperty( + utils::parking_departure::updatePathProperty( ego_predicted_path_params_, terminal_velocity_and_accel); // TODO(Sugahara): shoule judge is_object_front properly const bool is_object_front = true; @@ -1101,7 +1142,7 @@ bool StartPlannerModule::isSafePath() const const double hysteresis_factor = status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; - utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( + utils::parking_departure::updateSafetyCheckTargetObjectsData( start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( diff --git a/planning/behavior_path_planner/src/utils/start_planner/util.cpp b/planning/behavior_path_start_planner_module/src/util.cpp similarity index 83% rename from planning/behavior_path_planner/src/utils/start_planner/util.cpp rename to planning/behavior_path_start_planner_module/src/util.cpp index 0b26c793155f7..9a9466936c422 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_start_planner_module/src/util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_start_planner_module/util.hpp" #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -106,25 +106,4 @@ lanelet::ConstLanelets getPullOutLanes( /*forward_only_in_route*/ true); } -std::pair calcEndArcLength( - const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, - const Pose & goal_pose) -{ - const double s_forward_length = s_start + forward_path_length; - // use forward length if the goal pose is not in the lanelets - if (!utils::isInLanelets(goal_pose, road_lanes)) { - return {s_forward_length, false}; - } - - const double s_goal = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; - - // If the goal is behind the start or beyond the forward length, use forward length. - if (s_goal < s_start || s_goal >= s_forward_length) { - return {s_forward_length, false}; - } - - // path end is goal - return {s_goal, true}; -} - } // namespace behavior_path_planner::start_planner_utils diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp index ec8a89c5a3e9c..35e8ab53dd75d 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp @@ -15,20 +15,15 @@ #ifndef MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ #define MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "geometry_msgs/msg/pose.hpp" +#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" +#include "geometry_msgs/msg/detail/pose__struct.hpp" -#include "boost/optional.hpp" - -#include #include -#include +#include #include #include -namespace motion_velocity_smoother -{ -namespace trajectory_utils +namespace motion_velocity_smoother::trajectory_utils { using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; @@ -57,7 +52,7 @@ double getMaxAbsVelocity(const TrajectoryPoints & trajectory); void applyMaximumVelocityLimit( const size_t from, const size_t to, const double max_vel, TrajectoryPoints & trajectory); -boost::optional searchZeroVelocityIdx(const TrajectoryPoints & trajectory); +std::optional searchZeroVelocityIdx(const TrajectoryPoints & trajectory); bool calcStopDistWithJerkConstraints( const double v0, const double a0, const double jerk_acc, const double jerk_dec, @@ -68,12 +63,12 @@ bool isValidStopDist( const double v_end, const double a_end, const double v_target, const double a_target, const double v_margin, const double a_margin); -boost::optional applyDecelFilterWithJerkConstraint( +std::optional applyDecelFilterWithJerkConstraint( const TrajectoryPoints & input, const size_t start_index, const double v0, const double a0, const double min_acc, const double decel_target_vel, const std::map & jerk_profile); -boost::optional> updateStateWithJerkConstraint( +std::optional> updateStateWithJerkConstraint( const double v0, const double a0, const std::map & jerk_profile, const double t); std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( @@ -82,7 +77,6 @@ std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest); -} // namespace trajectory_utils -} // namespace motion_velocity_smoother +} // namespace motion_velocity_smoother::trajectory_utils #endif // MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml index 54c60c938970a..4bb0d8a2883dd 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -22,7 +22,6 @@ autoware_auto_planning_msgs geometry_msgs interpolation - libboost-dev motion_utils nav_msgs osqp_interface diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index dd361696a5dad..acc3673d66925 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -15,7 +15,6 @@ #include "motion_velocity_smoother/trajectory_utils.hpp" #include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -430,7 +429,7 @@ bool isValidStopDist( return true; } -boost::optional applyDecelFilterWithJerkConstraint( +std::optional applyDecelFilterWithJerkConstraint( const TrajectoryPoints & input, const size_t start_index, const double v0, const double a0, const double min_acc, const double decel_target_vel, const std::map & jerk_profile) @@ -536,7 +535,7 @@ boost::optional applyDecelFilterWithJerkConstraint( return output_trajectory; } -boost::optional> updateStateWithJerkConstraint( +std::optional> updateStateWithJerkConstraint( const double v0, const double a0, const std::map & jerk_profile, const double t) { // constexpr double eps = 1.0E-05; @@ -558,15 +557,14 @@ boost::optional> updateStateWithJerkC x = integ_x(x, v, a, j, dt); v = integ_v(v, a, j, dt); a = integ_a(a, j, dt); - const auto state = std::make_tuple(x, v, a, j); - return boost::optional>(state); + return std::make_tuple(x, v, a, j); } } RCLCPP_WARN_STREAM( rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), "Invalid jerk profile"); - return {}; + return std::nullopt; } std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit(