diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 57d0280330641..51f0a32284626 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -7,9 +7,9 @@ - + - + @@ -42,13 +42,13 @@ - + - + diff --git a/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt similarity index 89% rename from planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt index 98874b8923324..e7df28307e754 100644 --- a/planning/behavior_path_dynamic_avoidance_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_path_dynamic_avoidance_module) +project(autoware_behavior_path_dynamic_obstacle_avoidance_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_path_dynamic_avoidance_module/README.md b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md similarity index 99% rename from planning/behavior_path_dynamic_avoidance_module/README.md rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md index b8af767afd9a5..e0fec11d166a6 100644 --- a/planning/behavior_path_dynamic_avoidance_module/README.md +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md @@ -1,4 +1,4 @@ -# Dynamic avoidance design +# Avoidance module for dynamic objects This module is under development. diff --git a/planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/config/dynamic_obstacle_avoidance.param.yaml similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/config/dynamic_avoidance.param.yaml rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/config/dynamic_obstacle_avoidance.param.yaml diff --git a/planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-13-01.png b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-13-01.png similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-13-01.png rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-13-01.png diff --git a/planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-32-03.png b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-32-03.png similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/2024-04-18_15-32-03.png rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/2024-04-18_15-32-03.png diff --git a/planning/behavior_path_dynamic_avoidance_module/image/drivable_area_extraction_width.drawio.svg b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/drivable_area_extraction_width.drawio.svg similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/drivable_area_extraction_width.drawio.svg rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/drivable_area_extraction_width.drawio.svg diff --git a/planning/behavior_path_dynamic_avoidance_module/image/image-20230807-151945.png b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-151945.png similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/image-20230807-151945.png rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-151945.png diff --git a/planning/behavior_path_dynamic_avoidance_module/image/image-20230807-152835.png b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-152835.png similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/image-20230807-152835.png rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230807-152835.png diff --git a/planning/behavior_path_dynamic_avoidance_module/image/image-20230808-095936.png b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-095936.png similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/image-20230808-095936.png rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-095936.png diff --git a/planning/behavior_path_dynamic_avoidance_module/image/image-20230808-152853.png b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-152853.png similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/image-20230808-152853.png rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/image-20230808-152853.png diff --git a/planning/behavior_path_dynamic_avoidance_module/image/opposite_directional_object.svg b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/opposite_directional_object.svg similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/opposite_directional_object.svg rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/opposite_directional_object.svg diff --git a/planning/behavior_path_dynamic_avoidance_module/image/same_directional_object.svg b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/same_directional_object.svg similarity index 100% rename from planning/behavior_path_dynamic_avoidance_module/image/same_directional_object.svg rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/image/same_directional_object.svg diff --git a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp similarity index 69% rename from planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp index c2c17c4e55c1e..caf0544b0852e 100644 --- a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/manager.hpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ -#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ -#include "behavior_path_dynamic_avoidance_module/scene.hpp" +#include "autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -28,16 +28,19 @@ namespace behavior_path_planner { -class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface +class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { public: - DynamicAvoidanceModuleManager() : SceneModuleManagerInterface{"dynamic_avoidance"} {} + DynamicObstacleAvoidanceModuleManager() + : SceneModuleManagerInterface{"dynamic_obstacle_avoidance"} + { + } void init(rclcpp::Node * node) override; std::unique_ptr createNewSceneModuleInstance() override { - return std::make_unique( + return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, objects_of_interest_marker_interface_ptr_map_); } @@ -52,4 +55,4 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__MANAGER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp similarity index 97% rename from planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 353a5fab92032..22152911b46b2 100644 --- a/planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ -#define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -164,7 +164,7 @@ struct LatFeasiblePaths std::vector left_path; std::vector right_path; }; -class DynamicAvoidanceModule : public SceneModuleInterface +class DynamicObstacleAvoidanceModule : public SceneModuleInterface { public: struct DynamicAvoidanceObject @@ -336,7 +336,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::string reason{""}; }; - DynamicAvoidanceModule( + DynamicObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, @@ -444,8 +444,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface getLogger(), parameters_->enable_debug_info, reason_text.c_str(), obj_uuid.c_str()); } - std::vector target_objects_; - // std::vector prev_target_objects_; + std::vector target_objects_; + // std::vector prev_target_objects_; std::optional> prev_input_ref_path_points_{std::nullopt}; std::optional> ref_path_before_lane_change_{std::nullopt}; std::shared_ptr parameters_; @@ -458,4 +458,4 @@ class DynamicAvoidanceModule : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_dynamic_avoidance_module/package.xml b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml similarity index 89% rename from planning/behavior_path_dynamic_avoidance_module/package.xml rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index 11792a15b2cd2..9b64069eddafd 100644 --- a/planning/behavior_path_dynamic_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -1,9 +1,9 @@ - behavior_path_dynamic_avoidance_module + autoware_behavior_path_dynamic_obstacle_avoidance_module 0.1.0 - The behavior_path_dynamic_avoidance_module package + The autoware_behavior_path_dynamic_obstacle_avoidance_module package Takayuki Murooka Yuki Takagi diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml new file mode 100644 index 0000000000000..3827012b12e22 --- /dev/null +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp similarity index 97% rename from planning/behavior_path_dynamic_avoidance_module/src/manager.cpp rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp index baa1c631df8da..de524a3b63de9 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/manager.cpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_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_dynamic_avoidance_module/manager.hpp" +#include "autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -37,7 +37,7 @@ PolygonGenerationMethod convertToPolygonGenerationMethod(const std::string & str } } // namespace -void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) +void DynamicObstacleAvoidanceModuleManager::init(rclcpp::Node * node) { // init manager interface initInterface(node, {""}); @@ -149,7 +149,7 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) parameters_ = std::make_shared(p); } -void DynamicAvoidanceModuleManager::updateModuleParams( +void DynamicObstacleAvoidanceModuleManager::updateModuleParams( [[maybe_unused]] const std::vector & parameters) { using tier4_autoware_utils::updateParam; @@ -287,7 +287,7 @@ void DynamicAvoidanceModuleManager::updateModuleParams( }); } -bool DynamicAvoidanceModuleManager::isAlwaysExecutableModule() const +bool DynamicObstacleAvoidanceModuleManager::isAlwaysExecutableModule() const { return true; } @@ -295,5 +295,5 @@ bool DynamicAvoidanceModuleManager::isAlwaysExecutableModule() const #include PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::DynamicAvoidanceModuleManager, + behavior_path_planner::DynamicObstacleAvoidanceModuleManager, behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp similarity index 96% rename from planning/behavior_path_dynamic_avoidance_module/src/scene.cpp rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index a5368732ef3bb..efc56c4d61f5f 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_dynamic_avoidance_module/scene.hpp" +#include "autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -301,7 +301,7 @@ std::vector convertToPoints( } } // namespace -DynamicAvoidanceModule::DynamicAvoidanceModule( +DynamicObstacleAvoidanceModule::DynamicObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, @@ -315,7 +315,7 @@ DynamicAvoidanceModule::DynamicAvoidanceModule( { } -bool DynamicAvoidanceModule::isExecutionRequested() const +bool DynamicObstacleAvoidanceModule::isExecutionRequested() const { RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionRequested."); @@ -339,13 +339,13 @@ bool DynamicAvoidanceModule::isExecutionRequested() const return !target_objects_.empty(); } -bool DynamicAvoidanceModule::isExecutionReady() const +bool DynamicObstacleAvoidanceModule::isExecutionReady() const { RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionReady."); return true; } -void DynamicAvoidanceModule::updateData() +void DynamicObstacleAvoidanceModule::updateData() { // stop_watch_.tic(std::string(__func__)); @@ -380,12 +380,12 @@ void DynamicAvoidanceModule::updateData() // [ms]"); } -bool DynamicAvoidanceModule::canTransitSuccessState() +bool DynamicObstacleAvoidanceModule::canTransitSuccessState() { return target_objects_.empty(); } -BehaviorModuleOutput DynamicAvoidanceModule::plan() +BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan() { // stop_watch_.tic(std::string(__func__)); @@ -444,19 +444,19 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() return output; } -CandidateOutput DynamicAvoidanceModule::planCandidate() const +CandidateOutput DynamicObstacleAvoidanceModule::planCandidate() const { auto candidate_path = utils::generateCenterLinePath(planner_data_); return CandidateOutput(*candidate_path); } -BehaviorModuleOutput DynamicAvoidanceModule::planWaitingApproval() +BehaviorModuleOutput DynamicObstacleAvoidanceModule::planWaitingApproval() { BehaviorModuleOutput out = plan(); return out; } -ObjectType DynamicAvoidanceModule::getObjectType(const uint8_t label) const +ObjectType DynamicObstacleAvoidanceModule::getObjectType(const uint8_t label) const { using autoware_auto_perception_msgs::msg::ObjectClassification; @@ -487,7 +487,7 @@ ObjectType DynamicAvoidanceModule::getObjectType(const uint8_t label) const return ObjectType::OUT_OF_SCOPE; } -void DynamicAvoidanceModule::registerRegulatedObjects( +void DynamicObstacleAvoidanceModule::registerRegulatedObjects( const std::vector & prev_objects) { const auto input_path = getPreviousModuleOutput().path; @@ -575,7 +575,7 @@ void DynamicAvoidanceModule::registerRegulatedObjects( } } -void DynamicAvoidanceModule::registerUnregulatedObjects( +void DynamicObstacleAvoidanceModule::registerUnregulatedObjects( const std::vector & prev_objects) { const auto input_path = getPreviousModuleOutput().path; @@ -649,7 +649,7 @@ void DynamicAvoidanceModule::registerUnregulatedObjects( } } -void DynamicAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObjects( +void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObjects( const std::vector & prev_objects) { const auto & input_path = getPreviousModuleOutput().path; @@ -809,7 +809,7 @@ void DynamicAvoidanceModule::determineWhetherToAvoidAgainstRegulatedObjects( // prev_input_ref_path_points_ = input_ref_path_points; } -void DynamicAvoidanceModule::determineWhetherToAvoidAgainstUnregulatedObjects( +void DynamicObstacleAvoidanceModule::determineWhetherToAvoidAgainstUnregulatedObjects( const std::vector & prev_objects) { const auto & input_path = getPreviousModuleOutput().path; @@ -870,7 +870,7 @@ void DynamicAvoidanceModule::determineWhetherToAvoidAgainstUnregulatedObjects( } } -LatFeasiblePaths DynamicAvoidanceModule::generateLateralFeasiblePaths( +LatFeasiblePaths DynamicObstacleAvoidanceModule::generateLateralFeasiblePaths( const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const { const double lat_acc = parameters_->max_ego_lat_acc; @@ -907,7 +907,7 @@ LatFeasiblePaths DynamicAvoidanceModule::generateLateralFeasiblePaths( return ego_lat_feasible_paths; } -[[maybe_unused]] void DynamicAvoidanceModule::updateRefPathBeforeLaneChange( +[[maybe_unused]] void DynamicObstacleAvoidanceModule::updateRefPathBeforeLaneChange( const std::vector & ego_ref_path_points) { if (ref_path_before_lane_change_) { @@ -934,7 +934,7 @@ LatFeasiblePaths DynamicAvoidanceModule::generateLateralFeasiblePaths( } [[maybe_unused]] std::optional> -DynamicAvoidanceModule::calcCollisionSection( +DynamicObstacleAvoidanceModule::calcCollisionSection( const std::vector & ego_path, const PredictedPath & obj_path) const { const size_t ego_idx = planner_data_->findEgoIndex(ego_path); @@ -971,7 +971,7 @@ DynamicAvoidanceModule::calcCollisionSection( return std::make_pair(*collision_start_idx, ego_path.size() - 1); } -TimeWhileCollision DynamicAvoidanceModule::calcTimeWhileCollision( +TimeWhileCollision DynamicObstacleAvoidanceModule::calcTimeWhileCollision( const std::vector & ego_path, const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const { @@ -1027,7 +1027,7 @@ TimeWhileCollision DynamicAvoidanceModule::calcTimeWhileCollision( return TimeWhileCollision{time_to_start_collision, time_to_end_collision}; } -bool DynamicAvoidanceModule::isObjectFarFromPath( +bool DynamicObstacleAvoidanceModule::isObjectFarFromPath( const PredictedObject & predicted_object, const double obj_dist_to_path) const { const double obj_max_length = calcObstacleMaxLength(predicted_object.shape); @@ -1037,7 +1037,7 @@ bool DynamicAvoidanceModule::isObjectFarFromPath( return parameters_->max_obj_lat_offset_to_ego_path < min_obj_dist_to_path; } -bool DynamicAvoidanceModule::willObjectCutIn( +bool DynamicObstacleAvoidanceModule::willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const { @@ -1079,7 +1079,7 @@ bool DynamicAvoidanceModule::willObjectCutIn( return true; } -DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCutOut( +DynamicObstacleAvoidanceModule::DecisionWithReason DynamicObstacleAvoidanceModule::willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const { @@ -1120,7 +1120,7 @@ DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCut return DecisionWithReason{false}; } -[[maybe_unused]] bool DynamicAvoidanceModule::willObjectBeOutsideEgoChangingPath( +[[maybe_unused]] bool DynamicObstacleAvoidanceModule::willObjectBeOutsideEgoChangingPath( const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const { @@ -1139,7 +1139,8 @@ DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCut return true; } -std::pair DynamicAvoidanceModule::getAdjacentLanes( +std::pair +DynamicObstacleAvoidanceModule::getAdjacentLanes( const double forward_distance, const double backward_distance) const { const auto & rh = planner_data_->route_handler; @@ -1179,7 +1180,8 @@ std::pair DynamicAvoidanceModule return std::make_pair(right_lanes, left_lanes); } -DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudinalOffset( +DynamicObstacleAvoidanceModule::LatLonOffset +DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) const { @@ -1213,7 +1215,7 @@ DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudi obj_lon_min_max_offset.max_value, obj_lon_min_max_offset.min_value}; } -MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( +MinMaxValue DynamicObstacleAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, @@ -1286,7 +1288,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( obj_lon_offset.max_value + end_length_to_avoid}; } -double DynamicAvoidanceModule::calcValidLengthToAvoid( +double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape, const bool is_object_same_direction) const @@ -1379,7 +1381,8 @@ double DynamicAvoidanceModule::calcValidLengthToAvoid( } // min value denotes near side, max value denotes far side -std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject( +std::optional +DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidRegulatedObject( const std::vector & ref_path_points_for_obj_poly, const Polygon2d & obj_points, const geometry_msgs::msg::Point & obj_pos, const double obj_vel, const bool is_collision_left, const double obj_normal_vel, @@ -1473,7 +1476,8 @@ std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoi } // min value denotes near side, max value denotes far side -std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( +std::optional +DynamicObstacleAvoidanceModule::calcMinMaxLateralOffsetToAvoidUnregulatedObject( const std::vector & ref_path_points_for_obj_poly, const std::optional & prev_object, const DynamicAvoidanceObject & object) const @@ -1557,7 +1561,7 @@ std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoi // NOTE: object does not have const only to update min_bound_lat_offset. std::optional -DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( +DynamicObstacleAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { if (!object.lon_offset_to_avoid || !object.lat_offset_to_avoid) { @@ -1663,7 +1667,7 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( // should be replace by the function calcPredictedPathBasedDynamicObstaclePolygon() (takagi) std::optional -DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( +DynamicObstacleAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object) const { const auto obj_path = *std::max_element( @@ -1722,7 +1726,7 @@ DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( // except for the area required for ego safety. // input: an object, the minimum area required for ego safety, and some global params std::optional -DynamicAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( +DynamicObstacleAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const { std::vector obj_poses; @@ -1783,8 +1787,8 @@ DynamicAvoidanceModule::calcPredictedPathBasedDynamicObstaclePolygon( // Calculate the driving area required to ensure the safety of the own vehicle. // It is assumed that this area will not be clipped. // input: ego's reference path, ego's pose, ego's speed, and some global params -DynamicAvoidanceModule::EgoPathReservePoly DynamicAvoidanceModule::calcEgoPathReservePoly( - const PathWithLaneId & ego_path) const +DynamicObstacleAvoidanceModule::EgoPathReservePoly +DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & ego_path) const { // This function require almost 0.5 ms. Should be refactored in the future // double calculation_time; diff --git a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp similarity index 95% rename from planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 4ae77249b5d0f..bd9e6fd483c7c 100644 --- a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -47,7 +47,7 @@ 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"); std::vector params; params.emplace_back("launch_modules", module_names); @@ -60,8 +60,9 @@ 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", - ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + - "/config/avoidance.param.yaml"}); + ament_index_cpp::get_package_share_directory( + "autoware_behavior_path_dynamic_obstacle_avoidance_module") + + "/config/dynamic_obstacle_avoidance.param.yaml"}); return std::make_shared(node_options); } diff --git a/planning/behavior_path_avoidance_module/CMakeLists.txt b/planning/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt similarity index 91% rename from planning/behavior_path_avoidance_module/CMakeLists.txt rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt index c94591f9bbd30..e646ad29ed42a 100644 --- a/planning/behavior_path_avoidance_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_path_avoidance_module) +project(autoware_behavior_path_static_obstacle_avoidance_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_path_avoidance_module/README.md b/planning/autoware_behavior_path_static_obstacle_avoidance_module/README.md similarity index 98% rename from planning/behavior_path_avoidance_module/README.md rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/README.md index aa7a3a42efdb4..93b57ac3a4831 100644 --- a/planning/behavior_path_avoidance_module/README.md +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -758,9 +758,9 @@ WIP ## Parameters -The avoidance specific parameter configuration file can be located at `src/autoware/launcher/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml`. +The avoidance specific parameter configuration file can be located at `src/autoware/launcher/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml`. -{{ json_to_markdown("planning/behavior_path_avoidance_module/schema/avoidance.schema.json") }} +{{ json_to_markdown("planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json") }} ## Future extensions / Unimplemented parts @@ -797,7 +797,7 @@ Developers can see what is going on in each process by visualizing all the avoid ![fig1](./images/avoidance-debug-marker.png) -To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner avoidance.publish_debug_marker true` (no restart is needed) or simply set the `publish_debug_marker` to `true` in the `avoidance.param.yaml` for permanent effect (restart is needed). Then add the marker `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance` in `rviz2`. +To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner avoidance.publish_debug_marker true` (no restart is needed) or simply set the `publish_debug_marker` to `true` in the `static_obstacle_avoidance.param.yaml` for permanent effect (restart is needed). Then add the marker `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance` in `rviz2`. ### Echoing debug message to find out why the objects were ignored diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml similarity index 100% rename from planning/behavior_path_avoidance_module/config/avoidance.param.yaml rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml diff --git a/planning/behavior_path_avoidance_module/images/avoidance-debug-marker.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance-debug-marker.png similarity index 100% rename from planning/behavior_path_avoidance_module/images/avoidance-debug-marker.png rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance-debug-marker.png diff --git a/planning/behavior_path_avoidance_module/images/avoidance_debug_message_array.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_debug_message_array.png similarity index 100% rename from planning/behavior_path_avoidance_module/images/avoidance_debug_message_array.png rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_debug_message_array.png diff --git a/planning/behavior_path_avoidance_module/images/avoidance_design.fig1.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_design.fig1.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/avoidance_design.fig1.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_design.fig1.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png similarity index 100% rename from planning/behavior_path_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png diff --git a/planning/behavior_path_avoidance_module/images/dynamic_lateral_margin.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/dynamic_lateral_margin.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/dynamic_lateral_margin.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/dynamic_lateral_margin.svg diff --git a/planning/behavior_path_avoidance_module/images/envelope_polygon.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/envelope_polygon.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/envelope_polygon.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/envelope_polygon.svg diff --git a/planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/how_to_decide_path_shape_one_object.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/intersection_problem.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/intersection_problem.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/intersection_problem.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/intersection_problem.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/parked-car-detection.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/parked-car-detection.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/parked-car-detection.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/parked-car-detection.svg diff --git a/planning/behavior_path_avoidance_module/images/safety_check_flowchart.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_flowchart.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/safety_check_flowchart.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_flowchart.svg diff --git a/planning/behavior_path_avoidance_module/images/safety_check_margin.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_margin.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/safety_check_margin.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_margin.svg diff --git a/planning/behavior_path_avoidance_module/images/safety_check_step_1.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_1.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/safety_check_step_1.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_1.svg diff --git a/planning/behavior_path_avoidance_module/images/safety_check_step_2.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_2.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/safety_check_step_2.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_2.svg diff --git a/planning/behavior_path_avoidance_module/images/shift_length_parameters.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_length_parameters.drawio.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/shift_length_parameters.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_length_parameters.drawio.svg diff --git a/planning/behavior_path_avoidance_module/images/shift_line_generation.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_line_generation.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/shift_line_generation.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_line_generation.svg diff --git a/planning/behavior_path_avoidance_module/images/target_vehicle_selection.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_vehicle_selection.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/target_vehicle_selection.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_vehicle_selection.svg diff --git a/planning/behavior_path_avoidance_module/images/todo.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/todo.png similarity index 100% rename from planning/behavior_path_avoidance_module/images/todo.png rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/todo.png diff --git a/planning/behavior_path_avoidance_module/images/use_adjacent_lane.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_adjacent_lane.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/use_adjacent_lane.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_adjacent_lane.svg diff --git a/planning/behavior_path_avoidance_module/images/use_hatched_road_markings.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_hatched_road_markings.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/use_hatched_road_markings.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_hatched_road_markings.svg diff --git a/planning/behavior_path_avoidance_module/images/use_intersection_areas.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_intersection_areas.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/use_intersection_areas.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_intersection_areas.svg diff --git a/planning/behavior_path_avoidance_module/images/use_opposite_lane.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_opposite_lane.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/use_opposite_lane.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_opposite_lane.svg diff --git a/planning/behavior_path_avoidance_module/images/yield_limitation.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_limitation.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/yield_limitation.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_limitation.svg diff --git a/planning/behavior_path_avoidance_module/images/yield_maneuver.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/yield_maneuver.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver.svg diff --git a/planning/behavior_path_avoidance_module/images/yield_maneuver_flowchart.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver_flowchart.svg similarity index 100% rename from planning/behavior_path_avoidance_module/images/yield_maneuver_flowchart.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver_flowchart.svg diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp similarity index 98% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp index 2e432e093583f..a63c283704001 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ -#include "behavior_path_avoidance_module/type_alias.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include "behavior_path_planner_common/data_manager.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" @@ -670,4 +670,4 @@ struct DebugData } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp similarity index 79% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp index 6a4f206ddf309..85b0e1aad0ae3 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DEBUG_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DEBUG_HPP_ -#include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_avoidance_module/type_alias.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include #include -namespace behavior_path_planner::utils::avoidance +namespace behavior_path_planner::utils::static_obstacle_avoidance { using behavior_path_planner::AvoidanceParameters; @@ -50,7 +50,7 @@ MarkerArray createOtherObjectsMarkerArray( MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, const std::shared_ptr & parameters); -} // namespace behavior_path_planner::utils::avoidance +} // namespace behavior_path_planner::utils::static_obstacle_avoidance std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr); @@ -60,4 +60,4 @@ std::string toStrInfo(const behavior_path_planner::ShiftLine & sl); std::string toStrInfo(const behavior_path_planner::AvoidLine & ap); -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DEBUG_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp similarity index 95% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp index 55d14e424b7f6..5da56a320ad3b 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ -#include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_avoidance_module/type_alias.hpp" -#include "behavior_path_avoidance_module/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include @@ -26,7 +26,7 @@ #include #include -namespace behavior_path_planner::helper::avoidance +namespace behavior_path_planner::helper::static_obstacle_avoidance { using behavior_path_planner::PathShifter; @@ -200,7 +200,7 @@ class AvoidanceHelper double getShiftLength( const ObjectData & object, const bool & is_on_right, const double & margin) const { - using utils::avoidance::calcShiftLength; + using utils::static_obstacle_avoidance::calcShiftLength; const auto shift_length = calcShiftLength(is_on_right, object.overhang_points.front().first, margin); @@ -333,7 +333,7 @@ class AvoidanceHelper return true; } - const auto is_object_on_right = utils::avoidance::isOnRight(object); + const auto is_object_on_right = utils::static_obstacle_avoidance::isOnRight(object); const auto desire_shift_length = getShiftLength(object, is_object_on_right, object.avoid_margin.value()); @@ -513,6 +513,6 @@ class AvoidanceHelper std::optional> max_v_point_; }; -} // namespace behavior_path_planner::helper::avoidance +} // namespace behavior_path_planner::helper::static_obstacle_avoidance -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__HELPER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp similarity index 66% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp index e23a8a96ee7da..1616339840ba4 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_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_AVOIDANCE_MODULE__MANAGER_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ -#include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_avoidance_module/scene.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -28,16 +28,18 @@ namespace behavior_path_planner { -class AvoidanceModuleManager : public SceneModuleManagerInterface +class StaticObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { public: - AvoidanceModuleManager() : SceneModuleManagerInterface{"avoidance"} {} + StaticObstacleAvoidanceModuleManager() : SceneModuleManagerInterface{"static_obstacle_avoidance"} + { + } void init(rclcpp::Node * node) override; std::unique_ptr createNewSceneModuleInstance() override { - return std::make_unique( + return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, objects_of_interest_marker_interface_ptr_map_); } @@ -50,4 +52,4 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__MANAGER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp similarity index 98% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 461084c085ca7..56b9bad535e7d 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -11,12 +11,12 @@ // 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_MODULE__PARAMETER_HELPER_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ #include "tier4_autoware_utils/ros/parameter.hpp" -#include +#include #include #include @@ -403,4 +403,4 @@ AvoidanceParameters getParameter(rclcpp::Node * node) } } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp similarity index 94% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp index da5e29ef4d5e9..57090e9f551cf 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_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_avoidance_module/type_alias.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" @@ -36,13 +36,13 @@ namespace behavior_path_planner { -using helper::avoidance::AvoidanceHelper; +using helper::static_obstacle_avoidance::AvoidanceHelper; using tier4_planning_msgs::msg::AvoidanceDebugMsg; -class AvoidanceModule : public SceneModuleInterface +class StaticObstacleAvoidanceModule : public SceneModuleInterface { public: - AvoidanceModule( + StaticObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & @@ -424,7 +424,7 @@ class AvoidanceModule : public SceneModuleInterface std::shared_ptr parameters_; - utils::avoidance::ShiftLineGenerator generator_; + utils::static_obstacle_avoidance::ShiftLineGenerator generator_; AvoidancePlanningData avoid_data_; @@ -458,4 +458,4 @@ class AvoidanceModule : public SceneModuleInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__SCENE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp similarity index 91% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp index e721401207830..21eaef884b5e6 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ -#include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_avoidance_module/helper.hpp" -#include "behavior_path_avoidance_module/type_alias.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include -namespace behavior_path_planner::utils::avoidance +namespace behavior_path_planner::utils::static_obstacle_avoidance { using behavior_path_planner::PathShifter; -using behavior_path_planner::helper::avoidance::AvoidanceHelper; +using behavior_path_planner::helper::static_obstacle_avoidance::AvoidanceHelper; class ShiftLineGenerator { @@ -242,6 +242,6 @@ class ShiftLineGenerator double base_offset_{0.0}; }; -} // namespace behavior_path_planner::utils::avoidance +} // namespace behavior_path_planner::utils::static_obstacle_avoidance -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp similarity index 91% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp index 67bca3099df6f..afdfba30ce267 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ #include #include @@ -71,4 +71,4 @@ using tier4_autoware_utils::pose2transform; using tier4_autoware_utils::toHexString; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp similarity index 94% rename from planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp index cf68f577ed7c0..420edd17ca372 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ -#include "behavior_path_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_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" @@ -23,7 +23,7 @@ #include #include -namespace behavior_path_planner::utils::avoidance +namespace behavior_path_planner::utils::static_obstacle_avoidance { using behavior_path_planner::PlannerData; @@ -174,6 +174,6 @@ double calcDistanceToAvoidStartLine( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); -} // namespace behavior_path_planner::utils::avoidance +} // namespace behavior_path_planner::utils::static_obstacle_avoidance -#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_avoidance_module/package.xml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml similarity index 92% rename from planning/behavior_path_avoidance_module/package.xml rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index 5f0a658735b2f..3643257d243b4 100644 --- a/planning/behavior_path_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -1,9 +1,9 @@ - behavior_path_avoidance_module + autoware_behavior_path_static_obstacle_avoidance_module 0.1.0 - The behavior_path_avoidance_module package + The autoware_behavior_path_static_obstacle_avoidance_module package Takamasa Horibe Zulfaqar Azmi diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml new file mode 100644 index 0000000000000..a4e1da9d5dccb --- /dev/null +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json b/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json similarity index 100% rename from planning/behavior_path_avoidance_module/schema/avoidance.schema.json rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp similarity index 99% rename from planning/behavior_path_avoidance_module/src/debug.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index 4d3b4605ac956..b8e6bb99fbebb 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/autoware_behavior_path_static_obstacle_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_avoidance_module/debug.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp" #include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" @@ -23,7 +23,7 @@ #include #include -namespace behavior_path_planner::utils::avoidance +namespace behavior_path_planner::utils::static_obstacle_avoidance { namespace { @@ -653,7 +653,7 @@ MarkerArray createDebugMarkerArray( return msg; } -} // namespace behavior_path_planner::utils::avoidance +} // namespace behavior_path_planner::utils::static_obstacle_avoidance std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr) { diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp similarity index 96% rename from planning/behavior_path_avoidance_module/src/manager.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index bdf6f4e822f7b..bfc95e0730cd6 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_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_avoidance_module/manager.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp" -#include "behavior_path_avoidance_module/parameter_helper.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -26,7 +26,7 @@ namespace behavior_path_planner { -void AvoidanceModuleManager::init(rclcpp::Node * node) +void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) { using autoware_auto_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::getOrDeclareParameter; @@ -39,7 +39,8 @@ void AvoidanceModuleManager::init(rclcpp::Node * node) parameters_ = std::make_shared(p); } -void AvoidanceModuleManager::updateModuleParams(const std::vector & parameters) +void StaticObstacleAvoidanceModuleManager::updateModuleParams( + const std::vector & parameters) { using autoware_auto_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::updateParam; @@ -282,4 +283,5 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::AvoidanceModuleManager, behavior_path_planner::SceneModuleManagerInterface) + behavior_path_planner::StaticObstacleAvoidanceModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp similarity index 89% rename from planning/behavior_path_avoidance_module/src/scene.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 9247d9b3d200a..0a6d490f7fbf1 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_avoidance_module/scene.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp" -#include "behavior_path_avoidance_module/debug.hpp" -#include "behavior_path_avoidance_module/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -71,7 +71,7 @@ lanelet::BasicLineString3d toLineString3d(const std::vector & bound) } } // namespace -AvoidanceModule::AvoidanceModule( +StaticObstacleAvoidanceModule::StaticObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & @@ -83,7 +83,7 @@ AvoidanceModule::AvoidanceModule( { } -bool AvoidanceModule::isExecutionRequested() const +bool StaticObstacleAvoidanceModule::isExecutionRequested() const { RCLCPP_DEBUG(getLogger(), "AVOIDANCE isExecutionRequested"); @@ -105,7 +105,7 @@ bool AvoidanceModule::isExecutionRequested() const [this](const auto & o) { return !helper_->isAbsolutelyNotAvoidable(o); }); } -bool AvoidanceModule::isExecutionReady() const +bool StaticObstacleAvoidanceModule::isExecutionReady() const { RCLCPP_DEBUG_STREAM(getLogger(), "---Avoidance GO/NO-GO status---"); RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "SAFE:" << avoid_data_.safe); @@ -115,7 +115,8 @@ bool AvoidanceModule::isExecutionReady() const return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready; } -AvoidanceState AvoidanceModule::getCurrentModuleState(const AvoidancePlanningData & data) const +AvoidanceState StaticObstacleAvoidanceModule::getCurrentModuleState( + const AvoidancePlanningData & data) const { const bool has_avoidance_target = std::any_of( data.target_objects.begin(), data.target_objects.end(), @@ -159,7 +160,7 @@ AvoidanceState AvoidanceModule::getCurrentModuleState(const AvoidancePlanningDat return AvoidanceState::RUNNING; } -bool AvoidanceModule::canTransitSuccessState() +bool StaticObstacleAvoidanceModule::canTransitSuccessState() { const auto & data = avoid_data_; @@ -190,22 +191,23 @@ bool AvoidanceModule::canTransitSuccessState() return data.state == AvoidanceState::CANCEL || data.state == AvoidanceState::SUCCEEDED; } -void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugData & debug) +void StaticObstacleAvoidanceModule::fillFundamentalData( + AvoidancePlanningData & data, DebugData & debug) { // reference pose data.reference_pose = utils::getUnshiftedEgoPose(getEgoPose(), helper_->getPreviousSplineShiftPath()); // lanelet info - data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( + data.current_lanelets = utils::static_obstacle_avoidance::getCurrentLanesFromPath( getPreviousModuleOutput().reference_path, planner_data_); - data.extend_lanelets = - utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_); + data.extend_lanelets = utils::static_obstacle_avoidance::getExtendLanes( + data.current_lanelets, getEgoPose(), planner_data_); // expand drivable lanes const auto is_within_current_lane = - utils::avoidance::isWithinLanes(data.current_lanelets, planner_data_); + utils::static_obstacle_avoidance::isWithinLanes(data.current_lanelets, planner_data_); const auto red_signal_lane_itr = std::find_if( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { const auto next_lanes = planner_data_->route_handler->getNextLanelets(lanelet); @@ -218,12 +220,15 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { if (!not_use_adjacent_lane) { data.drivable_lanes.push_back( - utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + utils::static_obstacle_avoidance::generateExpandedDrivableLanes( + lanelet, planner_data_, parameters_)); } else if (red_signal_lane_itr->id() != lanelet.id()) { data.drivable_lanes.push_back( - utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + utils::static_obstacle_avoidance::generateExpandedDrivableLanes( + lanelet, planner_data_, parameters_)); } else { - data.drivable_lanes.push_back(utils::avoidance::generateNotExpandedDrivableLanes(lanelet)); + data.drivable_lanes.push_back( + utils::static_obstacle_avoidance::generateNotExpandedDrivableLanes(lanelet)); } }); @@ -271,18 +276,19 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat data.reference_path, 0, data.reference_path.points.size(), motion_utils::calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); - data.to_return_point = utils::avoidance::calcDistanceToReturnDeadLine( + data.to_return_point = utils::static_obstacle_avoidance::calcDistanceToReturnDeadLine( data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); - data.to_start_point = utils::avoidance::calcDistanceToAvoidStartLine( + data.to_start_point = utils::static_obstacle_avoidance::calcDistanceToAvoidStartLine( data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); // target objects for avoidance fillAvoidanceTargetObjects(data, debug); // lost object compensation - utils::avoidance::updateRegisteredObject(registered_objects_, data.target_objects, parameters_); - utils::avoidance::compensateDetectionLost( + utils::static_obstacle_avoidance::updateRegisteredObject( + registered_objects_, data.target_objects, parameters_); + utils::static_obstacle_avoidance::compensateDetectionLost( registered_objects_, data.target_objects, data.other_objects); // sort object order by longitudinal distance @@ -294,14 +300,14 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat path_shifter_.setPath(data.reference_path); } -void AvoidanceModule::fillAvoidanceTargetObjects( +void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { - using utils::avoidance::fillAvoidanceNecessity; - using utils::avoidance::fillObjectStoppableJudge; - using utils::avoidance::filterTargetObjects; - using utils::avoidance::separateObjectsByPath; - using utils::avoidance::updateRoadShoulderDistance; + using utils::static_obstacle_avoidance::fillAvoidanceNecessity; + using utils::static_obstacle_avoidance::fillObjectStoppableJudge; + using utils::static_obstacle_avoidance::filterTargetObjects; + using utils::static_obstacle_avoidance::separateObjectsByPath; + using utils::static_obstacle_avoidance::updateRoadShoulderDistance; using utils::traffic_light::calcDistanceToRedTrafficLight; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. @@ -360,7 +366,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( } } -ObjectData AvoidanceModule::createObjectData( +ObjectData StaticObstacleAvoidanceModule::createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const { using boost::geometry::return_centroid; @@ -384,14 +390,15 @@ ObjectData AvoidanceModule::createObjectData( object_data.distance_factor = object_parameter.max_expand_ratio * clamp + 1.0; // Calc envelop polygon. - utils::avoidance::fillObjectEnvelopePolygon( + utils::static_obstacle_avoidance::fillObjectEnvelopePolygon( object_data, registered_objects_, object_closest_pose, parameters_); // calc object centroid. object_data.centroid = return_centroid(object_data.envelope_poly); // Calc moving time. - utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_); + utils::static_obstacle_avoidance::fillObjectMovingTime( + object_data, stopped_objects_, parameters_); // Calc lateral deviation from path to target object. object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 @@ -401,7 +408,7 @@ ObjectData AvoidanceModule::createObjectData( return object_data; } -bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const +bool StaticObstacleAvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const { // transit yield maneuver only when the avoidance maneuver is not initiated. if (helper_->isShifted()) { @@ -453,7 +460,8 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const return true; } -void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const +void StaticObstacleAvoidanceModule::fillShiftLine( + AvoidancePlanningData & data, DebugData & debug) const { auto path_shifter = path_shifter_; @@ -485,12 +493,13 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de /** * STEP4: Generate avoidance path. */ - ShiftedPath spline_shift_path = utils::avoidance::toShiftedPath(data.reference_path); + ShiftedPath spline_shift_path = + utils::static_obstacle_avoidance::toShiftedPath(data.reference_path); const auto success_spline_path_generation = path_shifter.generate(&spline_shift_path, true, SHIFT_TYPE::SPLINE); data.candidate_path = success_spline_path_generation ? spline_shift_path - : utils::avoidance::toShiftedPath(data.reference_path); + : utils::static_obstacle_avoidance::toShiftedPath(data.reference_path); /** * STEP5: Check avoidance path safety. @@ -503,7 +512,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de helper_->isReady(data.target_objects); } -void AvoidanceModule::fillEgoStatus( +void StaticObstacleAvoidanceModule::fillEgoStatus( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { data.state = getCurrentModuleState(data); @@ -597,7 +606,7 @@ void AvoidanceModule::fillEgoStatus( } } -void AvoidanceModule::fillDebugData( +void StaticObstacleAvoidanceModule::fillDebugData( const AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { if (!data.stop_target_object) { @@ -624,8 +633,8 @@ void AvoidanceModule::fillDebugData( const auto max_avoid_margin = lateral_hard_margin * o_front.distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; - const auto avoidance_distance = helper_->getSharpAvoidanceDistance( - helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); + const auto avoidance_distance = helper_->getSharpAvoidanceDistance(helper_->getShiftLength( + o_front, utils::static_obstacle_avoidance::isOnRight(o_front), max_avoid_margin)); const auto prepare_distance = helper_->getNominalPrepareDistance(); const auto total_avoid_distance = prepare_distance + avoidance_distance + constant_distance; @@ -637,7 +646,8 @@ void AvoidanceModule::fillDebugData( } } -void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, ShiftedPath & path) +void StaticObstacleAvoidanceModule::updateEgoBehavior( + const AvoidancePlanningData & data, ShiftedPath & path) { if (parameters_->disable_path_update) { return; @@ -676,7 +686,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif setStopReason(StopReason::AVOIDANCE, path.path); } -bool AvoidanceModule::isSafePath( +bool StaticObstacleAvoidanceModule::isSafePath( ShiftedPath & shifted_path, [[maybe_unused]] DebugData & debug) const { const auto & p = planner_data_->parameters; @@ -717,8 +727,9 @@ bool AvoidanceModule::isSafePath( const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate; - const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( - avoid_data_, planner_data_, parameters_, has_left_shift, has_right_shift, debug); + const auto safety_check_target_objects = + utils::static_obstacle_avoidance::getSafetyCheckTargetObjects( + avoid_data_, planner_data_, parameters_, has_left_shift, has_right_shift, debug); if (safety_check_target_objects.empty()) { return true; @@ -782,7 +793,8 @@ bool AvoidanceModule::isSafePath( return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count; } -PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & original_path) const +PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength( + const PathWithLaneId & original_path) const { const auto previous_path = helper_->getPreviousReferencePath(); @@ -844,7 +856,7 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig return extended_path; } -BehaviorModuleOutput AvoidanceModule::plan() +BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() { const auto & data = avoid_data_; @@ -866,8 +878,10 @@ BehaviorModuleOutput AvoidanceModule::plan() } // generate path with shift points that have been inserted. - ShiftedPath linear_shift_path = utils::avoidance::toShiftedPath(data.reference_path); - ShiftedPath spline_shift_path = utils::avoidance::toShiftedPath(data.reference_path); + ShiftedPath linear_shift_path = + utils::static_obstacle_avoidance::toShiftedPath(data.reference_path); + ShiftedPath spline_shift_path = + utils::static_obstacle_avoidance::toShiftedPath(data.reference_path); const auto success_spline_path_generation = path_shifter_.generate(&spline_shift_path, true, SHIFT_TYPE::SPLINE); const auto success_linear_path_generation = @@ -956,7 +970,8 @@ BehaviorModuleOutput AvoidanceModule::plan() std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { current_drivable_area_info.drivable_lanes.push_back( - utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + utils::static_obstacle_avoidance::generateExpandedDrivableLanes( + lanelet, planner_data_, parameters_)); }); // expand hatched road markings current_drivable_area_info.enable_expanding_hatched_road_markings = @@ -976,7 +991,7 @@ BehaviorModuleOutput AvoidanceModule::plan() if (!object.is_avoidable) clip_objects.push_back(object); }); current_drivable_area_info.obstacles = - utils::avoidance::generateObstaclePolygonsForDrivableArea( + utils::static_obstacle_avoidance::generateObstaclePolygonsForDrivableArea( clip_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); } else { current_drivable_area_info.obstacles.clear(); @@ -991,7 +1006,7 @@ BehaviorModuleOutput AvoidanceModule::plan() return output; } -CandidateOutput AvoidanceModule::planCandidate() const +CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const { const auto & data = avoid_data_; @@ -1030,7 +1045,7 @@ CandidateOutput AvoidanceModule::planCandidate() const return output; } -BehaviorModuleOutput AvoidanceModule::planWaitingApproval() +BehaviorModuleOutput StaticObstacleAvoidanceModule::planWaitingApproval() { BehaviorModuleOutput out = plan(); @@ -1044,7 +1059,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() return out; } -void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) +void StaticObstacleAvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) { if (parameters_->disable_path_update) { return; @@ -1085,10 +1100,10 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) /** * set new shift points. remove old shift points if it has a conflict. */ -void AvoidanceModule::addNewShiftLines( +void StaticObstacleAvoidanceModule::addNewShiftLines( PathShifter & path_shifter, const AvoidLineArray & new_shift_lines) const { - ShiftLineArray future = utils::avoidance::toShiftLineArray(new_shift_lines); + ShiftLineArray future = utils::static_obstacle_avoidance::toShiftLineArray(new_shift_lines); size_t min_start_idx = std::numeric_limits::max(); for (const auto & sl : new_shift_lines) { @@ -1151,7 +1166,7 @@ void AvoidanceModule::addNewShiftLines( path_shifter.setLateralAccelerationLimit(helper_->getLateralMaxAccelLimit()); } -bool AvoidanceModule::isValidShiftLine( +bool StaticObstacleAvoidanceModule::isValidShiftLine( const AvoidLineArray & shift_lines, const PathShifter & shifter) const { if (shift_lines.empty()) { @@ -1214,9 +1229,9 @@ bool AvoidanceModule::isValidShiftLine( return true; // valid shift line. } -void AvoidanceModule::updateData() +void StaticObstacleAvoidanceModule::updateData() { - using utils::avoidance::toShiftedPath; + using utils::static_obstacle_avoidance::toShiftedPath; helper_->setData(planner_data_); @@ -1224,7 +1239,7 @@ void AvoidanceModule::updateData() helper_->setPreviousSplineShiftPath(toShiftedPath(getPreviousModuleOutput().path)); helper_->setPreviousLinearShiftPath(toShiftedPath(getPreviousModuleOutput().path)); helper_->setPreviousReferencePath(getPreviousModuleOutput().path); - helper_->setPreviousDrivingLanes(utils::avoidance::getCurrentLanesFromPath( + helper_->setPreviousDrivingLanes(utils::static_obstacle_avoidance::getCurrentLanesFromPath( getPreviousModuleOutput().reference_path, planner_data_)); } @@ -1266,18 +1281,18 @@ void AvoidanceModule::updateData() safe_ = avoid_data_.safe; } -void AvoidanceModule::processOnEntry() +void StaticObstacleAvoidanceModule::processOnEntry() { initVariables(); } -void AvoidanceModule::processOnExit() +void StaticObstacleAvoidanceModule::processOnExit() { initVariables(); initRTCStatus(); } -void AvoidanceModule::initVariables() +void StaticObstacleAvoidanceModule::initVariables() { helper_->reset(); generator_.reset(); @@ -1289,7 +1304,7 @@ void AvoidanceModule::initVariables() arrived_path_end_ = false; } -void AvoidanceModule::initRTCStatus() +void StaticObstacleAvoidanceModule::initRTCStatus() { left_shift_array_.clear(); right_shift_array_.clear(); @@ -1298,7 +1313,7 @@ void AvoidanceModule::initRTCStatus() candidate_uuid_ = generateUUID(); } -void AvoidanceModule::updateRTCData() +void StaticObstacleAvoidanceModule::updateRTCData() { const auto & data = avoid_data_; @@ -1333,23 +1348,24 @@ void AvoidanceModule::updateRTCData() updateCandidateRTCStatus(output); } -void AvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const +void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { - using utils::avoidance::createTargetObjectsMarkerArray; + using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray; info_marker_.markers.clear(); appendMarkerArray( createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_); } -void AvoidanceModule::updateDebugMarker( +void StaticObstacleAvoidanceModule::updateDebugMarker( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { debug_marker_.markers.clear(); - debug_marker_ = utils::avoidance::createDebugMarkerArray(data, shifter, debug, parameters_); + debug_marker_ = + utils::static_obstacle_avoidance::createDebugMarkerArray(data, shifter, debug, parameters_); } -void AvoidanceModule::updateAvoidanceDebugData( +void StaticObstacleAvoidanceModule::updateAvoidanceDebugData( std::vector & avoidance_debug_msg_array) const { debug_data_.avoidance_debug_msg_array.avoidance_info.clear(); @@ -1366,7 +1382,7 @@ void AvoidanceModule::updateAvoidanceDebugData( } } -double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const +double StaticObstacleAvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const { const auto & p = parameters_; const auto & vehicle_width = planner_data_->parameters.vehicle_width; @@ -1395,8 +1411,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const : object_parameter.lateral_hard_margin; const auto avoid_margin = lateral_hard_margin * object.distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; - const auto avoidance_distance = helper_->getMinAvoidanceDistance( - helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); + const auto avoidance_distance = helper_->getMinAvoidanceDistance(helper_->getShiftLength( + object, utils::static_obstacle_avoidance::isOnRight(object), avoid_margin)); const auto constant_distance = helper_->getFrontConstantDistance(object); const auto prepare_distance = helper_->getNominalPrepareDistance(0.0); @@ -1406,7 +1422,7 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const p->stop_max_distance); } -void AvoidanceModule::insertReturnDeadLine( +void StaticObstacleAvoidanceModule::insertReturnDeadLine( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { const auto & data = avoid_data_; @@ -1437,7 +1453,7 @@ void AvoidanceModule::insertReturnDeadLine( // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately if (!use_constraints_for_decel) { - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_); return; @@ -1450,7 +1466,7 @@ void AvoidanceModule::insertReturnDeadLine( return; } - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_); // insert slow down speed. @@ -1483,7 +1499,7 @@ void AvoidanceModule::insertReturnDeadLine( } } -void AvoidanceModule::insertWaitPoint( +void StaticObstacleAvoidanceModule::insertWaitPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { const auto & data = avoid_data_; @@ -1504,7 +1520,7 @@ void AvoidanceModule::insertWaitPoint( // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately if (!use_constraints_for_decel) { - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), data.to_stop_line, 0.0, shifted_path.path, stop_pose_); return; } @@ -1519,7 +1535,7 @@ void AvoidanceModule::insertWaitPoint( // If target object can be stopped for, insert a deceleration point and return if (data.stop_target_object.value().is_stoppable) { - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), data.to_stop_line, 0.0, shifted_path.path, stop_pose_); return; } @@ -1527,11 +1543,11 @@ void AvoidanceModule::insertWaitPoint( // If the object cannot be stopped for, calculate a "mild" deceleration distance // and insert a deceleration point at that distance const auto stop_distance = helper_->getFeasibleDecelDistance(0.0, false); - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), stop_distance, 0.0, shifted_path.path, stop_pose_); } -void AvoidanceModule::insertStopPoint( +void StaticObstacleAvoidanceModule::insertStopPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { const auto & data = avoid_data_; @@ -1563,7 +1579,7 @@ void AvoidanceModule::insertStopPoint( // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately if (!use_constraints_for_decel) { - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), stop_distance, 0.0, shifted_path.path, stop_pose_); return; } @@ -1575,11 +1591,11 @@ void AvoidanceModule::insertStopPoint( } constexpr double MARGIN = 1.0; - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), stop_distance - MARGIN, 0.0, shifted_path.path, stop_pose_); } -void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const +void StaticObstacleAvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { const auto & data = avoid_data_; @@ -1643,7 +1659,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto avoid_margin = lateral_hard_margin * object.value().distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto shift_length = helper_->getShiftLength( - object.value(), utils::avoidance::isOnRight(object.value()), avoid_margin); + object.value(), utils::static_obstacle_avoidance::isOnRight(object.value()), avoid_margin); // check slow down feasibility const auto min_avoid_distance = helper_->getMinAvoidanceDistance(shift_length); @@ -1661,7 +1677,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( shift_length, helper_->getLateralMinJerkLimit(), distance_to_object); if (current_target_velocity < getEgoSpeed() + parameters_->buf_slow_down_speed) { - utils::avoidance::insertDecelPoint( + utils::static_obstacle_avoidance::insertDecelPoint( getEgoPosition(), decel_distance, parameters_->velocity_map.front(), shifted_path.path, slow_pose_); return; @@ -1691,7 +1707,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const shifted_path.path.points, start_idx, distance_to_object); } -void AvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const +void StaticObstacleAvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const { const auto & data = avoid_data_; @@ -1738,20 +1754,21 @@ void AvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const shifted_path.path.points, start_idx, distance_to_accel_end_point); } -std::shared_ptr AvoidanceModule::get_debug_msg_array() const +std::shared_ptr StaticObstacleAvoidanceModule::get_debug_msg_array() const { debug_data_.avoidance_debug_msg_array.header.stamp = clock_->now(); return std::make_shared(debug_data_.avoidance_debug_msg_array); } -void AvoidanceModule::acceptVisitor(const std::shared_ptr & visitor) const +void StaticObstacleAvoidanceModule::acceptVisitor( + const std::shared_ptr & visitor) const { if (visitor) { visitor->visitAvoidanceModule(this); } } -void SceneModuleVisitor::visitAvoidanceModule(const AvoidanceModule * module) const +void SceneModuleVisitor::visitAvoidanceModule(const StaticObstacleAvoidanceModule * module) const { avoidance_visitor_ = module->get_debug_msg_array(); } diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp similarity index 94% rename from planning/behavior_path_avoidance_module/src/shift_line_generator.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index fc3b9e24ff9f6..f9e35ebd4c1b2 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_avoidance_module/shift_line_generator.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp" -#include "behavior_path_avoidance_module/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" -namespace behavior_path_planner::utils::avoidance +namespace behavior_path_planner::utils::static_obstacle_avoidance { namespace @@ -123,7 +123,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // use each object param const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto is_object_on_right = utils::avoidance::isOnRight(object); + const auto is_object_on_right = utils::static_obstacle_avoidance::isOnRight(object); // use absolute dist for return-to-center, relative dist from current for avoiding. const auto avoiding_shift = desire_shift_length - current_ego_shift; @@ -254,10 +254,11 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } } - const auto is_object_on_right = utils::avoidance::isOnRight(o); + const auto is_object_on_right = utils::static_obstacle_avoidance::isOnRight(o); const auto desire_shift_length = helper_->getShiftLength(o, is_object_on_right, o.avoid_margin.value()); - if (utils::avoidance::isSameDirectionShift(is_object_on_right, desire_shift_length)) { + if (utils::static_obstacle_avoidance::isSameDirectionShift( + is_object_on_right, desire_shift_length)) { o.info = ObjectInfo::SAME_DIRECTION_SHIFT; if (o.avoid_required && is_forward_object(o)) { break; @@ -302,7 +303,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance); }(); - al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength( + al_avoid.start_idx = utils::static_obstacle_avoidance::findPathIndexFromArclength( data.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); al_avoid.start = data.reference_path.points.at(al_avoid.start_idx).point.pose; al_avoid.start_shift_length = helper_->getLinearShift(al_avoid.start.position); @@ -314,7 +315,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // misc al_avoid.id = generateUUID(); al_avoid.object = o; - al_avoid.object_on_right = utils::avoidance::isOnRight(o); + al_avoid.object_on_right = utils::static_obstacle_avoidance::isOnRight(o); } AvoidLine al_return; @@ -340,7 +341,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // misc al_return.id = generateUUID(); al_return.object = o; - al_return.object_on_right = utils::avoidance::isOnRight(o); + al_return.object_on_right = utils::static_obstacle_avoidance::isOnRight(o); } const bool skip_return_shift = [&]() { @@ -376,7 +377,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( o.is_avoidable = true; } - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, outlines); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal(data, outlines); debug.step1_current_shift_line = toArray(outlines); @@ -479,7 +480,7 @@ void ShiftLineGenerator::generateTotalShiftLine( const auto & al = avoid_lines.at(j); for (size_t i = 0; i < N; ++i) { // calc current interpolated shift - const auto i_shift = utils::avoidance::lerpShiftLengthOnArc(arcs.at(i), al); + const auto i_shift = utils::static_obstacle_avoidance::lerpShiftLengthOnArc(arcs.at(i), al); // update maximum shift for positive direction if (i_shift > sl.pos_shift_line.at(i)) { @@ -548,8 +549,8 @@ void ShiftLineGenerator::generateTotalShiftLine( AvoidLineArray ShiftLineGenerator::extractShiftLinesFromLine( const AvoidancePlanningData & data, ShiftLineData & shift_line_data) const { - using utils::avoidance::setEndData; - using utils::avoidance::setStartData; + using utils::static_obstacle_avoidance::setEndData; + using utils::static_obstacle_avoidance::setStartData; const auto & path = data.reference_path; const auto & arcs = data.arclength_from_ego; @@ -706,8 +707,9 @@ AvoidOutlines ShiftLineGenerator::applyMergeProcess( } } - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, ret); - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, debug.step1_merged_shift_line); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal(data, ret); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal( + data, debug.step1_merged_shift_line); return ret; } @@ -749,8 +751,9 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( helper_->alignShiftLinesOrder(outline.middle_lines, false); } - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, ret); - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, debug.step1_filled_shift_line); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal(data, ret); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal( + data, debug.step1_filled_shift_line); return ret; } @@ -771,7 +774,7 @@ AvoidLineArray ShiftLineGenerator::applyFillGapProcess( // fill gap between ego and nearest shift line. if (sorted.front().start_longitudinal > 0.0) { AvoidLine ego_line{}; - utils::avoidance::setEndData( + utils::static_obstacle_avoidance::setEndData( ego_line, helper_->getEgoLinearShift(), data.reference_pose, data.ego_closest_path_index, 0.0); @@ -795,8 +798,9 @@ AvoidLineArray ShiftLineGenerator::applyFillGapProcess( helper_->alignShiftLinesOrder(ret, false); - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, ret); - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, debug.step1_front_shift_line); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal(data, ret); + utils::static_obstacle_avoidance::fillAdditionalInfoFromLongitudinal( + data, debug.step1_front_shift_line); return ret; } @@ -806,7 +810,8 @@ AvoidLineArray ShiftLineGenerator::applyCombineProcess( DebugData & debug) const { debug.step1_registered_shift_line = registered_lines; - return utils::avoidance::combineRawShiftLinesWithUniqueCheck(registered_lines, shift_lines); + return utils::static_obstacle_avoidance::combineRawShiftLinesWithUniqueCheck( + registered_lines, shift_lines); } AvoidLineArray ShiftLineGenerator::applyMergeProcess( @@ -821,7 +826,7 @@ AvoidLineArray ShiftLineGenerator::applyMergeProcess( // set parent id for (auto & al : merged_shift_lines) { - al.parent_ids = utils::avoidance::calcParentIds(shift_lines, al); + al.parent_ids = utils::static_obstacle_avoidance::calcParentIds(shift_lines, al); } // sort by distance from ego. @@ -963,15 +968,15 @@ void ShiftLineGenerator::applySimilarGradFilter( for (size_t i = 1; i < input.size(); ++i) { AvoidLine combine{}; - utils::avoidance::setStartData( + utils::static_obstacle_avoidance::setStartData( combine, base_line.start_shift_length, base_line.start, base_line.start_idx, base_line.start_longitudinal); - utils::avoidance::setEndData( + utils::static_obstacle_avoidance::setEndData( combine, input.at(i).end_shift_length, input.at(i).end, input.at(i).end_idx, input.at(i).end_longitudinal); - combine.parent_ids = - utils::avoidance::concatParentIds(base_line.parent_ids, input.at(i).parent_ids); + combine.parent_ids = utils::static_obstacle_avoidance::concatParentIds( + base_line.parent_ids, input.at(i).parent_ids); combine_buffer.push_back(input.at(i)); @@ -1060,14 +1065,16 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( // avoidance points: No, shift points: Yes -> select last shift point. if (!has_candidate_point && has_registered_point) { - last_sl = utils::avoidance::fillAdditionalInfo(data, AvoidLine{last_.value()}); + last_sl = + utils::static_obstacle_avoidance::fillAdditionalInfo(data, AvoidLine{last_.value()}); } // avoidance points: Yes, shift points: Yes -> select the last one from both. if (has_candidate_point && has_registered_point) { helper_->alignShiftLinesOrder(ret, false); const auto & al = ret.back(); - const auto & sl = utils::avoidance::fillAdditionalInfo(data, AvoidLine{last_.value()}); + const auto & sl = + utils::static_obstacle_avoidance::fillAdditionalInfo(data, AvoidLine{last_.value()}); last_sl = (sl.end_longitudinal > al.end_longitudinal) ? sl : al; } @@ -1190,8 +1197,8 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( al.start_idx = last_sl.end_idx; al.start = last_sl.end; al.start_longitudinal = arclength_from_ego.at(al.start_idx); - al.end_idx = - utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); + al.end_idx = utils::static_obstacle_avoidance::findPathIndexFromArclength( + arclength_from_ego, prepare_distance_scaled); al.end = data.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = prepare_distance_scaled; al.end_shift_length = last_sl.end_shift_length; @@ -1204,11 +1211,11 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( { AvoidLine al; al.id = generateUUID(); - al.start_idx = - utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); + al.start_idx = utils::static_obstacle_avoidance::findPathIndexFromArclength( + arclength_from_ego, prepare_distance_scaled); al.start = data.reference_path.points.at(al.start_idx).point.pose; al.start_longitudinal = prepare_distance_scaled; - al.end_idx = utils::avoidance::findPathIndexFromArclength( + al.end_idx = utils::static_obstacle_avoidance::findPathIndexFromArclength( arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled); al.end = data.reference_path.points.at(al.end_idx).point.pose; al.end_longitudinal = arclength_from_ego.at(al.end_idx); @@ -1315,7 +1322,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine( void ShiftLineGenerator::updateRegisteredRawShiftLines(const AvoidancePlanningData & data) { - utils::avoidance::fillAdditionalInfoFromPoint(data, raw_registered_); + utils::static_obstacle_avoidance::fillAdditionalInfoFromPoint(data, raw_registered_); AvoidLineArray avoid_lines; @@ -1364,7 +1371,7 @@ void ShiftLineGenerator::setRawRegisteredShiftLine( } auto future_with_info = shift_lines; - utils::avoidance::fillAdditionalInfoFromPoint(data, future_with_info); + utils::static_obstacle_avoidance::fillAdditionalInfoFromPoint(data, future_with_info); // sort by longitudinal std::sort(future_with_info.begin(), future_with_info.end(), [](auto a, auto b) { @@ -1405,4 +1412,4 @@ void ShiftLineGenerator::setRawRegisteredShiftLine( } } } -} // namespace behavior_path_planner::utils::avoidance +} // namespace behavior_path_planner::utils::static_obstacle_avoidance diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp similarity index 99% rename from planning/behavior_path_avoidance_module/src/utils.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 089571bc8392e..1533a7dd49edf 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -14,8 +14,8 @@ #include "behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_avoidance_module/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" @@ -37,7 +37,7 @@ #include #include -namespace behavior_path_planner::utils::avoidance +namespace behavior_path_planner::utils::static_obstacle_avoidance { using autoware_perception_msgs::msg::TrafficSignalElement; @@ -1681,7 +1681,8 @@ void filterTargetObjects( } // Find the footprint point closest to the path, set to object_data.overhang_distance. - o.overhang_points = utils::avoidance::calcEnvelopeOverhangDistance(o, data.reference_path); + o.overhang_points = + utils::static_obstacle_avoidance::calcEnvelopeOverhangDistance(o, data.reference_path); o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); // TODO(Satoshi Ota) parametrize stop time threshold if need. @@ -2303,4 +2304,4 @@ double calcDistanceToReturnDeadLine( return distance_to_return_dead_line; } -} // namespace behavior_path_planner::utils::avoidance +} // namespace behavior_path_planner::utils::static_obstacle_avoidance diff --git a/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp similarity index 84% rename from planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index cf38a0b4993fe..c70b78265a17b 100644 --- a/planning/behavior_path_dynamic_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -47,22 +47,22 @@ 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::DynamicAvoidanceModuleManager"); + module_names.emplace_back("behavior_path_planner::StaticObstacleAvoidanceModuleManager"); 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_dynamic_avoidance_module") + - "/config/dynamic_avoidance.param.yaml"}); + 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( + "autoware_behavior_path_static_obstacle_avoidance_module") + + "/config/static_obstacle_avoidance.param.yaml"}); return std::make_shared(node_options); } diff --git a/planning/behavior_path_avoidance_module/test/test_utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp similarity index 83% rename from planning/behavior_path_avoidance_module/test/test_utils.cpp rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp index 1a114eef9f7f7..da681e72533b0 100644 --- a/planning/behavior_path_avoidance_module/test/test_utils.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_avoidance_module/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" #include #include using behavior_path_planner::ObjectData; -using behavior_path_planner::utils::avoidance::isOnRight; -using behavior_path_planner::utils::avoidance::isSameDirectionShift; -using behavior_path_planner::utils::avoidance::isShiftNecessary; +using behavior_path_planner::utils::static_obstacle_avoidance::isOnRight; +using behavior_path_planner::utils::static_obstacle_avoidance::isSameDirectionShift; +using behavior_path_planner::utils::static_obstacle_avoidance::isShiftNecessary; TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) { 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 index 8e7d1f67d3157..d7fd8b82bd71e 100644 --- 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 @@ -14,7 +14,7 @@ #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 "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" namespace behavior_path_planner { diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp index 54a4a0c70486d..4ef4c0673c0c5 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ #define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ +#include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" -#include "behavior_path_avoidance_module/helper.hpp" #include "behavior_path_lane_change_module/scene.hpp" #include @@ -24,7 +24,7 @@ namespace behavior_path_planner { using AvoidanceDebugData = DebugData; -using helper::avoidance::AvoidanceHelper; +using helper::static_obstacle_avoidance::AvoidanceHelper; class AvoidanceByLaneChange : public NormalLaneChange { diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_avoidance_by_lane_change_module/package.xml index 0f9f3f6dc9d42..84540618e152d 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_avoidance_by_lane_change_module/package.xml @@ -18,7 +18,7 @@ autoware_cmake eigen3_cmake_module - behavior_path_avoidance_module + autoware_behavior_path_static_obstacle_avoidance_module behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common 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 index a3b28b4d63d06..c7c00d38956c1 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -14,8 +14,8 @@ #include "behavior_path_avoidance_by_lane_change_module/manager.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" -#include "behavior_path_avoidance_module/parameter_helper.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index d7d7fa8b60513..80ae361ee3ea1 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -14,13 +14,13 @@ #include "behavior_path_avoidance_by_lane_change_module/scene.hpp" -#include "behavior_path_avoidance_module/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.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 +#include #include #include #include @@ -103,13 +103,14 @@ void AvoidanceByLaneChange::updateSpecialData() if (avoidance_data_.target_objects.empty()) { direction_ = Direction::NONE; } else { - direction_ = utils::avoidance::isOnRight(avoidance_data_.target_objects.front()) + direction_ = utils::static_obstacle_avoidance::isOnRight(avoidance_data_.target_objects.front()) ? Direction::LEFT : Direction::RIGHT; } - utils::avoidance::updateRegisteredObject(registered_objects_, avoidance_data_.target_objects, p); - utils::avoidance::compensateDetectionLost( + utils::static_obstacle_avoidance::updateRegisteredObject( + registered_objects_, avoidance_data_.target_objects, p); + utils::static_obstacle_avoidance::compensateDetectionLost( registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); std::sort( @@ -222,28 +223,29 @@ std::optional AvoidanceByLaneChange::createObjectData( object_data.distance_factor = object_parameter.max_expand_ratio * clamp + 1.0; // Calc envelop polygon. - utils::avoidance::fillObjectEnvelopePolygon( + utils::static_obstacle_avoidance::fillObjectEnvelopePolygon( object_data, registered_objects_, object_closest_pose, p); // calc object centroid. object_data.centroid = return_centroid(object_data.envelope_poly); // Calc moving time. - utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, p); + utils::static_obstacle_avoidance::fillObjectMovingTime(object_data, stopped_objects_, p); object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 ? Direction::LEFT : Direction::RIGHT; // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_points = - utils::avoidance::calcEnvelopeOverhangDistance(object_data, data.reference_path); + object_data.overhang_points = utils::static_obstacle_avoidance::calcEnvelopeOverhangDistance( + object_data, data.reference_path); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; - utils::avoidance::fillAvoidanceNecessity(object_data, registered_objects_, vehicle_width, p); + utils::static_obstacle_avoidance::fillAvoidanceNecessity( + object_data, registered_objects_, vehicle_width, p); - utils::avoidance::fillLongitudinalAndLengthByClosestEnvelopeFootprint( + utils::static_obstacle_avoidance::fillLongitudinalAndLengthByClosestEnvelopeFootprint( data.reference_path_rough, getEgoPosition(), object_data); return object_data; } @@ -262,7 +264,7 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_ avoidance_helper_->setData(planner_data_); const auto shift_length = avoidance_helper_->getShiftLength( - nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin); + nearest_object, utils::static_obstacle_avoidance::isOnRight(nearest_object), avoid_margin); return avoidance_helper_->getMinAvoidanceDistance(shift_length); } 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 index f66e944cb7cbc..69ce87aabc7b4 100644 --- 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 @@ -67,8 +67,9 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", behavior_path_lane_change_module_dir + "/config/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( + "autoware_behavior_path_static_obstacle_avoidance_module") + + "/config/static_obstacle_avoidance.param.yaml", ament_index_cpp::get_package_share_directory("behavior_path_avoidance_by_lane_change_module") + "/config/avoidance_by_lane_change.param.yaml"}); diff --git a/planning/behavior_path_avoidance_module/plugins.xml b/planning/behavior_path_avoidance_module/plugins.xml deleted file mode 100644 index f25677dad1e9a..0000000000000 --- a/planning/behavior_path_avoidance_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_path_dynamic_avoidance_module/plugins.xml b/planning/behavior_path_dynamic_avoidance_module/plugins.xml deleted file mode 100644 index fd2e1bc4137b7..0000000000000 --- a/planning/behavior_path_dynamic_avoidance_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index beaa1ae6c5263..a799fad0a1c36 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -24,17 +24,17 @@ Essentially, the module has three primary responsibilities: Behavior Path Planner has following scene modules -| Name | Description | Details | -| :----------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :----------------------------------------------------------------- | -| Lane Following | this module generates reference path from lanelet centerline. | LINK | -| Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_module/README.md) | -| Dynamic Avoidance | WIP | [LINK](../behavior_path_dynamic_avoidance_module/README.md) | -| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | -| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../behavior_path_lane_change_module/README.md) | -| External Lane Change | WIP | LINK | -| Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../behavior_path_goal_planner_module/README.md) | -| Start Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../behavior_path_start_planner_module/README.md) | -| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../behavior_path_side_shift_module/README.md) | +| Name | Description | Details | +| :------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------------- | +| Lane Following | this module generates reference path from lanelet centerline. | LINK | +| Static Obstacle Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](../autoware_behavior_path_static_obstacle_avoidance_module/README.md) | +| Dynamic Obstacle Avoidance | WIP | [LINK](../autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md) | +| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | +| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../behavior_path_lane_change_module/README.md) | +| External Lane Change | WIP | LINK | +| Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../behavior_path_goal_planner_module/README.md) | +| Start Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../behavior_path_start_planner_module/README.md) | +| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../behavior_path_side_shift_module/README.md) | !!! Note @@ -138,19 +138,19 @@ Enabling and disabling the modules in the behavior path planner is primarily man The `default_preset.yaml` file acts as a configuration file for enabling or disabling specific modules within the planner. It contains a series of arguments which represent the behavior path planner's modules or features. For example: -- `launch_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. +- `launch_static_obstacle_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. !!! note Click [here](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/preset/default_preset.yaml) to view the `default_preset.yaml`. -The `behavior_path_planner.launch.xml` file references the settings defined in `default_preset.yaml` to apply the configurations when the behavior path planner's node is running. For instance, the parameter `avoidance.enable_module` in +The `behavior_path_planner.launch.xml` file references the settings defined in `default_preset.yaml` to apply the configurations when the behavior path planner's node is running. For instance, the parameter `static_obstacle_avoidance.enable_module` in ```xml - + ``` -corresponds to launch_avoidance_module from `default_preset.yaml`. +corresponds to launch_static_obstacle_avoidance_module from `default_preset.yaml`. Therefore, to enable or disable a module, simply set the corresponding module in `default_preset.yaml` to `true` or `false`. These changes will be applied upon the next launch of Autoware. @@ -245,12 +245,12 @@ behavior_path_planner ├── behavior_path_planner.param.yaml ├── drivable_area_expansion.param.yaml ├── scene_module_manager.param.yaml -├── avoidance -│ └── avoidance.param.yaml +├── static_obstacle_avoidance +│ └── static_obstacle_avoidance.param.yaml ├── avoidance_by_lc │ └── avoidance_by_lc.param.yaml -├── dynamic_avoidance -│ └── dynamic_avoidance.param.yaml +├── dynamic_obstacle_avoidance +│ └── dynamic_obstacle_avoidance.param.yaml ├── goal_planner │ └── goal_planner.param.yaml ├── lane_change diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index c2b6babc402a2..6cf8719b2ef83 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -59,7 +59,7 @@ priority: 1 max_module_size: 1 - avoidance: + static_obstacle_avoidance: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false @@ -75,7 +75,7 @@ priority: 3 max_module_size: 1 - dynamic_avoidance: + dynamic_obstacle_avoidance: enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp index ffc5daa7aa2ff..1bce30b18edd7 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp @@ -21,7 +21,7 @@ namespace behavior_path_planner { // Forward Declaration -class AvoidanceModule; +class StaticObstacleAvoidanceModule; class AvoidanceByLCModule; class ExternalRequestLaneChangeModule; class LaneChangeInterface; @@ -35,7 +35,7 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; class SceneModuleVisitor { public: - void visitAvoidanceModule(const AvoidanceModule * module) const; + void visitAvoidanceModule(const StaticObstacleAvoidanceModule * module) const; std::shared_ptr getAvoidanceModuleDebugMsg() const; diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index 5e4b202071e49..ea80dc9bb9d1f 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -62,9 +62,9 @@ Module getModuleType(const std::string & module_name) module.type = Module::AVOIDANCE_BY_LC_LEFT; } else if (module_name == "avoidance_by_lane_change_right") { module.type = Module::AVOIDANCE_BY_LC_RIGHT; - } else if (module_name == "avoidance_left") { + } else if (module_name == "static_obstacle_avoidance_left") { module.type = Module::AVOIDANCE_LEFT; - } else if (module_name == "avoidance_right") { + } else if (module_name == "static_obstacle_avoidance_right") { module.type = Module::AVOIDANCE_RIGHT; } else if (module_name == "goal_planner") { module.type = Module::GOAL_PLANNER;