Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

chore: sync upstream #878

Merged
merged 146 commits into from
Sep 26, 2023
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
146 commits
Select commit Hold shift + click to select a range
e1c04a6
refactor(goal_planner): remove unused variables (#4966)
kosuke55 Sep 12, 2023
c7b5140
fix(goal_planner): fix occpancy grid initializing check (#4969)
kosuke55 Sep 12, 2023
9402bb5
feat(goal_planner): use only static objects in pull over lanes to pat…
kosuke55 Sep 12, 2023
535537d
fix(tensorrt_yolox): update yolox-s model (#4949)
badai-nguyen Sep 13, 2023
4247a70
feat(tier4_system_rviz_plugin): add package (#4945)
kminoda Sep 13, 2023
a959ae7
chore(build): only include forward declaration of lanelet stuff in co…
soblin Sep 13, 2023
dd1ec18
fix(lane_change): fix target object filter (#4930)
zulfaqar-azmi-t4 Sep 13, 2023
be5fa16
feat(traffic_light_visualization): update visualization for w/o fine_…
miursh Sep 13, 2023
33f62b4
build(imu_corrector): add missing diagnostic_updater dependency (#4980)
esteve Sep 13, 2023
424879c
chore(multi_object_tracker): fix spell-check errors (#4985)
kminoda Sep 14, 2023
06dd051
chore(lidar_centerpoint): fix spell-check errors (#4986)
kminoda Sep 14, 2023
6e510b4
fix(tier4_perception_launch): camera lidar fusion launch (#4983)
miursh Sep 14, 2023
fc43334
fix(rtc_manager_panel): fix panel chattering (#4988)
satoshi-ota Sep 14, 2023
40d5e47
change WARN to DEBUG (#4979)
miursh Sep 14, 2023
eded1f1
refactor(avoidance): align function style (#4976)
satoshi-ota Sep 14, 2023
0ef1388
chore(build): only include forward declaration of lanelet stuff in pe…
soblin Sep 14, 2023
e4e24c4
feat(ar_tag_based_localizer): add ekf_pose subscriber (#4946)
SakodaShintaro Sep 14, 2023
b2e9ee3
feat(goal_planner): add options of occupancy grid map to use only for…
kosuke55 Sep 14, 2023
ca6c46c
fix(simple_object_merger): fix timeout (#4990)
scepter914 Sep 14, 2023
ac614b5
feat(object_merger): add queue size parameter (#4994)
scepter914 Sep 14, 2023
8d639b0
fix(resample): remove duplicated poinnt before resampling (#4965)
TakaHoribe Sep 14, 2023
ed0da32
fix(lidar_apollo_segmentation_tvm): add include tier4_autoware_utils …
lexavtanke Sep 14, 2023
b5db5bc
fix(adaptive_cruise_planner): prevent large vibration of acceleration…
tkimura4 Sep 15, 2023
7ad98f3
feat: support transverse mercator projection (#4883)
kminoda Sep 15, 2023
229ce22
refactor(safety_check): check if the object is oncoming (#4997)
satoshi-ota Sep 15, 2023
afe8990
fix(rviz config): remove initial_pose_button_panel (#4959)
YamatoAndo Sep 15, 2023
fd7db70
chore(build): only include forward declaration of lanelet stuff in ro…
soblin Sep 15, 2023
2776b57
feat(behavior_path_planner): extend geometric parallel parking featur…
TakumIto Sep 15, 2023
15d272a
feat(tier4_autoware_utils): add function to print backtrace in runtim…
TakaHoribe Sep 15, 2023
5b40aa5
fix(behavior_path_planner): fix condition when collision detected bef…
kyoichi-sugahara Sep 15, 2023
3ae2018
chore(perception_reproducer): load only rosbag file (#4991)
TakaHoribe Sep 16, 2023
67eae01
fix(behavior_path_planner): define hysteresis_factor_expand_rate (#5002)
kyoichi-sugahara Sep 16, 2023
ea9d4b0
fix(behaivor_path_planner): fix uninitialized variables (#5009)
kosuke55 Sep 16, 2023
38bbc67
feat(behavior_path_planner): update function of createPredictedPath (…
kyoichi-sugahara Sep 18, 2023
ee7a97e
fix(goal_planner): fix disabling safety check (#5020)
kosuke55 Sep 18, 2023
498dbef
feat(behavior_path_planner): use common function of createPredictedPa…
kyoichi-sugahara Sep 18, 2023
8eeb11d
refactor(perception): rearrange clustering pipeline (#4999)
badai-nguyen Sep 19, 2023
f492916
refactor(behavior_path_planner): use util function convertToPredicted…
kyoichi-sugahara Sep 19, 2023
7eabe35
fix(lanelet2_projector): fix werror return-type (#5019)
kosuke55 Sep 19, 2023
1506a94
refactor(avoidance): generate drivable lanes in utils (#5018)
satoshi-ota Sep 19, 2023
31dbd2e
fix(radar_fusion_to_detected_object): fix confidence (#4992)
scepter914 Sep 19, 2023
51fa0f5
fix(radar_tracks_msg_converter): fix twist coordinate conversion (#4993)
scepter914 Sep 19, 2023
cca5fee
fix(tier4_perception_launch): add object_merger of far_objects to fus…
scepter914 Sep 19, 2023
7080d7e
fix(mission_planner/behavior_path_planner): use transient local for m…
kosuke55 Sep 19, 2023
c33fc1d
feat(avoidance): support s-shape avoidance (#4974)
satoshi-ota Sep 19, 2023
2067a6c
chore(build): simplify common/tier4_autoware_utils header (#5008)
soblin Sep 19, 2023
e8f7233
chore: fix spell-check errors in apollo-related packages (#4987)
kminoda Sep 19, 2023
6790c2c
fix(obstacle_cruise_planner): fix backward virtual wall (#5028)
kosuke55 Sep 19, 2023
a66e40f
refactor(yabloc_pose_initializer): use cpp DNN module instead of pyth…
KYabuuchi Sep 19, 2023
a4d12dc
fix(obstacle_cruise_planner): fix stop against objects after backward…
kosuke55 Sep 19, 2023
aae343b
fix(pull_out): add empty check (#5041)
tkimura4 Sep 19, 2023
5519131
feat(behavior_path_planner): account for delay before departure in pr…
kyoichi-sugahara Sep 20, 2023
f1b3538
refactor(avoidance): clean up markers (#4975)
satoshi-ota Sep 20, 2023
33dcea4
chore(radar_object_tracker): fix spell-check error (#5024)
kminoda Sep 20, 2023
fa96a32
fix(behavior_path_planner): fix Werror=maybe-uninitialized (#5039)
kosuke55 Sep 20, 2023
909f12f
refactor(behavior_path_planner): make object type filter method commo…
zulfaqar-azmi-t4 Sep 20, 2023
cddc8f2
feat(ekf_localizer): add diagnostics (#4914)
YamatoAndo Sep 20, 2023
0f441c1
feat(avoidance): insert return dead line point (#5029)
satoshi-ota Sep 20, 2023
c7525bb
feat(ndt_scan_matcher): added logging initial pose (#5045)
SakodaShintaro Sep 20, 2023
9f4ccf6
feat(ekf_localizer): ignore dead band of velocity sensor (#5042)
kminoda Sep 20, 2023
a5cbb3a
feat(autoware_auto_msgs_adapter): add perception adapter (#4555)
beginningfan Sep 20, 2023
33463ce
chore: add maintainer in localization_error_monitor and gnss_poser (#…
kminoda Sep 20, 2023
ac9521d
chore: update CODEOWNERS (#4862)
awf-autoware-bot[bot] Sep 20, 2023
a5feaba
build(tensorrt_yolo): remove download logic from CMake (#3142)
esteve Sep 20, 2023
32a3d86
fix(ndt_scan_matcher): pose_array_interpolator (#5053)
YamatoAndo Sep 20, 2023
bbb5271
build(image_projection_based_fusion): remove downloading logic from C…
lexavtanke Sep 20, 2023
2476395
build(traffic_light_ssd_fine_detector): remove download logic from CM…
esteve Sep 20, 2023
81983eb
refactor(common): extern template for motion_utils / remove tier4_aut…
soblin Sep 20, 2023
47049dc
build(lidar_centerpoint): remove download logic from CMake (#3141)
esteve Sep 20, 2023
27220a8
chore(planning_validator): add maintainer (#5050)
TakaHoribe Sep 20, 2023
43fbcf7
fix(operation_mode_transition_manager): check trajectory_follower_cmd…
TakaHoribe Sep 20, 2023
607b8e7
build(lidar_apollo_instance_segmentation): remove download logic from…
esteve Sep 20, 2023
e68e6e2
build(traffic_light_fine_detector): remove download logic from CMake …
lexavtanke Sep 20, 2023
326868b
fix(avoidance): exit if there is no avoidable object (#5051)
satoshi-ota Sep 21, 2023
d2c2de2
fix(object_merger): fixed sync_queue_size parameter (#5058)
FranzAlbers Sep 21, 2023
057c2d5
chore: update CODEOWNERS (#5059)
awf-autoware-bot[bot] Sep 21, 2023
e942c2d
refactor(control_performance_analysis): rework parameters (#4730)
yuntianyi-chen Sep 21, 2023
89b16e8
fix(avoidance): fix candidate path trimming process (#5022)
satoshi-ota Sep 21, 2023
08122d9
refactor(mission_planner): rework parameters (#4579)
lchojnack Sep 21, 2023
c4a7e54
fix(planning_validator): improve curvature calculation for less noise…
TakaHoribe Sep 21, 2023
7956761
fix(avoidance): fix bug in backward length calculation (#5046)
satoshi-ota Sep 21, 2023
08c809c
fix(mpc): precise resampling around ego for stabling control (#5003)
takayuki5168 Sep 21, 2023
a84418a
feat(vehicle_cmd_gate)!: add steer and steer_rate filter (#5044)
TakaHoribe Sep 21, 2023
ffcbae0
fix(concatenate_data): replace data empty check to avoid nullptr (#5011)
amadeuszsz Sep 21, 2023
55fba78
feat(start_planner): prevent start planner execution in the middle of…
danielsanchezaran Sep 21, 2023
9991b9a
refactor(behavior_path_planner): generarize generateFeasibleStopPath …
kyoichi-sugahara Sep 21, 2023
18ad744
refactor(behavior_path_planner): visualize debug information (#5061)
kyoichi-sugahara Sep 21, 2023
4dd6960
build(tensorrt_yolox): remove download logic from CMake (#3143)
esteve Sep 21, 2023
3fd7b7c
fix(ekf_localizer): fix bug in #5054 (#5066)
kminoda Sep 21, 2023
e0bcedc
fix(mission_planner): routing api uninitialization (#4942)
TakaHoribe Sep 21, 2023
b87bfac
feat(behavior_velocity_planner): enable dynamic load/unload modules (…
TakaHoribe Sep 21, 2023
68e64ac
refactor(traffic_light_classifier): update training docs (#4917)
kaancolak Sep 21, 2023
cd2cd52
refactor(behavior_path_planner): uninitialized values in createPredic…
zulfaqar-azmi-t4 Sep 21, 2023
1a5aa69
refactor(behavior_velocity_planner_common): simplify header files (#5…
soblin Sep 21, 2023
2aa9c7b
build(traffic_light_classifier): remove download logic from CMake (#3…
esteve Sep 21, 2023
df62286
fix(image_projection_based_fusion): fix unexpected matches between ro…
yukke42 Sep 22, 2023
4c9146e
fix(euclidean_cluster): print warning on empty cloud, prevent log spa…
VRichardJP Sep 22, 2023
bf03fe3
fix(simple_planning_simulator): fix build error (#5062)
satoshi-ota Sep 22, 2023
f09e1b7
refactor(ndt_scan_matcher): rework parameters (#5030)
TaikiYamada4 Sep 22, 2023
12d8333
feat(lane_change): regulate lane change on crosswalk and intersection…
rej55 Sep 22, 2023
44fb0fd
feat(ar_tag_based_localizer): split the package `ar_tag_based_localiz…
SakodaShintaro Sep 22, 2023
b58c009
docs(ndt_scan_matcher): update readme (#5074)
TaikiYamada4 Sep 22, 2023
d8885f9
fix(tier4_localization_launch): fixed exec_depend (#5075)
SakodaShintaro Sep 22, 2023
fe19de9
build(tier4_target_object_type_rviz_plugin): add missing cv_bridge de…
esteve Sep 22, 2023
1911464
refactor(goal_planner): do not finish goal planner (#5037)
kosuke55 Sep 23, 2023
4991521
chore(geometric_parallel_parking): remove unused params and initializ…
kosuke55 Sep 24, 2023
837699b
feat(start_planner): visualize backward driving path (#5088)
kosuke55 Sep 24, 2023
a4c94cc
fix(start/goal_planner): resample path and make params (#5085)
kosuke55 Sep 24, 2023
1ada567
feat(behavior_path_planner): add public member function to get runnin…
kyoichi-sugahara Sep 24, 2023
ec99b2f
fix(start_planner): fix backward drving path cutting (#5098)
kosuke55 Sep 24, 2023
1276da9
refactor(perception_reproducer): improve computational efficiency (#5…
TakaHoribe Sep 25, 2023
7af40fc
chore(motion_velocity_smoother): add enable curve filtering param (#5…
TakaHoribe Sep 25, 2023
6006146
feat(mission_planner): add param of check_footprint_inside_lanes (#5097)
takayuki5168 Sep 25, 2023
7b1999a
refactor(pid_longitudinal_controller): use common elevetation angle c…
takayuki5168 Sep 25, 2023
460fc46
feat(merge_from_private): make stop_distance_threshold tunable (#5086)
takayuki5168 Sep 25, 2023
617a627
fix(pid_longitudinal_controller): use is_autoware_control_enabled for…
takayuki5168 Sep 25, 2023
a73bbf9
feat(pid_longitudinal_controller): transit state from emergency under…
takayuki5168 Sep 25, 2023
b6446b0
fix(motion_velocity_smoother): change curvature calculation distance …
TakaHoribe Sep 25, 2023
e2123a2
feat(imu_corrector): add gyro_bias estimation in diag (#5054)
kminoda Sep 25, 2023
5b92c71
feat(intersection): consider amber traffic signal for collision detec…
takayuki5168 Sep 25, 2023
367686b
fix(pid_longitudinal_controller): fix the sign of velocity (#5094)
takayuki5168 Sep 25, 2023
7f98ecf
fix(pid_longitudinal_controller): better slope compensation (#5095)
takayuki5168 Sep 25, 2023
092ecea
feat(tier4_planning_rviz_plugin): visualize text of slope angle (#5091)
takayuki5168 Sep 25, 2023
eeab6c0
fix(start_planner): remove inverse orientation points (#5100)
kosuke55 Sep 25, 2023
f7a1b16
feat(obstacle_cruise_planner)!: add ego pose consideration (#5036)
yuki-takagi-66 Sep 25, 2023
71da68c
feat(lane_change): object filter visualization (#5082)
zulfaqar-azmi-t4 Sep 25, 2023
29e67e7
fix(utils): add flag check in drivable bound generation (#5109)
satoshi-ota Sep 25, 2023
770cf17
feat(intersection): modify occlusion distance calculation and publish…
soblin Sep 25, 2023
9e4b4e9
feat(intersection): ensure temporal stop before peeking into oncoming…
soblin Sep 25, 2023
403c4de
feat(logging_level_configure): add rviz plugin to change logging leve…
TakaHoribe Sep 25, 2023
dcbd062
feat(new module): behavior velocity module template (#5073)
danielsanchezaran Sep 25, 2023
46f871f
build(yabloc_pose_initializer): remove downloading logic from CMake (…
lexavtanke Sep 25, 2023
d9ab02a
refactor(controllers): use specific logger name (#5111)
TakaHoribe Sep 26, 2023
231459f
feat(goal_planner): sort goal candidates by weighted distance (#5110)
kosuke55 Sep 26, 2023
443386d
feat: system diagnostic monitor (#4722)
isamu-takagi Sep 26, 2023
16dc535
feat(lane_change): enable lane change in crosswalk/intersection if eg…
rej55 Sep 26, 2023
0153e54
fix(crosswalk): fix module launch condition (#5129)
takayuki5168 Sep 26, 2023
1ca5dc1
fix(lane_change): object filter other lane object (#5083)
zulfaqar-azmi-t4 Sep 26, 2023
32f0547
fix(elastic_band, obstacle_avoidance_planner): fix velocity update (#…
takayuki5168 Sep 26, 2023
72e7855
feat(ar_tag_based_localizer): add diagnostic (#5132)
SakodaShintaro Sep 26, 2023
078be98
refactor(behavior_path_planner): refactor process in updateState (#5103)
kyoichi-sugahara Sep 26, 2023
19ab508
chore: add workaround for tf2 update (#5127)
wep21 Sep 26, 2023
68ee0f2
fix(pointcloud_preprocessor): organize input twist topic (#5125)
kminoda Sep 26, 2023
9a8a043
perf(processing_time/behavior_path_planner): add processing time publ…
Owen-Liuyuxuan Sep 26, 2023
05f8ad2
fix(ntp_monitor): move chronyc command execution to a timer (#4634)
ito-san Sep 26, 2023
28e4213
Merge branch 'beta/v0.13.0' into sync-upstream
TakaHoribe Sep 26, 2023
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
fix(start/goal_planner): resample path and make params (autowarefound…
…ation#5085)

* chore(geometric_parallel_parking): remove unused parames and initialize variables

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix(start/goal_planner): resample path and make params

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Co-authored-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
  • Loading branch information
kosuke55 and kyoichi-sugahara authored Sep 24, 2023
commit a4c94ccc7b02c8aea3a899dd6dbc4cf1ce406945
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
th_arrived_distance: 1.0
th_stopped_velocity: 0.01
th_stopped_time: 2.0 # It must be greater than the state_machine's.
center_line_path_interval: 1.0

# goal search
goal_search:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
collision_check_distance_from_end: 1.0
th_moving_object_velocity: 1.0
th_distance_to_middle_of_the_road: 0.1
center_line_path_interval: 1.0
# shift pull out
enable_shift_pull_out: true
check_shift_path_lane_departure: false
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -118,11 +118,12 @@ Either one is activated when all conditions are met.

## General parameters for goal_planner

| Name | Unit | Type | Description | Default value |
| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ |
| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 |
| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 |
| Name | Unit | Type | Description | Default value |
| :------------------------ | :---- | :----- | :------------------------------------------------- | :------------ |
| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 |
| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 |
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |

## **collision check**

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,7 @@ PullOutPath --o PullOutPlannerBase
| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 |
| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 |
| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 |
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |

## Safety check with static obstacles

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,9 @@ using geometry_msgs::msg::PoseArray;

struct ParallelParkingParameters
{
// common
double center_line_path_interval{0.0};

// forward parking
double after_forward_parking_straight_distance{0.0};
double forward_parking_velocity{0.0};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ struct GoalPlannerParameters
double th_stopped_velocity{0.0};
double th_stopped_time{0.0};
double th_blinker_on_lateral_offset{0.0};
double center_line_path_interval{0.0};

// goal search
std::string search_priority; // "efficient_path" or "close_goal"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,6 @@ class ShiftPullOver : public PullOverPlannerBase
LaneDepartureChecker lane_departure_checker_{};
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_{};

static constexpr double resample_interval_{1.0};
bool left_side_parking_{true};
};
} // namespace behavior_path_planner
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,9 +41,7 @@ class ShiftPullOut : public PullOutPlannerBase

std::vector<PullOutPath> calcPullOutPaths(
const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
const Pose & start_pose, const Pose & goal_pose,
const BehaviorPathPlannerParameters & common_parameter,
const behavior_path_planner::StartPlannerParameters & parameter);
const Pose & start_pose, const Pose & goal_pose);

double calcBeforeShiftedArcLength(
const PathWithLaneId & path, const double target_after_arc_length, const double dr);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ struct StartPlannerParameters
double collision_check_margin{0.0};
double collision_check_distance_from_end{0.0};
double th_moving_object_velocity{0.0};
double center_line_path_interval{0.0};

// shift pull out
bool enable_shift_pull_out{false};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
p.th_stopped_velocity = node->declare_parameter<double>(base_ns + "th_stopped_velocity");
p.th_arrived_distance = node->declare_parameter<double>(base_ns + "th_arrived_distance");
p.th_stopped_time = node->declare_parameter<double>(base_ns + "th_stopped_time");
p.center_line_path_interval =
node->declare_parameter<double>(base_ns + "center_line_path_interval");
}

// goal search
Expand Down Expand Up @@ -121,6 +123,13 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
node->declare_parameter<double>(ns + "after_shift_straight_distance");
}

// parallel parking common
{
const std::string ns = base_ns + "pull_over.parallel_parking.";
p.parallel_parking_parameters.center_line_path_interval =
p.center_line_path_interval; // for geometric parallel parking
}

// forward parallel parking forward
{
const std::string ns = base_ns + "pull_over.parallel_parking.forward.";
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ StartPlannerModuleManager::StartPlannerModuleManager(
p.collision_check_distance_from_end =
node->declare_parameter<double>(ns + "collision_check_distance_from_end");
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 =
Expand All @@ -71,6 +72,8 @@ StartPlannerModuleManager::StartPlannerModuleManager(
node->declare_parameter<double>(ns + "lane_departure_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"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -286,8 +286,9 @@ bool GeometricParallelParking::planPullOut(
s_start, planner_data_->parameters.forward_path_length, road_lanes, goal_pose);
const double s_end = path_end_info.first;
const bool path_terminal_is_goal = path_end_info.second;
PathWithLaneId road_center_line_path =
planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true);
const PathWithLaneId road_center_line_path = utils::resamplePathWithSpline(
planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true),
parameters_.center_line_path_interval);

if (road_center_line_path.points.empty()) {
continue;
Expand Down Expand Up @@ -362,8 +363,10 @@ PathWithLaneId GeometricParallelParking::generateStraightPath(
const Pose current_pose = planner_data_->self_odometry->pose.pose;
const auto current_arc_position = lanelet::utils::getArcCoordinates(road_lanes, current_pose);

auto path = planner_data_->route_handler->getCenterLinePath(
road_lanes, current_arc_position.length, start_arc_position.length, true);
auto path = utils::resamplePathWithSpline(
planner_data_->route_handler->getCenterLinePath(
road_lanes, current_arc_position.length, start_arc_position.length, true),
parameters_.center_line_path_interval);
path.header = planner_data_->route_handler->getRouteHeader();
if (!path.points.empty()) {
path.points.back().point.longitudinal_velocity_mps = 0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,7 @@ boost::optional<PullOverPath> ShiftPullOver::generatePullOverPath(

// generate road lane reference path to shift end
const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline(
generateReferencePath(road_lanes, shift_end_pose), resample_interval_);
generateReferencePath(road_lanes, shift_end_pose), parameters_.center_line_path_interval);

// calculate shift length
const Pose & shift_end_pose_road_lane =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,9 @@ boost::optional<PullOutPath> FreespacePullOut::plan(const Pose & start_pose, con
const bool path_terminal_is_goal = path_end_info.second;

const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end);
last_path = start_planner_utils::combineReferencePath(last_path, road_center_line_path);
last_path = utils::resamplePathWithSpline(
start_planner_utils::combineReferencePath(last_path, road_center_line_path),
parameters_.center_line_path_interval);

const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps;
utils::correctDividedPathVelocity(partial_paths);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -59,8 +59,7 @@ boost::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const P
planner_data_, backward_path_length, std::numeric_limits<double>::max(),
/*forward_only_in_route*/ true);
// find candidate paths
auto pull_out_paths = calcPullOutPaths(
*route_handler, road_lanes, start_pose, goal_pose, common_parameters, parameters_);
auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose);
if (pull_out_paths.empty()) {
return boost::none;
}
Expand Down Expand Up @@ -160,8 +159,7 @@ boost::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const P

std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,
const Pose & start_pose, const Pose & goal_pose,
const BehaviorPathPlannerParameters & common_parameter, const StartPlannerParameters & parameter)
const Pose & start_pose, const Pose & goal_pose)
{
std::vector<PullOutPath> candidate_paths{};

Expand All @@ -170,12 +168,16 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
}

// rename parameter
const double forward_path_length = common_parameter.forward_path_length;
const double backward_path_length = common_parameter.backward_path_length;
const double lateral_jerk = parameter.lateral_jerk;
const double minimum_lateral_acc = parameter.minimum_lateral_acc;
const double maximum_lateral_acc = parameter.maximum_lateral_acc;
const int lateral_acceleration_sampling_num = parameter.lateral_acceleration_sampling_num;
const auto & common_parameters = planner_data_->parameters;
const double forward_path_length = common_parameters.forward_path_length;
const double backward_path_length = common_parameters.backward_path_length;
const double lateral_jerk = parameters_.lateral_jerk;
const double minimum_lateral_acc = parameters_.minimum_lateral_acc;
const double maximum_lateral_acc = parameters_.maximum_lateral_acc;
const double maximum_curvature = parameters_.maximum_curvature;
const double minimum_shift_pull_out_distance = parameters_.minimum_shift_pull_out_distance;
const int lateral_acceleration_sampling_num = parameters_.lateral_acceleration_sampling_num;

// set minimum acc for breaking loop when sampling num is 1
const double acc_resolution = std::max(
std::abs(maximum_lateral_acc - minimum_lateral_acc) / lateral_acceleration_sampling_num,
Expand All @@ -189,9 +191,9 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
const double s_end = path_end_info.first;
const bool path_terminal_is_goal = path_end_info.second;

constexpr double RESAMPLE_INTERVAL = 1.0;
PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline(
route_handler.getCenterLinePath(road_lanes, s_start, s_end), RESAMPLE_INTERVAL);
route_handler.getCenterLinePath(road_lanes, s_start, s_end),
parameters_.center_line_path_interval);

// non_shifted_path for when shift length or pull out distance is too short
const PullOutPath non_shifted_path = std::invoke([&]() {
Expand Down Expand Up @@ -230,8 +232,8 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc);
const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, /* max acc */ 1.0);
const auto pull_out_distance = calcPullOutLongitudinalDistance(
longitudinal_acc, shift_time, shift_length, parameter.maximum_curvature,
parameter.minimum_shift_pull_out_distance);
longitudinal_acc, shift_time, shift_length, maximum_curvature,
minimum_shift_pull_out_distance);
const double terminal_velocity = longitudinal_acc * shift_time;

// clip from ego pose
Expand All @@ -253,7 +255,9 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
std::max(pull_out_distance, pull_out_distance_converted);

// if before_shifted_pull_out_distance is too short, shifting path fails, so add non shifted
if (before_shifted_pull_out_distance < RESAMPLE_INTERVAL && !has_non_shifted_path) {
if (
before_shifted_pull_out_distance < parameters_.center_line_path_interval &&
!has_non_shifted_path) {
candidate_paths.push_back(non_shifted_path);
has_non_shifted_path = true;
continue;
Expand Down