@@ -720,10 +720,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
720
720
google::InstallFailureSignalHandler ();
721
721
}
722
722
enable_delay_compensation_ = declare_parameter<bool >(" enable_delay_compensation" );
723
- prediction_time_horizon_.vehicle = declare_parameter<double >(" prediction_time_horizon.vehicle" );
724
- prediction_time_horizon_.pedestrian =
725
- declare_parameter<double >(" prediction_time_horizon.pedestrian" );
726
- prediction_time_horizon_.unknown = declare_parameter<double >(" prediction_time_horizon.unknown" );
723
+ prediction_time_horizon_ = declare_parameter<double >(" prediction_time_horizon" );
727
724
lateral_control_time_horizon_ =
728
725
declare_parameter<double >(" lateral_control_time_horizon" ); // [s] for lateral control point
729
726
prediction_sampling_time_interval_ = declare_parameter<double >(" prediction_sampling_delta_time" );
@@ -794,7 +791,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
794
791
" crosswalk_with_signal.timeout_set_for_no_intention_to_walk" );
795
792
796
793
path_generator_ = std::make_shared<PathGenerator>(
797
- prediction_sampling_time_interval_, min_crosswalk_user_velocity_);
794
+ prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_,
795
+ min_crosswalk_user_velocity_);
798
796
799
797
path_generator_->setUseVehicleAcceleration (use_vehicle_acceleration_);
800
798
path_generator_->setAccelerationHalfLife (acceleration_exponential_half_life_);
@@ -971,8 +969,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
971
969
972
970
// For off lane obstacles
973
971
if (current_lanelets.empty ()) {
974
- PredictedPath predicted_path = path_generator_-> generatePathForOffLaneVehicle (
975
- transformed_object, prediction_time_horizon_. vehicle );
972
+ PredictedPath predicted_path =
973
+ path_generator_-> generatePathForOffLaneVehicle ( transformed_object);
976
974
predicted_path.confidence = 1.0 ;
977
975
if (predicted_path.path .empty ()) break ;
978
976
@@ -987,8 +985,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
987
985
transformed_object.kinematics .twist_with_covariance .twist .linear .x ,
988
986
transformed_object.kinematics .twist_with_covariance .twist .linear .y );
989
987
if (std::fabs (abs_obj_speed) < min_velocity_for_map_based_prediction_) {
990
- PredictedPath predicted_path = path_generator_-> generatePathForLowSpeedVehicle (
991
- transformed_object, prediction_time_horizon_. vehicle );
988
+ PredictedPath predicted_path =
989
+ path_generator_-> generatePathForLowSpeedVehicle ( transformed_object);
992
990
predicted_path.confidence = 1.0 ;
993
991
if (predicted_path.path .empty ()) break ;
994
992
@@ -1000,14 +998,13 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
1000
998
1001
999
// Get Predicted Reference Path for Each Maneuver and current lanelets
1002
1000
// return: <probability, paths>
1003
- const auto ref_paths = getPredictedReferencePath (
1004
- transformed_object, current_lanelets, objects_detected_time,
1005
- prediction_time_horizon_.vehicle );
1001
+ const auto ref_paths =
1002
+ getPredictedReferencePath (transformed_object, current_lanelets, objects_detected_time);
1006
1003
1007
1004
// If predicted reference path is empty, assume this object is out of the lane
1008
1005
if (ref_paths.empty ()) {
1009
- PredictedPath predicted_path = path_generator_-> generatePathForLowSpeedVehicle (
1010
- transformed_object, prediction_time_horizon_. vehicle );
1006
+ PredictedPath predicted_path =
1007
+ path_generator_-> generatePathForLowSpeedVehicle ( transformed_object);
1011
1008
predicted_path.confidence = 1.0 ;
1012
1009
if (predicted_path.path .empty ()) break ;
1013
1010
@@ -1042,8 +1039,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
1042
1039
1043
1040
for (const auto & ref_path : ref_paths) {
1044
1041
PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle (
1045
- yaw_fixed_transformed_object, ref_path.path , prediction_time_horizon_.vehicle ,
1046
- lateral_control_time_horizon_, ref_path.speed_limit );
1042
+ yaw_fixed_transformed_object, ref_path.path , ref_path.speed_limit );
1047
1043
if (predicted_path.path .empty ()) continue ;
1048
1044
1049
1045
if (!check_lateral_acceleration_constraints_) {
@@ -1106,8 +1102,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
1106
1102
}
1107
1103
default : {
1108
1104
auto predicted_unknown_object = convertToPredictedObject (transformed_object);
1109
- PredictedPath predicted_path = path_generator_-> generatePathForNonVehicleObject (
1110
- transformed_object, prediction_time_horizon_. unknown );
1105
+ PredictedPath predicted_path =
1106
+ path_generator_-> generatePathForNonVehicleObject ( transformed_object);
1111
1107
predicted_path.confidence = 1.0 ;
1112
1108
1113
1109
predicted_unknown_object.kinematics .predicted_paths .push_back (predicted_path);
@@ -1174,8 +1170,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
1174
1170
{
1175
1171
auto predicted_object = convertToPredictedObject (object);
1176
1172
{
1177
- PredictedPath predicted_path =
1178
- path_generator_->generatePathForNonVehicleObject (object, prediction_time_horizon_.pedestrian );
1173
+ PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject (object);
1179
1174
predicted_path.confidence = 1.0 ;
1180
1175
1181
1176
predicted_object.kinematics .predicted_paths .push_back (predicted_path);
@@ -1226,7 +1221,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
1226
1221
1227
1222
if (hasPotentialToReach (
1228
1223
object, edge_points.front_center_point , edge_points.front_right_point ,
1229
- edge_points.front_left_point , prediction_time_horizon_. pedestrian * 2.0 ,
1224
+ edge_points.front_left_point , prediction_time_horizon_ * 2.0 ,
1230
1225
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
1231
1226
PredictedPath predicted_path =
1232
1227
path_generator_->generatePathToTargetPoint (object, edge_points.front_center_point );
@@ -1236,7 +1231,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
1236
1231
1237
1232
if (hasPotentialToReach (
1238
1233
object, edge_points.back_center_point , edge_points.back_right_point ,
1239
- edge_points.back_left_point , prediction_time_horizon_. pedestrian * 2.0 ,
1234
+ edge_points.back_left_point , prediction_time_horizon_ * 2.0 ,
1240
1235
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
1241
1236
PredictedPath predicted_path =
1242
1237
path_generator_->generatePathToTargetPoint (object, edge_points.back_center_point );
@@ -1260,27 +1255,27 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
1260
1255
1261
1256
const auto reachable_first = hasPotentialToReach (
1262
1257
object, edge_points.front_center_point , edge_points.front_right_point ,
1263
- edge_points.front_left_point , prediction_time_horizon_. pedestrian ,
1264
- min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
1258
+ edge_points.front_left_point , prediction_time_horizon_, min_crosswalk_user_velocity_ ,
1259
+ max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
1265
1260
const auto reachable_second = hasPotentialToReach (
1266
1261
object, edge_points.back_center_point , edge_points.back_right_point ,
1267
- edge_points.back_left_point , prediction_time_horizon_. pedestrian ,
1268
- min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
1262
+ edge_points.back_left_point , prediction_time_horizon_, min_crosswalk_user_velocity_ ,
1263
+ max_crosswalk_user_delta_yaw_threshold_for_lanelet_);
1269
1264
1270
1265
if (!reachable_first && !reachable_second) {
1271
1266
continue ;
1272
1267
}
1273
1268
1274
1269
const auto reachable_crosswalk = isReachableCrosswalkEdgePoints (
1275
- object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_. pedestrian ,
1270
+ object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_,
1276
1271
min_crosswalk_user_velocity_);
1277
1272
1278
1273
if (!reachable_crosswalk) {
1279
1274
continue ;
1280
1275
}
1281
1276
1282
- PredictedPath predicted_path = path_generator_-> generatePathForCrosswalkUser (
1283
- object, reachable_crosswalk.get (), prediction_time_horizon_. pedestrian );
1277
+ PredictedPath predicted_path =
1278
+ path_generator_-> generatePathForCrosswalkUser ( object, reachable_crosswalk.get ());
1284
1279
predicted_path.confidence = 1.0 ;
1285
1280
1286
1281
if (predicted_path.path .empty ()) {
@@ -1618,7 +1613,7 @@ void MapBasedPredictionNode::updateObjectsHistory(
1618
1613
1619
1614
std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath (
1620
1615
const TrackedObject & object, const LaneletsData & current_lanelets_data,
1621
- const double object_detected_time, const double time_horizon )
1616
+ const double object_detected_time)
1622
1617
{
1623
1618
const double obj_vel = std::hypot (
1624
1619
object.kinematics .twist_with_covariance .twist .linear .x ,
@@ -1630,7 +1625,7 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
1630
1625
object.kinematics .acceleration_with_covariance .accel .linear .x ,
1631
1626
object.kinematics .acceleration_with_covariance .accel .linear .y )
1632
1627
: 0.0 ;
1633
- const double t_h = time_horizon ;
1628
+ const double t_h = prediction_time_horizon_ ;
1634
1629
const double λ = std::log (2 ) / acceleration_exponential_half_life_;
1635
1630
1636
1631
auto get_search_distance_with_decaying_acc = [&]() -> double {
0 commit comments