@@ -766,7 +766,10 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
766
766
google::InstallFailureSignalHandler ();
767
767
}
768
768
enable_delay_compensation_ = declare_parameter<bool >(" enable_delay_compensation" );
769
- prediction_time_horizon_ = declare_parameter<double >(" prediction_time_horizon" );
769
+ prediction_time_horizon_.vehicle = declare_parameter<double >(" prediction_time_horizon.vehicle" );
770
+ prediction_time_horizon_.pedestrian =
771
+ declare_parameter<double >(" prediction_time_horizon.pedestrian" );
772
+ prediction_time_horizon_.unknown = declare_parameter<double >(" prediction_time_horizon.unknown" );
770
773
lateral_control_time_horizon_ =
771
774
declare_parameter<double >(" lateral_control_time_horizon" ); // [s] for lateral control point
772
775
prediction_sampling_time_interval_ = declare_parameter<double >(" prediction_sampling_delta_time" );
@@ -840,8 +843,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
840
843
" crosswalk_with_signal.timeout_set_for_no_intention_to_walk" );
841
844
842
845
path_generator_ = std::make_shared<PathGenerator>(
843
- prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_,
844
- min_crosswalk_user_velocity_);
846
+ prediction_sampling_time_interval_, min_crosswalk_user_velocity_);
845
847
846
848
path_generator_->setUseVehicleAcceleration (use_vehicle_acceleration_);
847
849
path_generator_->setAccelerationHalfLife (acceleration_exponential_half_life_);
@@ -1050,8 +1052,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
1050
1052
1051
1053
// For off lane obstacles
1052
1054
if (current_lanelets.empty ()) {
1053
- PredictedPath predicted_path =
1054
- path_generator_-> generatePathForOffLaneVehicle ( transformed_object);
1055
+ PredictedPath predicted_path = path_generator_-> generatePathForOffLaneVehicle (
1056
+ transformed_object, prediction_time_horizon_. vehicle );
1055
1057
predicted_path.confidence = 1.0 ;
1056
1058
if (predicted_path.path .empty ()) break ;
1057
1059
@@ -1066,8 +1068,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
1066
1068
transformed_object.kinematics .twist_with_covariance .twist .linear .x ,
1067
1069
transformed_object.kinematics .twist_with_covariance .twist .linear .y );
1068
1070
if (std::fabs (abs_obj_speed) < min_velocity_for_map_based_prediction_) {
1069
- PredictedPath predicted_path =
1070
- path_generator_-> generatePathForLowSpeedVehicle ( transformed_object);
1071
+ PredictedPath predicted_path = path_generator_-> generatePathForLowSpeedVehicle (
1072
+ transformed_object, prediction_time_horizon_. vehicle );
1071
1073
predicted_path.confidence = 1.0 ;
1072
1074
if (predicted_path.path .empty ()) break ;
1073
1075
@@ -1079,13 +1081,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
1079
1081
1080
1082
// Get Predicted Reference Path for Each Maneuver and current lanelets
1081
1083
// return: <probability, paths>
1082
- const auto ref_paths =
1083
- getPredictedReferencePath (transformed_object, current_lanelets, objects_detected_time);
1084
+ const auto ref_paths = getPredictedReferencePath (
1085
+ transformed_object, current_lanelets, objects_detected_time,
1086
+ prediction_time_horizon_.vehicle );
1084
1087
1085
1088
// If predicted reference path is empty, assume this object is out of the lane
1086
1089
if (ref_paths.empty ()) {
1087
- PredictedPath predicted_path =
1088
- path_generator_-> generatePathForLowSpeedVehicle ( transformed_object);
1090
+ PredictedPath predicted_path = path_generator_-> generatePathForLowSpeedVehicle (
1091
+ transformed_object, prediction_time_horizon_. vehicle );
1089
1092
predicted_path.confidence = 1.0 ;
1090
1093
if (predicted_path.path .empty ()) break ;
1091
1094
@@ -1120,7 +1123,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
1120
1123
1121
1124
for (const auto & ref_path : ref_paths) {
1122
1125
PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle (
1123
- yaw_fixed_transformed_object, ref_path.path , ref_path.speed_limit );
1126
+ yaw_fixed_transformed_object, ref_path.path , prediction_time_horizon_.vehicle ,
1127
+ lateral_control_time_horizon_, ref_path.speed_limit );
1124
1128
if (predicted_path.path .empty ()) continue ;
1125
1129
1126
1130
if (!check_lateral_acceleration_constraints_) {
@@ -1183,8 +1187,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
1183
1187
}
1184
1188
default : {
1185
1189
auto predicted_unknown_object = convertToPredictedObject (transformed_object);
1186
- PredictedPath predicted_path =
1187
- path_generator_-> generatePathForNonVehicleObject ( transformed_object);
1190
+ PredictedPath predicted_path = path_generator_-> generatePathForNonVehicleObject (
1191
+ transformed_object, prediction_time_horizon_. unknown );
1188
1192
predicted_path.confidence = 1.0 ;
1189
1193
1190
1194
predicted_unknown_object.kinematics .predicted_paths .push_back (predicted_path);
@@ -1337,7 +1341,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
1337
1341
{
1338
1342
auto predicted_object = convertToPredictedObject (object);
1339
1343
{
1340
- PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject (object);
1344
+ PredictedPath predicted_path =
1345
+ path_generator_->generatePathForNonVehicleObject (object, prediction_time_horizon_.pedestrian );
1341
1346
predicted_path.confidence = 1.0 ;
1342
1347
1343
1348
predicted_object.kinematics .predicted_paths .push_back (predicted_path);
@@ -1388,7 +1393,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
1388
1393
1389
1394
if (hasPotentialToReach (
1390
1395
object, edge_points.front_center_point , edge_points.front_right_point ,
1391
- edge_points.front_left_point , prediction_time_horizon_ * 2.0 ,
1396
+ edge_points.front_left_point , prediction_time_horizon_. pedestrian * 2.0 ,
1392
1397
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
1393
1398
PredictedPath predicted_path =
1394
1399
path_generator_->generatePathToTargetPoint (object, edge_points.front_center_point );
@@ -1398,7 +1403,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
1398
1403
1399
1404
if (hasPotentialToReach (
1400
1405
object, edge_points.back_center_point , edge_points.back_right_point ,
1401
- edge_points.back_left_point , prediction_time_horizon_ * 2.0 ,
1406
+ edge_points.back_left_point , prediction_time_horizon_. pedestrian * 2.0 ,
1402
1407
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
1403
1408
PredictedPath predicted_path =
1404
1409
path_generator_->generatePathToTargetPoint (object, edge_points.back_center_point );
@@ -1422,27 +1427,27 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
1422
1427
1423
1428
const auto reachable_first = hasPotentialToReach (
1424
1429
object, edge_points.front_center_point , edge_points.front_right_point ,
1425
- edge_points.front_left_point , prediction_time_horizon_, min_crosswalk_user_velocity_ ,
1426
- max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
1430
+ edge_points.front_left_point , prediction_time_horizon_. pedestrian ,
1431
+ min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
1427
1432
const auto reachable_second = hasPotentialToReach (
1428
1433
object, edge_points.back_center_point , edge_points.back_right_point ,
1429
- edge_points.back_left_point , prediction_time_horizon_, min_crosswalk_user_velocity_ ,
1430
- max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
1434
+ edge_points.back_left_point , prediction_time_horizon_. pedestrian ,
1435
+ min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
1431
1436
1432
1437
if (!reachable_first && !reachable_second) {
1433
1438
continue ;
1434
1439
}
1435
1440
1436
1441
const auto reachable_crosswalk = isReachableCrosswalkEdgePoints (
1437
- object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_,
1442
+ object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_. pedestrian ,
1438
1443
min_crosswalk_user_velocity_);
1439
1444
1440
1445
if (!reachable_crosswalk) {
1441
1446
continue ;
1442
1447
}
1443
1448
1444
- PredictedPath predicted_path =
1445
- path_generator_-> generatePathForCrosswalkUser ( object, reachable_crosswalk.get ());
1449
+ PredictedPath predicted_path = path_generator_-> generatePathForCrosswalkUser (
1450
+ object, reachable_crosswalk.get (), prediction_time_horizon_. pedestrian );
1446
1451
predicted_path.confidence = 1.0 ;
1447
1452
1448
1453
if (predicted_path.path .empty ()) {
@@ -1740,7 +1745,7 @@ void MapBasedPredictionNode::updateRoadUsersHistory(
1740
1745
1741
1746
std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath (
1742
1747
const TrackedObject & object, const LaneletsData & current_lanelets_data,
1743
- const double object_detected_time)
1748
+ const double object_detected_time, const double time_horizon )
1744
1749
{
1745
1750
const double obj_vel = std::hypot (
1746
1751
object.kinematics .twist_with_covariance .twist .linear .x ,
@@ -1752,7 +1757,7 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
1752
1757
object.kinematics .acceleration_with_covariance .accel .linear .x ,
1753
1758
object.kinematics .acceleration_with_covariance .accel .linear .y )
1754
1759
: 0.0 ;
1755
- const double t_h = prediction_time_horizon_ ;
1760
+ const double t_h = time_horizon ;
1756
1761
const double λ = std::log (2 ) / acceleration_exponential_half_life_;
1757
1762
1758
1763
auto get_search_distance_with_decaying_acc = [&]() -> double {
0 commit comments