Skip to content

Commit

Permalink
refactor(behavior_path_planner): planner data parameter initializer f…
Browse files Browse the repository at this point in the history 
…unction (autowarefoundation#8767)

Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin authored Sep 5, 2024
1 parent 8213bdf commit 71b4b8d
Show file tree
Hide file tree
Showing 3 changed files with 75 additions and 74 deletions.
3 changes: 0 additions & 3 deletions ...havior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,9 +147,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
void takeData();
bool isDataReady();

// parameters
BehaviorPathPlannerParameters getCommonParam();

// callback
void onOdometry(const Odometry::ConstSharedPtr msg);
void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg);
Expand Down
72 changes: 1 addition & 71 deletions ...g/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
// data_manager
{
planner_data_ = std::make_shared<PlannerData>();
planner_data_->parameters = getCommonParam();
planner_data_->drivable_area_expansion_parameters.init(*this);
planner_data_->init_parameters(*this);
}

// publisher
Expand Down Expand Up @@ -153,75 +152,6 @@ std::vector<std::string> BehaviorPathPlannerNode::getRunningModules()
return running_modules;
}

BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
{
BehaviorPathPlannerParameters p{};

p.traffic_light_signal_timeout = declare_parameter<double>("traffic_light_signal_timeout");

// vehicle info
const auto vehicle_info = VehicleInfoUtils(*this).getVehicleInfo();
p.vehicle_info = vehicle_info;
p.vehicle_width = vehicle_info.vehicle_width_m;
p.vehicle_length = vehicle_info.vehicle_length_m;
p.wheel_tread = vehicle_info.wheel_tread_m;
p.wheel_base = vehicle_info.wheel_base_m;
p.front_overhang = vehicle_info.front_overhang_m;
p.rear_overhang = vehicle_info.rear_overhang_m;
p.left_over_hang = vehicle_info.left_overhang_m;
p.right_over_hang = vehicle_info.right_overhang_m;
p.base_link2front = vehicle_info.max_longitudinal_offset_m;
p.base_link2rear = p.rear_overhang;

// NOTE: backward_path_length is used not only calculating path length but also calculating the
// size of a drivable area.
// The drivable area has to cover not the base link but the vehicle itself. Therefore
// rear_overhang must be added to backward_path_length. In addition, because of the
// calculation of the drivable area in the autoware_path_optimizer package, the drivable
// area has to be a little longer than the backward_path_length parameter by adding
// min_backward_offset.
constexpr double min_backward_offset = 1.0;
const double backward_offset = vehicle_info.rear_overhang_m + min_backward_offset;

// ROS parameters
p.backward_path_length = declare_parameter<double>("backward_path_length") + backward_offset;
p.forward_path_length = declare_parameter<double>("forward_path_length");

// acceleration parameters
p.min_acc = declare_parameter<double>("normal.min_acc");
p.max_acc = declare_parameter<double>("normal.max_acc");

p.max_vel = declare_parameter<double>("max_vel");
p.backward_length_buffer_for_end_of_pull_over =
declare_parameter<double>("backward_length_buffer_for_end_of_pull_over");
p.backward_length_buffer_for_end_of_pull_out =
declare_parameter<double>("backward_length_buffer_for_end_of_pull_out");

p.minimum_pull_over_length = declare_parameter<double>("minimum_pull_over_length");
p.refine_goal_search_radius_range = declare_parameter<double>("refine_goal_search_radius_range");
p.turn_signal_intersection_search_distance =
declare_parameter<double>("turn_signal_intersection_search_distance");
p.turn_signal_intersection_angle_threshold_deg =
declare_parameter<double>("turn_signal_intersection_angle_threshold_deg");
p.turn_signal_minimum_search_distance =
declare_parameter<double>("turn_signal_minimum_search_distance");
p.turn_signal_search_time = declare_parameter<double>("turn_signal_search_time");
p.turn_signal_shift_length_threshold =
declare_parameter<double>("turn_signal_shift_length_threshold");
p.turn_signal_remaining_shift_length_threshold =
declare_parameter<double>("turn_signal_remaining_shift_length_threshold");
p.turn_signal_on_swerving = declare_parameter<bool>("turn_signal_on_swerving");

p.enable_akima_spline_first = declare_parameter<bool>("enable_akima_spline_first");
p.enable_cog_on_centerline = declare_parameter<bool>("enable_cog_on_centerline");
p.input_path_interval = declare_parameter<double>("input_path_interval");
p.output_path_interval = declare_parameter<double>("output_path_interval");
p.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
p.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");

return p;
}

void BehaviorPathPlannerNode::takeData()
{
// route
Expand Down
74 changes: 74 additions & 0 deletions ...havior_path_planner_common/include/autoware/behavior_path_planner_common/data_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,6 +170,80 @@ struct PlannerData
mutable std::vector<double> drivable_area_expansion_prev_curvatures{};
mutable TurnSignalDecider turn_signal_decider;

void init_parameters(rclcpp::Node & node)
{
parameters.traffic_light_signal_timeout =
node.declare_parameter<double>("traffic_light_signal_timeout");

// vehicle info
const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo();
parameters.vehicle_info = vehicle_info;
parameters.vehicle_width = vehicle_info.vehicle_width_m;
parameters.vehicle_length = vehicle_info.vehicle_length_m;
parameters.wheel_tread = vehicle_info.wheel_tread_m;
parameters.wheel_base = vehicle_info.wheel_base_m;
parameters.front_overhang = vehicle_info.front_overhang_m;
parameters.rear_overhang = vehicle_info.rear_overhang_m;
parameters.left_over_hang = vehicle_info.left_overhang_m;
parameters.right_over_hang = vehicle_info.right_overhang_m;
parameters.base_link2front = vehicle_info.max_longitudinal_offset_m;
parameters.base_link2rear = parameters.rear_overhang;

// NOTE: backward_path_length is used not only calculating path length but also calculating the
// size of a drivable area.
// The drivable area has to cover not the base link but the vehicle itself. Therefore
// rear_overhang must be added to backward_path_length. In addition, because of the
// calculation of the drivable area in the autoware_path_optimizer package, the drivable
// area has to be a little longer than the backward_path_length parameter by adding
// min_backward_offset.
constexpr double min_backward_offset = 1.0;
const double backward_offset = vehicle_info.rear_overhang_m + min_backward_offset;

// ROS parameters
parameters.backward_path_length =
node.declare_parameter<double>("backward_path_length") + backward_offset;
parameters.forward_path_length = node.declare_parameter<double>("forward_path_length");

// acceleration parameters
parameters.min_acc = node.declare_parameter<double>("normal.min_acc");
parameters.max_acc = node.declare_parameter<double>("normal.max_acc");

parameters.max_vel = node.declare_parameter<double>("max_vel");
parameters.backward_length_buffer_for_end_of_pull_over =
node.declare_parameter<double>("backward_length_buffer_for_end_of_pull_over");
parameters.backward_length_buffer_for_end_of_pull_out =
node.declare_parameter<double>("backward_length_buffer_for_end_of_pull_out");

parameters.minimum_pull_over_length =
node.declare_parameter<double>("minimum_pull_over_length");
parameters.refine_goal_search_radius_range =
node.declare_parameter<double>("refine_goal_search_radius_range");
parameters.turn_signal_intersection_search_distance =
node.declare_parameter<double>("turn_signal_intersection_search_distance");
parameters.turn_signal_intersection_angle_threshold_deg =
node.declare_parameter<double>("turn_signal_intersection_angle_threshold_deg");
parameters.turn_signal_minimum_search_distance =
node.declare_parameter<double>("turn_signal_minimum_search_distance");
parameters.turn_signal_search_time = node.declare_parameter<double>("turn_signal_search_time");
parameters.turn_signal_shift_length_threshold =
node.declare_parameter<double>("turn_signal_shift_length_threshold");
parameters.turn_signal_remaining_shift_length_threshold =
node.declare_parameter<double>("turn_signal_remaining_shift_length_threshold");
parameters.turn_signal_on_swerving = node.declare_parameter<bool>("turn_signal_on_swerving");

parameters.enable_akima_spline_first =
node.declare_parameter<bool>("enable_akima_spline_first");
parameters.enable_cog_on_centerline = node.declare_parameter<bool>("enable_cog_on_centerline");
parameters.input_path_interval = node.declare_parameter<double>("input_path_interval");
parameters.output_path_interval = node.declare_parameter<double>("output_path_interval");
parameters.ego_nearest_dist_threshold =
node.declare_parameter<double>("ego_nearest_dist_threshold");
parameters.ego_nearest_yaw_threshold =
node.declare_parameter<double>("ego_nearest_yaw_threshold");

drivable_area_expansion_parameters.init(node);
}

std::pair<TurnSignalInfo, bool> getBehaviorTurnSignalInfo(
const PathWithLaneId & path, const size_t shift_start_idx, const size_t shift_end_idx,
const lanelet::ConstLanelets & current_lanelets, const double current_shift_length,
Expand Down

0 comments on commit 71b4b8d

Please sign in to comment.