Skip to content

Commit 56dc552

Browse files
anhnv3991anhnv3991
authored
Merge branch 'main' into feature/smooth_ndt_map_update
2 parents 5a87868 + 9530c8a commit 56dc552

File tree

38 files changed

+3174
-1164
lines changed

38 files changed

+3174
-1164
lines changed

‎.github/CODEOWNERS

+4-4
Original file line numberDiff line numberDiff line change
@@ -67,7 +67,7 @@ control/lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier
6767
control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
6868
control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
6969
control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp
70-
control/pid_longitudinal_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
70+
control/pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
7171
control/predicted_path_checker/** berkay@leodrive.ai
7272
control/pure_pursuit/** takamasa.horibe@tier4.jp
7373
control/shift_decider/** takamasa.horibe@tier4.jp
@@ -113,7 +113,7 @@ map/map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuu
113113
map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
114114
map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
115115
map/map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
116-
map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp
116+
map/util/lanelet2_map_preprocessor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
117117
perception/bytetrack/** manato.hirabayashi@tier4.jp
118118
perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
119119
perception/compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp
@@ -159,7 +159,7 @@ planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp kyoichi.suga
159159
planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp
160160
planning/behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
161161
planning/behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
162-
planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
162+
planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
163163
planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
164164
planning/behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
165165
planning/behavior_path_sampling_planner_module/** daniel.sanchez@tier4.jp maxime.clement@tier4.jp
@@ -235,7 +235,7 @@ system/diagnostic_graph_aggregator/** isamu.takagi@tier4.jp
235235
system/dummy_diag_publisher/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp
236236
system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp
237237
system/duplicated_node_checker/** shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp
238-
system/emergency_handler/** makoto.kurihara@tier4.jp
238+
system/emergency_handler/** makoto.kurihara@tier4.jp ryuta.kambe@tier4.jp tetsuhiro.kawaguchi@tier4.jp
239239
system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp
240240
system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp
241241
system/system_error_monitor/** fumihito.ito@tier4.jp

‎launch/tier4_control_launch/launch/control.launch.py

+30-19
Original file line numberDiff line numberDiff line change
@@ -116,23 +116,6 @@ def launch_setup(context, *args, **kwargs):
116116
parameters=[nearest_search_param, lane_departure_checker_param, vehicle_info_param],
117117
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
118118
)
119-
# control validator checker
120-
control_validator_component = ComposableNode(
121-
package="control_validator",
122-
plugin="control_validator::ControlValidator",
123-
name="control_validator",
124-
remappings=[
125-
("~/input/kinematics", "/localization/kinematic_state"),
126-
("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"),
127-
(
128-
"~/input/predicted_trajectory",
129-
"/control/trajectory_follower/lateral/predicted_trajectory",
130-
),
131-
("~/output/validation_status", "~/validation_status"),
132-
],
133-
parameters=[control_validator_param],
134-
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
135-
)
136119

137120
# shift decider
138121
shift_decider_component = ComposableNode(
@@ -351,7 +334,6 @@ def launch_setup(context, *args, **kwargs):
351334
executable=LaunchConfiguration("container_executable"),
352335
composable_node_descriptions=[
353336
controller_component,
354-
control_validator_component,
355337
lane_departure_component,
356338
shift_decider_component,
357339
vehicle_cmd_gate_component,
@@ -360,6 +342,23 @@ def launch_setup(context, *args, **kwargs):
360342
],
361343
)
362344

345+
# control validator checker
346+
control_validator_component = ComposableNode(
347+
package="control_validator",
348+
plugin="control_validator::ControlValidator",
349+
name="control_validator",
350+
remappings=[
351+
("~/input/kinematics", "/localization/kinematic_state"),
352+
("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"),
353+
(
354+
"~/input/predicted_trajectory",
355+
"/control/trajectory_follower/lateral/predicted_trajectory",
356+
),
357+
("~/output/validation_status", "~/validation_status"),
358+
],
359+
parameters=[control_validator_param],
360+
)
361+
363362
group = GroupAction(
364363
[
365364
PushRosNamespace("control"),
@@ -372,7 +371,19 @@ def launch_setup(context, *args, **kwargs):
372371
]
373372
)
374373

375-
return [group]
374+
control_validator_group = GroupAction(
375+
[
376+
PushRosNamespace("control"),
377+
ComposableNodeContainer(
378+
name="control_validator_container",
379+
namespace="",
380+
package="rclcpp_components",
381+
executable=LaunchConfiguration("container_executable"),
382+
composable_node_descriptions=[control_validator_component, glog_component],
383+
),
384+
]
385+
)
386+
return [group, control_validator_group]
376387

377388

378389
def generate_launch_description():

‎launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml

+33-30
Original file line numberDiff line numberDiff line change
@@ -117,36 +117,39 @@
117117

118118
<!-- roi based clustering -->
119119
<group>
120-
<include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml" if="$(var use_roi_based_cluster)">
121-
<arg name="input/camera_info0" value="$(var camera_info0)"/>
122-
<arg name="input/rois0" value="$(var detection_rois0)"/>
123-
<arg name="input/camera_info1" value="$(var camera_info1)"/>
124-
<arg name="input/rois1" value="$(var detection_rois1)"/>
125-
<arg name="input/camera_info2" value="$(var camera_info2)"/>
126-
<arg name="input/rois2" value="$(var detection_rois2)"/>
127-
<arg name="input/camera_info3" value="$(var camera_info3)"/>
128-
<arg name="input/rois3" value="$(var detection_rois3)"/>
129-
<arg name="input/camera_info4" value="$(var camera_info4)"/>
130-
<arg name="input/rois4" value="$(var detection_rois4)"/>
131-
<arg name="input/camera_info5" value="$(var camera_info5)"/>
132-
<arg name="input/rois5" value="$(var detection_rois5)"/>
133-
<arg name="input/camera_info6" value="$(var camera_info6)"/>
134-
<arg name="input/rois6" value="$(var detection_rois6)"/>
135-
<arg name="input/camera_info7" value="$(var camera_info7)"/>
136-
<arg name="input/rois7" value="$(var detection_rois7)"/>
137-
<arg name="input/rois_number" value="$(var image_number)"/>
138-
<arg name="input/image0" value="$(var image_raw0)"/>
139-
<arg name="input/image1" value="$(var image_raw1)"/>
140-
<arg name="input/image2" value="$(var image_raw2)"/>
141-
<arg name="input/image3" value="$(var image_raw3)"/>
142-
<arg name="input/image4" value="$(var image_raw4)"/>
143-
<arg name="input/image5" value="$(var image_raw5)"/>
144-
<arg name="input/image6" value="$(var image_raw6)"/>
145-
<arg name="input/image7" value="$(var image_raw7)"/>
146-
<arg name="input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
147-
<arg name="output_clusters" value="roi_cluster/clusters"/>
148-
<arg name="param_path" value="$(var roi_pointcloud_fusion_param_path)"/>
149-
</include>
120+
<push-ros-namespace namespace="roi_cluster"/>
121+
<group>
122+
<include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml" if="$(var use_roi_based_cluster)">
123+
<arg name="input/camera_info0" value="$(var camera_info0)"/>
124+
<arg name="input/rois0" value="$(var detection_rois0)"/>
125+
<arg name="input/camera_info1" value="$(var camera_info1)"/>
126+
<arg name="input/rois1" value="$(var detection_rois1)"/>
127+
<arg name="input/camera_info2" value="$(var camera_info2)"/>
128+
<arg name="input/rois2" value="$(var detection_rois2)"/>
129+
<arg name="input/camera_info3" value="$(var camera_info3)"/>
130+
<arg name="input/rois3" value="$(var detection_rois3)"/>
131+
<arg name="input/camera_info4" value="$(var camera_info4)"/>
132+
<arg name="input/rois4" value="$(var detection_rois4)"/>
133+
<arg name="input/camera_info5" value="$(var camera_info5)"/>
134+
<arg name="input/rois5" value="$(var detection_rois5)"/>
135+
<arg name="input/camera_info6" value="$(var camera_info6)"/>
136+
<arg name="input/rois6" value="$(var detection_rois6)"/>
137+
<arg name="input/camera_info7" value="$(var camera_info7)"/>
138+
<arg name="input/rois7" value="$(var detection_rois7)"/>
139+
<arg name="input/rois_number" value="$(var image_number)"/>
140+
<arg name="input/image0" value="$(var image_raw0)"/>
141+
<arg name="input/image1" value="$(var image_raw1)"/>
142+
<arg name="input/image2" value="$(var image_raw2)"/>
143+
<arg name="input/image3" value="$(var image_raw3)"/>
144+
<arg name="input/image4" value="$(var image_raw4)"/>
145+
<arg name="input/image5" value="$(var image_raw5)"/>
146+
<arg name="input/image6" value="$(var image_raw6)"/>
147+
<arg name="input/image7" value="$(var image_raw7)"/>
148+
<arg name="input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
149+
<arg name="output_clusters" value="clusters"/>
150+
<arg name="param_path" value="$(var roi_pointcloud_fusion_param_path)"/>
151+
</include>
152+
</group>
150153
</group>
151154

152155
<!-- simple_cluster_merger -->

‎localization/pose_instability_detector/config/pose_instability_detector.param.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
/**:
22
ros__parameters:
3-
interval_sec: 1.0 # [sec]
3+
interval_sec: 0.5 # [sec]
44
threshold_diff_position_x: 1.0 # [m]
55
threshold_diff_position_y: 1.0 # [m]
66
threshold_diff_position_z: 1.0 # [m]

‎localization/pose_instability_detector/schema/pose_instability_detector.schema.json

+1-1
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
"properties": {
99
"interval_sec": {
1010
"type": "number",
11-
"default": 1.0,
11+
"default": 0.5,
1212
"exclusiveMinimum": 0,
1313
"description": "The interval of timer_callback in seconds."
1414
},

‎perception/bytetrack/README.md

+1-1
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ the number of false negatives is expected to decrease by using it.
2525
The paper just says that the 2d tracking algorithm is a simple Kalman filter.
2626
Original codes use the `top-left-corner` and `aspect ratio` and `size` as the state vector.
2727

28-
This is sometimes unstable because the aspectratio can be changed by the occlusion.
28+
This is sometimes unstable because the aspect ratio can be changed by the occlusion.
2929
So, we use the `top-left` and `size` as the state vector.
3030

3131
Kalman filter settings can be controlled by the parameters in `config/bytetrack_node.param.yaml`.

‎perception/bytetrack/lib/include/data_type.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@ typedef Eigen::Matrix<float, 1, 128, Eigen::RowMajor> FEATURE;
5050
typedef Eigen::Matrix<float, Eigen::Dynamic, 128, Eigen::RowMajor> FEATURESS;
5151
// typedef std::vector<FEATURE> FEATURESS;
5252

53-
// Kalmanfilter
53+
// Kalman Filter
5454
// typedef Eigen::Matrix<float, 8, 8, Eigen::RowMajor> KAL_FILTER;
5555
typedef Eigen::Matrix<float, 1, 8, Eigen::RowMajor> KAL_MEAN;
5656
typedef Eigen::Matrix<float, 8, 8, Eigen::RowMajor> KAL_COVA;

‎perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -241,8 +241,10 @@ bool VoxelGridMapLoader::is_in_voxel(
241241
const double dist_x = map->points.at(voxel_index).x - target_point.x;
242242
const double dist_y = map->points.at(voxel_index).y - target_point.y;
243243
const double dist_z = map->points.at(voxel_index).z - target_point.z;
244-
const double sqr_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;
245-
if (sqr_distance < distance_threshold * distance_threshold * downsize_ratio_z_axis_) {
244+
// check if the point is inside the distance threshold voxel
245+
if (
246+
std::abs(dist_x) < distance_threshold && std::abs(dist_y) < distance_threshold &&
247+
std::abs(dist_z) < distance_threshold * downsize_ratio_z_axis_) {
246248
return true;
247249
}
248250
}

‎perception/image_projection_based_fusion/src/fusion_node.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -132,7 +132,7 @@ FusionNode<Msg, ObjType, Msg2D>::FusionNode(
132132
using tier4_autoware_utils::DebugPublisher;
133133
using tier4_autoware_utils::StopWatch;
134134
stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
135-
debug_publisher_ = std::make_unique<DebugPublisher>(this, "image_projection_based_fusion");
135+
debug_publisher_ = std::make_unique<DebugPublisher>(this, get_name());
136136
stop_watch_ptr_->tic("cyclic_time");
137137
stop_watch_ptr_->tic("processing_time");
138138
}

‎perception/probabilistic_occupancy_grid_map/test/test_synchronized_grid_map_fusion_node.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ def generate_test_description():
5757
FindPackageShare(PACKAGE_NAME).find(PACKAGE_NAME)
5858
+ "/launch/synchronized_occupancy_grid_map_fusion.launch.xml"
5959
)
60-
# use default launch arguments and parms
60+
# use default launch arguments and params
6161
launch_args = []
6262
# action to include launch file
6363
test_launch_file = launch.actions.IncludeLaunchDescription(

‎perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp

+9-9
Original file line numberDiff line numberDiff line change
@@ -387,11 +387,11 @@ bool ConstantTurnRateMotionTracker::measureWithPose(
387387
const bool enable_yaw_measurement = trust_yaw_input_ && object_has_orientation;
388388

389389
if (enable_yaw_measurement) {
390-
Eigen::MatrixXd Cyaw = Eigen::MatrixXd::Zero(1, ekf_params_.dim_x);
391-
Cyaw(0, IDX::YAW) = 1;
392-
C_list.push_back(Cyaw);
390+
Eigen::MatrixXd C_yaw = Eigen::MatrixXd::Zero(1, ekf_params_.dim_x);
391+
C_yaw(0, IDX::YAW) = 1;
392+
C_list.push_back(C_yaw);
393393

394-
Eigen::MatrixXd Yyaw = Eigen::MatrixXd::Zero(1, 1);
394+
Eigen::MatrixXd Y_yaw = Eigen::MatrixXd::Zero(1, 1);
395395
const auto yaw = [&] {
396396
auto obj_yaw = tier4_autoware_utils::normalizeRadian(
397397
tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation));
@@ -404,12 +404,12 @@ bool ConstantTurnRateMotionTracker::measureWithPose(
404404
return obj_yaw;
405405
}();
406406

407-
Yyaw << yaw;
408-
Y_list.push_back(Yyaw);
407+
Y_yaw << yaw;
408+
Y_list.push_back(Y_yaw);
409409

410-
Eigen::MatrixXd Ryaw = Eigen::MatrixXd::Zero(1, 1);
411-
Ryaw << ekf_params_.r_cov_yaw;
412-
R_block_list.push_back(Ryaw);
410+
Eigen::MatrixXd R_yaw = Eigen::MatrixXd::Zero(1, 1);
411+
R_yaw << ekf_params_.r_cov_yaw;
412+
R_block_list.push_back(R_yaw);
413413
}
414414

415415
// 3. add linear velocity measurement

‎perception/tracking_object_merger/src/decorative_tracker_merger.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -380,7 +380,7 @@ TrackerState DecorativeTrackerMergerNode::createNewTracker(
380380
const MEASUREMENT_STATE input_index, rclcpp::Time current_time,
381381
const autoware_auto_perception_msgs::msg::TrackedObject & input_object)
382382
{
383-
// check if object id is not included in innner_tracker_objects_
383+
// check if object id is not included in inner_tracker_objects_
384384
for (const auto & object : inner_tracker_objects_) {
385385
if (object.const_uuid_ == input_object.object_id) {
386386
// create new uuid

‎perception/tracking_object_merger/src/utils/utils.cpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -272,11 +272,11 @@ bool objectsHaveSameMotionDirections(const TrackedObject & main_obj, const Track
272272
const auto sub_vx = sub_obj.kinematics.twist_with_covariance.twist.linear.x;
273273
const auto sub_vy = sub_obj.kinematics.twist_with_covariance.twist.linear.y;
274274
// calc velocity direction
275-
const auto main_vyaw = std::atan2(main_vy, main_vx);
276-
const auto sub_vyaw = std::atan2(sub_vy, sub_vx);
275+
const auto main_v_yaw = std::atan2(main_vy, main_vx);
276+
const auto sub_v_yaw = std::atan2(sub_vy, sub_vx);
277277
// get motion yaw angle
278-
const auto main_motion_yaw = main_yaw + main_vyaw;
279-
const auto sub_motion_yaw = sub_yaw + sub_vyaw;
278+
const auto main_motion_yaw = main_yaw + main_v_yaw;
279+
const auto sub_motion_yaw = sub_yaw + sub_v_yaw;
280280
// diff of motion yaw angle
281281
const auto motion_yaw_diff = std::fabs(main_motion_yaw - sub_motion_yaw);
282282
const auto normalized_motion_yaw_diff =

‎planning/.pages

+1
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ nav:
2424
- 'Blind Spot': planning/behavior_velocity_blind_spot_module
2525
- 'Crosswalk': planning/behavior_velocity_crosswalk_module
2626
- 'Detection Area': planning/behavior_velocity_detection_area_module
27+
- 'Dynamic Obstacle Stop': planning/behavior_velocity_dynamic_obstacle_stop_module
2728
- 'Intersection': planning/behavior_velocity_intersection_module
2829
- 'No Drivable Lane': planning/behavior_velocity_no_drivable_lane_module
2930
- 'No Stopping Area': planning/behavior_velocity_no_stopping_area_module

‎planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -155,8 +155,8 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
155155
// expand drivable lanes
156156
std::for_each(
157157
data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
158-
data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes(
159-
lanelet, planner_data_, avoidance_parameters_, false));
158+
data.drivable_lanes.push_back(utils::avoidance::generateExpandedDrivableLanes(
159+
lanelet, planner_data_, avoidance_parameters_));
160160
});
161161

162162
// calc drivable bound

‎planning/behavior_path_avoidance_module/config/avoidance.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -187,6 +187,7 @@
187187
time_horizon_for_rear_object: 10.0 # [s]
188188
delay_until_departure: 0.0 # [s]
189189
# rss parameters
190+
extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path"
190191
expected_front_deceleration: -1.0 # [m/ss]
191192
expected_rear_deceleration: -1.0 # [m/ss]
192193
rear_vehicle_reaction_time: 2.0 # [s]

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -223,6 +223,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
223223
// safety check rss params
224224
{
225225
const std::string ns = "avoidance.safety_check.";
226+
p.rss_params.extended_polygon_policy =
227+
getOrDeclareParameter<std::string>(*node, ns + "extended_polygon_policy");
226228
p.rss_params.longitudinal_distance_min_threshold =
227229
getOrDeclareParameter<double>(*node, ns + "longitudinal_distance_min_threshold");
228230
p.rss_params.longitudinal_velocity_delta_time =

0 commit comments

Comments
 (0)