19
19
#include < tier4_v2x_msgs/msg/key_value.hpp>
20
20
21
21
#include < algorithm>
22
+ #include < limits>
22
23
#include < string>
23
24
#include < vector>
24
25
26
+ namespace tier4_autoware_utils
27
+ {
28
+ template <>
29
+ inline geometry_msgs::msg::Pose getPose (
30
+ const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p)
31
+ {
32
+ return p.point .pose ;
33
+ }
34
+ } // namespace tier4_autoware_utils
35
+
25
36
namespace behavior_velocity_planner
26
37
{
27
38
namespace
@@ -182,7 +193,7 @@ SegmentIndexWithOffset findBackwardOffsetSegment(
182
193
const double offset_length)
183
194
{
184
195
double sum_length = 0.0 ;
185
- for (size_t i = base_idx - 1 ; i > 0 ; --i) {
196
+ for (size_t i = base_idx; i > 0 ; --i) {
186
197
const auto p_front = path.points .at (i - 1 );
187
198
const auto p_back = path.points .at (i);
188
199
@@ -399,10 +410,10 @@ bool VirtualTrafficLightModule::modifyPathVelocity(
399
410
return true ;
400
411
}
401
412
402
- // Stop at stop_line if state is timeout
403
- if (isBeforeStopLine () || planner_param_. check_timeout_after_stop_line ) {
413
+ // Stop at stop_line if state is timeout before stop_line
414
+ if (isBeforeStopLine ()) {
404
415
if (isStateTimeout (*virtual_traffic_light_state)) {
405
- RCLCPP_DEBUG (logger_, " state is timeout" );
416
+ RCLCPP_DEBUG (logger_, " state is timeout before stop line " );
406
417
insertStopVelocityAtStopLine (path, stop_reason);
407
418
}
408
419
@@ -413,6 +424,15 @@ bool VirtualTrafficLightModule::modifyPathVelocity(
413
424
// After stop_line
414
425
state_ = State::PASSING;
415
426
427
+ // Check timeout after stop_line if the option is on
428
+ if (
429
+ planner_param_.check_timeout_after_stop_line && isStateTimeout (*virtual_traffic_light_state)) {
430
+ RCLCPP_DEBUG (logger_, " state is timeout after stop line" );
431
+ insertStopVelocityAtStopLine (path, stop_reason);
432
+ updateInfrastructureCommand ();
433
+ return true ;
434
+ }
435
+
416
436
// Stop at stop_line if finalization isn't completed
417
437
if (!virtual_traffic_light_state->is_finalized ) {
418
438
RCLCPP_DEBUG (logger_, " finalization isn't completed" );
@@ -453,10 +473,12 @@ bool VirtualTrafficLightModule::isBeforeStartLine()
453
473
return false ;
454
474
}
455
475
476
+ const double max_dist = std::numeric_limits<double >::max ();
456
477
const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength (
457
- module_data_.path .points , module_data_.head_pose .position , collision->point );
478
+ module_data_.path .points , module_data_.head_pose , collision->point , max_dist,
479
+ planner_param_.max_yaw_deviation_rad );
458
480
459
- return signed_arc_length > 0 ;
481
+ return * signed_arc_length > 0 ;
460
482
}
461
483
462
484
bool VirtualTrafficLightModule::isBeforeStopLine ()
@@ -469,10 +491,12 @@ bool VirtualTrafficLightModule::isBeforeStopLine()
469
491
return false ;
470
492
}
471
493
494
+ const double max_dist = std::numeric_limits<double >::max ();
472
495
const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength (
473
- module_data_.path .points , module_data_.head_pose .position , collision->point );
496
+ module_data_.path .points , module_data_.head_pose , collision->point , max_dist,
497
+ planner_param_.max_yaw_deviation_rad );
474
498
475
- return signed_arc_length > -planner_param_.dead_line_margin ;
499
+ return * signed_arc_length > -planner_param_.dead_line_margin ;
476
500
}
477
501
478
502
bool VirtualTrafficLightModule::isAfterAnyEndLine ()
@@ -484,16 +508,18 @@ bool VirtualTrafficLightModule::isAfterAnyEndLine()
484
508
485
509
const auto collision = findCollision (module_data_.path .points , map_data_.end_lines );
486
510
487
- // Since the module is registered, a collision should be detected usually .
488
- // Therefore if no collision found, vehicle's path is fully after the line.
511
+ // If the goal is set before the end line, collision will not be detected.
512
+ // Therefore if there is no collision, the ego vehicle is assumed to be before the end line.
489
513
if (!collision) {
490
514
return false ;
491
515
}
492
516
517
+ const double max_dist = std::numeric_limits<double >::max ();
493
518
const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength (
494
- module_data_.path .points , module_data_.head_pose .position , collision->point );
519
+ module_data_.path .points , module_data_.head_pose , collision->point , max_dist,
520
+ planner_param_.max_yaw_deviation_rad );
495
521
496
- return signed_arc_length < -planner_param_.dead_line_margin ;
522
+ return * signed_arc_length < -planner_param_.dead_line_margin ;
497
523
}
498
524
499
525
bool VirtualTrafficLightModule::isNearAnyEndLine ()
@@ -504,10 +530,12 @@ bool VirtualTrafficLightModule::isNearAnyEndLine()
504
530
return false ;
505
531
}
506
532
533
+ const double max_dist = std::numeric_limits<double >::max ();
507
534
const auto signed_arc_length = tier4_autoware_utils::calcSignedArcLength (
508
- module_data_.path .points , module_data_.head_pose .position , collision->point );
535
+ module_data_.path .points , module_data_.head_pose , collision->point , max_dist,
536
+ planner_param_.max_yaw_deviation_rad );
509
537
510
- return std::abs (signed_arc_length) < planner_param_.near_line_distance ;
538
+ return std::abs (* signed_arc_length) < planner_param_.near_line_distance ;
511
539
}
512
540
513
541
boost::optional<tier4_v2x_msgs::msg::VirtualTrafficLightState>
0 commit comments