@@ -279,10 +279,104 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
279
279
280
280
auto p = parameters_;
281
281
282
- const std::string ns = name_ + " ." ;
283
- updateParam<double >(
284
- parameters, ns + " finish_judge_lateral_threshold" , p->finish_judge_lateral_threshold );
282
+ {
283
+ const std::string ns = " lane_change." ;
284
+ updateParam<double >(parameters, ns + " backward_lane_length" , p->backward_lane_length );
285
+ updateParam<double >(parameters, ns + " prepare_duration" , p->lane_change_prepare_duration );
286
+
287
+ updateParam<double >(
288
+ parameters, ns + " backward_length_buffer_for_end_of_lane" ,
289
+ p->backward_length_buffer_for_end_of_lane );
290
+ updateParam<double >(
291
+ parameters, ns + " backward_length_buffer_for_blocking_object" ,
292
+ p->backward_length_buffer_for_blocking_object );
293
+ updateParam<double >(
294
+ parameters, ns + " lane_change_finish_judge_buffer" , p->lane_change_finish_judge_buffer );
295
+
296
+ updateParam<double >(
297
+ parameters, ns + " lane_changing_lateral_jerk" , p->lane_changing_lateral_jerk );
298
+
299
+ updateParam<double >(
300
+ parameters, ns + " minimum_lane_changing_velocity" , p->minimum_lane_changing_velocity );
301
+ updateParam<double >(
302
+ parameters, ns + " prediction_time_resolution" , p->prediction_time_resolution );
303
+
304
+ int longitudinal_acc_sampling_num = 0 ;
305
+ updateParam<int >(
306
+ parameters, ns + " longitudinal_acceleration_sampling_num" , longitudinal_acc_sampling_num);
307
+ if (longitudinal_acc_sampling_num > 0 ) {
308
+ p->longitudinal_acc_sampling_num = longitudinal_acc_sampling_num;
309
+ }
310
+
311
+ int lateral_acc_sampling_num = 0 ;
312
+ updateParam<int >(
313
+ parameters, ns + " lateral_acceleration_sampling_num" , lateral_acc_sampling_num);
314
+ if (lateral_acc_sampling_num > 0 ) {
315
+ p->lateral_acc_sampling_num = lateral_acc_sampling_num;
316
+ }
317
+
318
+ updateParam<double >(
319
+ parameters, ns + " finish_judge_lateral_threshold" , p->finish_judge_lateral_threshold );
320
+ updateParam<bool >(parameters, ns + " publish_debug_marker" , p->publish_debug_marker );
321
+
322
+ // longitudinal acceleration
323
+ updateParam<double >(parameters, ns + " min_longitudinal_acc" , p->min_longitudinal_acc );
324
+ updateParam<double >(parameters, ns + " max_longitudinal_acc" , p->max_longitudinal_acc );
325
+ }
326
+
327
+ {
328
+ const std::string ns = " lane_change.safety_check.lane_expansion." ;
329
+ updateParam<double >(parameters, ns + " left_offset" , p->lane_expansion_left_offset );
330
+ updateParam<double >(parameters, ns + " right_offset" , p->lane_expansion_right_offset );
331
+ }
332
+
333
+ {
334
+ const std::string ns = " lane_change.target_object." ;
335
+ updateParam<bool >(parameters, ns + " car" , p->object_types_to_check .check_car );
336
+ updateParam<bool >(parameters, ns + " truck" , p->object_types_to_check .check_truck );
337
+ updateParam<bool >(parameters, ns + " bus" , p->object_types_to_check .check_bus );
338
+ updateParam<bool >(parameters, ns + " trailer" , p->object_types_to_check .check_trailer );
339
+ updateParam<bool >(parameters, ns + " unknown" , p->object_types_to_check .check_unknown );
340
+ updateParam<bool >(parameters, ns + " bicycle" , p->object_types_to_check .check_bicycle );
341
+ updateParam<bool >(parameters, ns + " motorcycle" , p->object_types_to_check .check_motorcycle );
342
+ updateParam<bool >(parameters, ns + " pedestrian" , p->object_types_to_check .check_pedestrian );
343
+ }
344
+
345
+ {
346
+ const std::string ns = " lane_change.regulation." ;
347
+ updateParam<bool >(parameters, ns + " crosswalk" , p->regulate_on_crosswalk );
348
+ updateParam<bool >(parameters, ns + " intersection" , p->regulate_on_intersection );
349
+ updateParam<bool >(parameters, ns + " traffic_light" , p->regulate_on_traffic_light );
350
+ }
351
+
352
+ {
353
+ const std::string ns = " lane_change.stuck_detection." ;
354
+ updateParam<double >(parameters, ns + " velocity" , p->stop_velocity_threshold );
355
+ updateParam<double >(parameters, ns + " stop_time" , p->stop_time_threshold );
356
+ }
285
357
358
+ {
359
+ const std::string ns = " lane_change.cancel." ;
360
+ bool enable_on_prepare_phase = true ;
361
+ updateParam<bool >(parameters, ns + " enable_on_prepare_phase" , enable_on_prepare_phase);
362
+ if (!enable_on_prepare_phase) {
363
+ RCLCPP_WARN_ONCE (node_->get_logger (), " WARNING! Lane Change cancel function is disabled." );
364
+ p->cancel .enable_on_prepare_phase = enable_on_prepare_phase;
365
+ }
366
+
367
+ bool enable_on_lane_changing_phase = true ;
368
+ updateParam<bool >(
369
+ parameters, ns + " enable_on_lane_changing_phase" , enable_on_lane_changing_phase);
370
+ if (!enable_on_lane_changing_phase) {
371
+ RCLCPP_WARN_ONCE (node_->get_logger (), " WARNING! Lane Change abort function is disabled." );
372
+ p->cancel .enable_on_lane_changing_phase = enable_on_lane_changing_phase;
373
+ }
374
+
375
+ updateParam<double >(parameters, ns + " delta_time" , p->cancel .delta_time );
376
+ updateParam<double >(parameters, ns + " duration" , p->cancel .duration );
377
+ updateParam<double >(parameters, ns + " max_lateral_jerk" , p->cancel .max_lateral_jerk );
378
+ updateParam<double >(parameters, ns + " overhang_tolerance" , p->cancel .overhang_tolerance );
379
+ }
286
380
std::for_each (observers_.begin (), observers_.end (), [&p](const auto & observer) {
287
381
if (!observer.expired ()) observer.lock ()->updateModuleParams (p);
288
382
});
0 commit comments