Skip to content

Commit

Permalink
group parameters
Browse files Browse the repository at this point in the history
Signed-off-by: mitukou1109 <mitukou1109@gmail.com>
  • Loading branch information
mitukou1109 committed Feb 20, 2025
1 parent 6e1d40b commit 2b8ba7f
Show file tree
Hide file tree
Showing 3 changed files with 22 additions and 18 deletions.
Original file line number Diff line number Diff line change
@@ -1,7 +1,9 @@
/**:
ros__parameters:
planning_hz: 10.0
backward_path_length: 5.0
forward_path_length: 300.0
waypoint_group_separation_threshold: 1.0
waypoint_group_interval_margin_ratio: 10.0
path_length:
backward: 5.0
forward: 300.0
waypoint_group:
separation_threshold: 1.0
interval_margin_ratio: 10.0
Original file line number Diff line number Diff line change
Expand Up @@ -2,14 +2,16 @@ path_generator:
planning_hz:
type: double

backward_path_length:
type: double
path_length:
backward:
type: double

forward_path_length:
type: double
forward:
type: double

waypoint_group_separation_threshold:
type: double
waypoint_group:
separation_threshold:
type: double

waypoint_group_interval_margin_ratio:
type: double
interval_margin_ratio:
type: double
12 changes: 6 additions & 6 deletions planning/autoware_path_generator/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -208,8 +208,8 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
}

const auto lanelets = utils::get_lanelets_within_route(
current_lane, planner_data_, current_pose, params.backward_path_length,
params.forward_path_length);
current_lane, planner_data_, current_pose, params.path_length.backward,
params.path_length.forward);
if (!lanelets) {
return std::nullopt;
}
Expand All @@ -229,9 +229,9 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(

const auto arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose);
const auto s = arc_coordinates.length; // s denotes longitudinal position in Frenet coordinates
const auto s_start = std::max(0., s - params.backward_path_length);
const auto s_start = std::max(0., s - params.path_length.backward);
const auto s_end = [&]() {
auto s_end = s + params.forward_path_length;
auto s_end = s + params.path_length.forward;

if (!utils::get_next_lanelet_within_route(lanelets.back(), planner_data_)) {
s_end = std::min(s_end, lanelet::utils::getLaneletLength2d(lanelets));
Expand Down Expand Up @@ -268,8 +268,8 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
};

const auto waypoint_groups = utils::get_waypoint_groups(
lanelet_sequence, *planner_data_.lanelet_map_ptr, params.waypoint_group_separation_threshold,
params.waypoint_group_interval_margin_ratio);
lanelet_sequence, *planner_data_.lanelet_map_ptr, params.waypoint_group.separation_threshold,
params.waypoint_group.interval_margin_ratio);

auto extended_lanelets = lanelet_sequence.lanelets();
auto s_offset = 0.;
Expand Down

0 comments on commit 2b8ba7f

Please sign in to comment.