Skip to content

Commit

Permalink
fix(autoware_behavior_path_start_planner_module): fix shadowVariable (a…
Browse files Browse the repository at this point in the history 
…utowarefoundation#7982)

* fix:shadowVariable

Signed-off-by: kobayu858 <[email protected]>

* fix:shadowVariable

Signed-off-by: kobayu858 <[email protected]>

* refactor:clang format

Signed-off-by: kobayu858 <[email protected]>

* refactor:clang format

Signed-off-by: kobayu858 <[email protected]>

* refactor:clang format

Signed-off-by: kobayu858 <[email protected]>

* refactor: change of declaration location

Signed-off-by: kobayu858 <[email protected]>

* fix:shadowVariable

Signed-off-by: kobayu858 <[email protected]>

* fix:shadowVariable

Signed-off-by: kobayu858 <[email protected]>

* fix:shadowVariable

Signed-off-by: kobayu858 <[email protected]>

* refactor:clang format

Signed-off-by: kobayu858 <[email protected]>

* refactor: namespace

Signed-off-by: kobayu858 <[email protected]>

* refactor:clang format

Signed-off-by: kobayu858 <[email protected]>

---------

Signed-off-by: kobayu858 <[email protected]>
  • Loading branch information
@kobayu858
kobayu858 committed Jul 25, 2024
1 parent 4ef61d7 commit 3e496cc
Show file tree
Hide file tree
Showing 2 changed files with 98 additions and 106 deletions.
200 changes: 96 additions & 104 deletions planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,21 +31,78 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)

StartPlannerParameters p;

const std::string ns = "start_planner.";

p.th_arrived_distance = node->declare_parameter<double>(ns + "th_arrived_distance");
p.th_stopped_velocity = node->declare_parameter<double>(ns + "th_stopped_velocity");
p.th_stopped_time = node->declare_parameter<double>(ns + "th_stopped_time");
p.prepare_time_before_start = node->declare_parameter<double>(ns + "prepare_time_before_start");
p.th_distance_to_middle_of_the_road =
node->declare_parameter<double>(ns + "th_distance_to_middle_of_the_road");
p.extra_width_margin_for_rear_obstacle =
node->declare_parameter<double>(ns + "extra_width_margin_for_rear_obstacle");
p.collision_check_margins =
node->declare_parameter<std::vector<double>>(ns + "collision_check_margins");
p.collision_check_margin_from_front_object =
node->declare_parameter<double>(ns + "collision_check_margin_from_front_object");
p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
{
const std::string ns = "start_planner.";

p.th_arrived_distance = node->declare_parameter<double>(ns + "th_arrived_distance");
p.th_stopped_velocity = node->declare_parameter<double>(ns + "th_stopped_velocity");
p.th_stopped_time = node->declare_parameter<double>(ns + "th_stopped_time");
p.prepare_time_before_start = node->declare_parameter<double>(ns + "prepare_time_before_start");
p.th_distance_to_middle_of_the_road =
node->declare_parameter<double>(ns + "th_distance_to_middle_of_the_road");
p.extra_width_margin_for_rear_obstacle =
node->declare_parameter<double>(ns + "extra_width_margin_for_rear_obstacle");
p.collision_check_margins =
node->declare_parameter<std::vector<double>>(ns + "collision_check_margins");
p.collision_check_margin_from_front_object =
node->declare_parameter<double>(ns + "collision_check_margin_from_front_object");
p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
p.center_line_path_interval = node->declare_parameter<double>(ns + "center_line_path_interval");
// shift pull out
p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
p.check_shift_path_lane_departure =
node->declare_parameter<bool>(ns + "check_shift_path_lane_departure");
p.allow_check_shift_path_lane_departure_override =
node->declare_parameter<bool>(ns + "allow_check_shift_path_lane_departure_override");
p.shift_collision_check_distance_from_end =
node->declare_parameter<double>(ns + "shift_collision_check_distance_from_end");
p.minimum_shift_pull_out_distance =
node->declare_parameter<double>(ns + "minimum_shift_pull_out_distance");
p.lateral_acceleration_sampling_num =
node->declare_parameter<int>(ns + "lateral_acceleration_sampling_num");
p.lateral_jerk = node->declare_parameter<double>(ns + "lateral_jerk");
p.maximum_lateral_acc = node->declare_parameter<double>(ns + "maximum_lateral_acc");
p.minimum_lateral_acc = node->declare_parameter<double>(ns + "minimum_lateral_acc");
p.maximum_curvature = node->declare_parameter<double>(ns + "maximum_curvature");
p.end_pose_curvature_threshold =
node->declare_parameter<double>(ns + "end_pose_curvature_threshold");
p.maximum_longitudinal_deviation =
node->declare_parameter<double>(ns + "maximum_longitudinal_deviation");
// geometric pull out
p.enable_geometric_pull_out = node->declare_parameter<bool>(ns + "enable_geometric_pull_out");
p.geometric_collision_check_distance_from_end =
node->declare_parameter<double>(ns + "geometric_collision_check_distance_from_end");
p.divide_pull_out_path = node->declare_parameter<bool>(ns + "divide_pull_out_path");
p.parallel_parking_parameters.pull_out_velocity =
node->declare_parameter<double>(ns + "geometric_pull_out_velocity");
p.parallel_parking_parameters.pull_out_arc_path_interval =
node->declare_parameter<double>(ns + "arc_path_interval");
p.parallel_parking_parameters.pull_out_lane_departure_margin =
node->declare_parameter<double>(ns + "lane_departure_margin");
p.lane_departure_check_expansion_margin =
node->declare_parameter<double>(ns + "lane_departure_check_expansion_margin");
p.parallel_parking_parameters.pull_out_max_steer_angle =
node->declare_parameter<double>(ns + "pull_out_max_steer_angle"); // 15deg
p.parallel_parking_parameters.center_line_path_interval =
p.center_line_path_interval; // for geometric parallel parking
// search start pose backward
p.search_priority = node->declare_parameter<std::string>(
ns + "search_priority"); // "efficient_path" or "short_back_distance"
p.enable_back = node->declare_parameter<bool>(ns + "enable_back");
p.backward_velocity = node->declare_parameter<double>(ns + "backward_velocity");
p.max_back_distance = node->declare_parameter<double>(ns + "max_back_distance");
p.backward_search_resolution =
node->declare_parameter<double>(ns + "backward_search_resolution");
p.backward_path_update_duration =
node->declare_parameter<double>(ns + "backward_path_update_duration");
p.ignore_distance_from_lane_end =
node->declare_parameter<double>(ns + "ignore_distance_from_lane_end");
// stop condition
p.maximum_deceleration_for_stop =
node->declare_parameter<double>(ns + "stop_condition.maximum_deceleration_for_stop");
p.maximum_jerk_for_stop =
node->declare_parameter<double>(ns + "stop_condition.maximum_jerk_for_stop");
}
{
const std::string ns = "start_planner.object_types_to_check_for_path_generation.";
p.object_types_to_check_for_path_generation.check_car =
Expand All @@ -65,55 +122,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
p.object_types_to_check_for_path_generation.check_pedestrian =
node->declare_parameter<bool>(ns + "check_pedestrian");
}
p.center_line_path_interval = node->declare_parameter<double>(ns + "center_line_path_interval");
// shift pull out
p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
p.check_shift_path_lane_departure =
node->declare_parameter<bool>(ns + "check_shift_path_lane_departure");
p.allow_check_shift_path_lane_departure_override =
node->declare_parameter<bool>(ns + "allow_check_shift_path_lane_departure_override");
p.shift_collision_check_distance_from_end =
node->declare_parameter<double>(ns + "shift_collision_check_distance_from_end");
p.minimum_shift_pull_out_distance =
node->declare_parameter<double>(ns + "minimum_shift_pull_out_distance");
p.lateral_acceleration_sampling_num =
node->declare_parameter<int>(ns + "lateral_acceleration_sampling_num");
p.lateral_jerk = node->declare_parameter<double>(ns + "lateral_jerk");
p.maximum_lateral_acc = node->declare_parameter<double>(ns + "maximum_lateral_acc");
p.minimum_lateral_acc = node->declare_parameter<double>(ns + "minimum_lateral_acc");
p.maximum_curvature = node->declare_parameter<double>(ns + "maximum_curvature");
p.end_pose_curvature_threshold =
node->declare_parameter<double>(ns + "end_pose_curvature_threshold");
p.maximum_longitudinal_deviation =
node->declare_parameter<double>(ns + "maximum_longitudinal_deviation");
// geometric pull out
p.enable_geometric_pull_out = node->declare_parameter<bool>(ns + "enable_geometric_pull_out");
p.geometric_collision_check_distance_from_end =
node->declare_parameter<double>(ns + "geometric_collision_check_distance_from_end");
p.divide_pull_out_path = node->declare_parameter<bool>(ns + "divide_pull_out_path");
p.parallel_parking_parameters.pull_out_velocity =
node->declare_parameter<double>(ns + "geometric_pull_out_velocity");
p.parallel_parking_parameters.pull_out_arc_path_interval =
node->declare_parameter<double>(ns + "arc_path_interval");
p.parallel_parking_parameters.pull_out_lane_departure_margin =
node->declare_parameter<double>(ns + "lane_departure_margin");
p.lane_departure_check_expansion_margin =
node->declare_parameter<double>(ns + "lane_departure_check_expansion_margin");
p.parallel_parking_parameters.pull_out_max_steer_angle =
node->declare_parameter<double>(ns + "pull_out_max_steer_angle"); // 15deg
p.parallel_parking_parameters.center_line_path_interval =
p.center_line_path_interval; // for geometric parallel parking
// search start pose backward
p.search_priority = node->declare_parameter<std::string>(
ns + "search_priority"); // "efficient_path" or "short_back_distance"
p.enable_back = node->declare_parameter<bool>(ns + "enable_back");
p.backward_velocity = node->declare_parameter<double>(ns + "backward_velocity");
p.max_back_distance = node->declare_parameter<double>(ns + "max_back_distance");
p.backward_search_resolution = node->declare_parameter<double>(ns + "backward_search_resolution");
p.backward_path_update_duration =
node->declare_parameter<double>(ns + "backward_path_update_duration");
p.ignore_distance_from_lane_end =
node->declare_parameter<double>(ns + "ignore_distance_from_lane_end");
// freespace planner general params
{
const std::string ns = "start_planner.freespace_planner.";
Expand Down Expand Up @@ -184,19 +192,10 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
p.rrt_star_parameters.margin = node->declare_parameter<double>(ns + "margin");
}

// stop condition
{
p.maximum_deceleration_for_stop =
node->declare_parameter<double>(ns + "stop_condition.maximum_deceleration_for_stop");
p.maximum_jerk_for_stop =
node->declare_parameter<double>(ns + "stop_condition.maximum_jerk_for_stop");
}

const std::string base_ns = "start_planner.path_safety_check.";

// EgoPredictedPath
const std::string ego_path_ns = base_ns + "ego_predicted_path.";
{
const std::string ego_path_ns = base_ns + "ego_predicted_path.";
p.ego_predicted_path_params.min_velocity =
node->declare_parameter<double>(ego_path_ns + "min_velocity");
p.ego_predicted_path_params.acceleration =
Expand All @@ -210,7 +209,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
p.ego_predicted_path_params.delay_until_departure =
node->declare_parameter<double>(ego_path_ns + "delay_until_departure");
}

// ObjectFilteringParams
const std::string obj_filter_ns = base_ns + "target_filtering.";
{
Expand All @@ -235,10 +233,9 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
p.objects_filtering_params.use_predicted_path_outside_lanelet =
node->declare_parameter<bool>(obj_filter_ns + "use_predicted_path_outside_lanelet");
}

// ObjectTypesToCheck
const std::string obj_types_ns = obj_filter_ns + "object_types_to_check.";
{
const std::string obj_types_ns = obj_filter_ns + "object_types_to_check.";
p.objects_filtering_params.object_types_to_check.check_car =
node->declare_parameter<bool>(obj_types_ns + "check_car");
p.objects_filtering_params.object_types_to_check.check_truck =
Expand All @@ -256,10 +253,9 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
p.objects_filtering_params.object_types_to_check.check_pedestrian =
node->declare_parameter<bool>(obj_types_ns + "check_pedestrian");
}

// ObjectLaneConfiguration
const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration.";
{
const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration.";
p.objects_filtering_params.object_lane_configuration.check_current_lane =
node->declare_parameter<bool>(obj_lane_ns + "check_current_lane");
p.objects_filtering_params.object_lane_configuration.check_right_lane =
Expand All @@ -271,7 +267,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
p.objects_filtering_params.object_lane_configuration.check_other_lane =
node->declare_parameter<bool>(obj_lane_ns + "check_other_lane");
}

// SafetyCheckParams
const std::string safety_check_ns = base_ns + "safety_check_params.";
{
Expand All @@ -288,10 +283,9 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
p.safety_check_params.collision_check_yaw_diff_threshold =
node->declare_parameter<double>(safety_check_ns + "collision_check_yaw_diff_threshold");
}

// RSSparams
const std::string rss_ns = safety_check_ns + "rss_params.";
{
const std::string rss_ns = safety_check_ns + "rss_params.";
p.safety_check_params.rss_params.rear_vehicle_reaction_time =
node->declare_parameter<double>(rss_ns + "rear_vehicle_reaction_time");
p.safety_check_params.rss_params.rear_vehicle_safety_time_margin =
Expand All @@ -305,17 +299,17 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
p.safety_check_params.rss_params.extended_polygon_policy =
node->declare_parameter<std::string>(rss_ns + "extended_polygon_policy");
}

// surround moving obstacle check
std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check.";
{
const std::string surround_moving_obstacle_check_ns =
"start_planner.surround_moving_obstacle_check.";
p.search_radius =
node->declare_parameter<double>(surround_moving_obstacle_check_ns + "search_radius");
p.th_moving_obstacle_velocity = node->declare_parameter<double>(
surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity");
// ObjectTypesToCheck
std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check.";
{
const std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check.";
p.surround_moving_obstacles_type_to_check.check_car =
node->declare_parameter<bool>(obj_types_ns + "check_car");
p.surround_moving_obstacles_type_to_check.check_truck =
Expand All @@ -336,8 +330,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
}

// debug
std::string debug_ns = ns + "debug.";
{
const std::string debug_ns = "start_planner.debug.";
p.print_debug_info = node->declare_parameter<bool>(debug_ns + "print_debug_info");
}

Expand All @@ -361,9 +355,8 @@ void StartPlannerModuleManager::updateModuleParams(

auto & p = parameters_;

const std::string ns = "start_planner.";

{
const std::string ns = "start_planner.";
updateParam<double>(parameters, ns + "th_arrived_distance", p->th_arrived_distance);
updateParam<double>(parameters, ns + "th_stopped_velocity", p->th_stopped_velocity);
updateParam<double>(parameters, ns + "th_stopped_time", p->th_stopped_time);
Expand Down Expand Up @@ -458,6 +451,11 @@ void StartPlannerModuleManager::updateModuleParams(
parameters, ns + "backward_path_update_duration", p->backward_path_update_duration);
updateParam<double>(
parameters, ns + "ignore_distance_from_lane_end", p->ignore_distance_from_lane_end);
updateParam<double>(
parameters, ns + "stop_condition.maximum_deceleration_for_stop",
p->maximum_deceleration_for_stop);
updateParam<double>(
parameters, ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop);
}
{
const std::string ns = "start_planner.freespace_planner.";
Expand Down Expand Up @@ -525,6 +523,7 @@ void StartPlannerModuleManager::updateModuleParams(
updateParam<double>(
parameters, ns + "distance_heuristic_weight", p->astar_parameters.distance_heuristic_weight);
}

{
const std::string ns = "start_planner.freespace_planner.rrtstar.";

Expand All @@ -537,14 +536,6 @@ void StartPlannerModuleManager::updateModuleParams(
updateParam<double>(parameters, ns + "margin", p->rrt_star_parameters.margin);
}

{
updateParam<double>(
parameters, ns + "stop_condition.maximum_deceleration_for_stop",
p->maximum_deceleration_for_stop);
updateParam<double>(
parameters, ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop);
}

const std::string base_ns = "start_planner.path_safety_check.";
const std::string ego_path_ns = base_ns + "ego_predicted_path.";

Expand All @@ -567,7 +558,6 @@ void StartPlannerModuleManager::updateModuleParams(
}

const std::string obj_filter_ns = base_ns + "target_filtering.";

{
updateParam<double>(
parameters, obj_filter_ns + "safety_check_time_horizon",
Expand Down Expand Up @@ -601,9 +591,8 @@ void StartPlannerModuleManager::updateModuleParams(
p->objects_filtering_params.use_predicted_path_outside_lanelet);
}

const std::string obj_types_ns = obj_filter_ns + "object_types_to_check.";

{
const std::string obj_types_ns = obj_filter_ns + "object_types_to_check.";
updateParam<bool>(
parameters, obj_types_ns + "check_car",
p->objects_filtering_params.object_types_to_check.check_car);
Expand All @@ -629,8 +618,8 @@ void StartPlannerModuleManager::updateModuleParams(
parameters, obj_types_ns + "check_pedestrian",
p->objects_filtering_params.object_types_to_check.check_pedestrian);
}
const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration.";

const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration.";
{
updateParam<bool>(
parameters, obj_lane_ns + "check_current_lane",
Expand All @@ -648,6 +637,7 @@ void StartPlannerModuleManager::updateModuleParams(
parameters, obj_lane_ns + "check_other_lane",
p->objects_filtering_params.object_lane_configuration.check_other_lane);
}

const std::string safety_check_ns = base_ns + "safety_check_params.";
{
updateParam<bool>(
Expand All @@ -669,8 +659,9 @@ void StartPlannerModuleManager::updateModuleParams(
parameters, safety_check_ns + "collision_check_yaw_diff_threshold",
p->safety_check_params.collision_check_yaw_diff_threshold);
}
const std::string rss_ns = safety_check_ns + "rss_params.";

{
const std::string rss_ns = safety_check_ns + "rss_params.";
updateParam<double>(
parameters, rss_ns + "rear_vehicle_reaction_time",
p->safety_check_params.rss_params.rear_vehicle_reaction_time);
Expand All @@ -690,17 +681,18 @@ void StartPlannerModuleManager::updateModuleParams(
parameters, rss_ns + "extended_polygon_policy",
p->safety_check_params.rss_params.extended_polygon_policy);
}
std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check.";
{
const std::string surround_moving_obstacle_check_ns =
"start_planner.surround_moving_obstacle_check.";
updateParam<double>(
parameters, surround_moving_obstacle_check_ns + "search_radius", p->search_radius);
updateParam<double>(
parameters, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity",
p->th_moving_obstacle_velocity);

// ObjectTypesToCheck
std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check.";
{
std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check.";
updateParam<bool>(
parameters, obj_types_ns + "check_car",
p->surround_moving_obstacles_type_to_check.check_car);
Expand Down Expand Up @@ -728,8 +720,8 @@ void StartPlannerModuleManager::updateModuleParams(
}
}

std::string debug_ns = ns + "debug.";
{
const std::string debug_ns = "start_planner.debug.";
updateParam<bool>(parameters, debug_ns + "print_debug_info", p->print_debug_info);
}

Expand Down
Loading

0 comments on commit 3e496cc

Please sign in to comment.