diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 24bc144c40afd..854c29aa89bc5 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -47,6 +47,7 @@ max_ego_path_lat_cover_ratio: 0.3 # [-] The object will be ignored if the ratio of the object covering the ego's path is higher than this value. drivable_area_generation: + polygon_generation_method: "ego_path_base" # choose "ego_path_base" and "object_path_base" object_path_base: min_longitudinal_polygon_margin: 3.0 # [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 49ce760fec4c9..e37ef27d44426 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -46,6 +46,11 @@ struct MinMaxValue double max_value{0.0}; }; +enum class PolygonGenerationMethod { + EGO_PATH_BASE = 0, + OBJECT_PATH_BASE, +}; + struct DynamicAvoidanceParameters { // common @@ -84,6 +89,7 @@ struct DynamicAvoidanceParameters double max_oncoming_crossing_object_angle{0.0}; // drivable area generation + PolygonGenerationMethod polygon_generation_method{}; double min_obj_path_based_lon_polygon_margin{0.0}; double lat_offset_from_obstacle{0.0}; double max_lat_offset_to_avoid{0.0}; @@ -103,10 +109,6 @@ struct DynamicAvoidanceParameters class DynamicAvoidanceModule : public SceneModuleInterface { public: - enum class PolygonGenerationMethod { - EGO_PATH_BASE = 0, - OBJECT_PATH_BASE, - }; struct DynamicAvoidanceObject { DynamicAvoidanceObject( @@ -141,18 +143,15 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::optional lat_offset_to_avoid{std::nullopt}; bool is_collision_left{false}; bool should_be_avoided{false}; - PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::OBJECT_PATH_BASE}; void update( const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid, - const bool arg_is_collision_left, const bool arg_should_be_avoided, - const PolygonGenerationMethod & arg_polygon_generation_method) + const bool arg_is_collision_left, const bool arg_should_be_avoided) { lon_offset_to_avoid = arg_lon_offset_to_avoid; lat_offset_to_avoid = arg_lat_offset_to_avoid; is_collision_left = arg_is_collision_left; should_be_avoided = arg_should_be_avoided; - polygon_generation_method = arg_polygon_generation_method; } }; @@ -246,12 +245,11 @@ class DynamicAvoidanceModule : public SceneModuleInterface void updateObject( const std::string & uuid, const MinMaxValue & lon_offset_to_avoid, const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left, - const bool should_be_avoided, const PolygonGenerationMethod & polygon_generation_method) + const bool should_be_avoided) { if (object_map_.count(uuid) != 0) { object_map_.at(uuid).update( - lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, - polygon_generation_method); + lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); } } @@ -311,8 +309,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface void updateTargetObjects(); bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset, - PolygonGenerationMethod & polygon_generation_method) const; + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; DecisionWithReason willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 1310c91d93afe..de76166e068d3 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -319,10 +319,10 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() std::vector obstacles_for_drivable_area; for (const auto & object : target_objects_) { const auto obstacle_poly = [&]() { - if (object.polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) { + if (parameters_->polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) { return calcEgoPathBasedDynamicObstaclePolygon(object); } - if (object.polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) { + if (parameters_->polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) { return calcObjectPathBasedDynamicObstaclePolygon(object); } throw std::logic_error("The polygon_generation_method's string is invalid."); @@ -492,7 +492,6 @@ void DynamicAvoidanceModule::updateTargetObjects() // 2. Precise filtering of target objects and check if they should be avoided for (const auto & object : target_objects_manager_.getValidObjects()) { - PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::EGO_PATH_BASE}; const auto obj_uuid = object.uuid; const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); const auto obj_path = *std::max_element( @@ -519,8 +518,8 @@ void DynamicAvoidanceModule::updateTargetObjects() getLateralLongitudinalOffset(input_path->points, object.pose, object.shape); // 2.c. check if object will not cut in - const bool will_object_cut_in = willObjectCutIn( - input_path->points, obj_path, object.vel, lat_lon_offset, polygon_generation_method); + const bool will_object_cut_in = + willObjectCutIn(input_path->points, obj_path, object.vel, lat_lon_offset); if (will_object_cut_in) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -607,8 +606,7 @@ void DynamicAvoidanceModule::updateTargetObjects() const bool should_be_avoided = true; target_objects_manager_.updateObject( - obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided, - polygon_generation_method); + obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided); } prev_input_ref_path_points = input_ref_path_points; @@ -686,8 +684,7 @@ bool DynamicAvoidanceModule::isObjectFarFromPath( bool DynamicAvoidanceModule::willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset, - PolygonGenerationMethod & polygon_generation_method) const + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const { // Ignore oncoming object if (obj_tangent_vel < parameters_->min_cut_in_object_vel) { @@ -721,7 +718,6 @@ bool DynamicAvoidanceModule::willObjectCutIn( lon_offset_ego_to_obj < std::max( parameters_->min_lon_offset_ego_to_cut_in_object, relative_velocity * parameters_->min_time_to_start_cut_in)) { - polygon_generation_method = PolygonGenerationMethod::EGO_PATH_BASE; return false; } diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index bf4197c59ffd9..2824a6221591a 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -24,6 +24,18 @@ namespace behavior_path_planner { +namespace +{ +PolygonGenerationMethod convertToPolygonGenerationMethod(const std::string & str) +{ + if (str == "ego_path_base") { + return PolygonGenerationMethod::EGO_PATH_BASE; + } else if (str == "object_path_base") { + return PolygonGenerationMethod::OBJECT_PATH_BASE; + } + throw std::logic_error("The polygon_generation_method's string is invalid."); +} +} // namespace void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) { @@ -91,6 +103,8 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) { // drivable_area_generation const std::string ns = "dynamic_avoidance.drivable_area_generation."; + p.polygon_generation_method = convertToPolygonGenerationMethod( + node->declare_parameter(ns + "polygon_generation_method")); p.min_obj_path_based_lon_polygon_margin = node->declare_parameter(ns + "object_path_base.min_longitudinal_polygon_margin"); p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle"); @@ -197,6 +211,12 @@ void DynamicAvoidanceModuleManager::updateModuleParams( { // drivable_area_generation const std::string ns = "dynamic_avoidance.drivable_area_generation."; + std::string polygon_generation_method_str; + if (updateParam( + parameters, ns + "polygon_generation_method", polygon_generation_method_str)) { + p->polygon_generation_method = + convertToPolygonGenerationMethod(polygon_generation_method_str); + } updateParam( parameters, ns + "object_path_base.min_longitudinal_polygon_margin", p->min_obj_path_based_lon_polygon_margin);