Skip to content

Commit afcc1e0

Browse files
committed
Revert "feat(map_based_prediction): use different time horizon (#6877)"
This reverts commit d5aa5d6.
1 parent 0a56e0e commit afcc1e0

File tree

6 files changed

+73
-109
lines changed

6 files changed

+73
-109
lines changed

perception/map_based_prediction/config/map_based_prediction.param.yaml

+1-4
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,7 @@
11
/**:
22
ros__parameters:
33
enable_delay_compensation: true
4-
prediction_time_horizon:
5-
vehicle: 15.0 #[s]
6-
pedestrian: 10.0 #[s]
7-
unknown: 10.0 #[s]
4+
prediction_time_horizon: 10.0 #[s]
85
lateral_control_time_horizon: 5.0 #[s]
96
prediction_sampling_delta_time: 0.5 #[s]
107
min_velocity_for_map_based_prediction: 1.39 #[m/s]

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

+2-9
Original file line numberDiff line numberDiff line change
@@ -99,13 +99,6 @@ struct PredictedRefPath
9999
Maneuver maneuver;
100100
};
101101

102-
struct PredictionTimeHorizon
103-
{
104-
double vehicle;
105-
double pedestrian;
106-
double unknown;
107-
};
108-
109102
using LaneletsData = std::vector<LaneletData>;
110103
using ManeuverProbability = std::unordered_map<Maneuver, float>;
111104
using autoware_auto_mapping_msgs::msg::HADMapBin;
@@ -168,7 +161,7 @@ class MapBasedPredictionNode : public rclcpp::Node
168161

169162
// Parameters
170163
bool enable_delay_compensation_;
171-
PredictionTimeHorizon prediction_time_horizon_;
164+
double prediction_time_horizon_;
172165
double lateral_control_time_horizon_;
173166
double prediction_time_horizon_rate_for_validate_lane_length_;
174167
double prediction_sampling_time_interval_;
@@ -245,7 +238,7 @@ class MapBasedPredictionNode : public rclcpp::Node
245238

246239
std::vector<PredictedRefPath> getPredictedReferencePath(
247240
const TrackedObject & object, const LaneletsData & current_lanelets_data,
248-
const double object_detected_time, const double time_horizon);
241+
const double object_detected_time);
249242
Maneuver predictObjectManeuver(
250243
const TrackedObject & object, const LaneletData & current_lanelet_data,
251244
const double object_detected_time);

perception/map_based_prediction/include/map_based_prediction/path_generator.hpp

+14-17
Original file line numberDiff line numberDiff line change
@@ -80,23 +80,21 @@ using PosePath = std::vector<geometry_msgs::msg::Pose>;
8080
class PathGenerator
8181
{
8282
public:
83-
PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity);
83+
PathGenerator(
84+
const double time_horizon, const double lateral_time_horizon,
85+
const double sampling_time_interval, const double min_crosswalk_user_velocity);
8486

85-
PredictedPath generatePathForNonVehicleObject(
86-
const TrackedObject & object, const double duration);
87+
PredictedPath generatePathForNonVehicleObject(const TrackedObject & object);
8788

88-
PredictedPath generatePathForLowSpeedVehicle(
89-
const TrackedObject & object, const double duration) const;
89+
PredictedPath generatePathForLowSpeedVehicle(const TrackedObject & object) const;
9090

91-
PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object, const double duration);
91+
PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object);
9292

9393
PredictedPath generatePathForOnLaneVehicle(
94-
const TrackedObject & object, const PosePath & ref_paths, const double duration,
95-
const double lateral_duration, const double speed_limit = 0.0);
94+
const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0);
9695

9796
PredictedPath generatePathForCrosswalkUser(
98-
const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk,
99-
const double duration) const;
97+
const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const;
10098

10199
PredictedPath generatePathToTargetPoint(
102100
const TrackedObject & object, const Eigen::Vector2d & point) const;
@@ -113,21 +111,21 @@ class PathGenerator
113111

114112
private:
115113
// Parameters
114+
double time_horizon_;
115+
double lateral_time_horizon_;
116116
double sampling_time_interval_;
117117
double min_crosswalk_user_velocity_;
118118
bool use_vehicle_acceleration_;
119119
double acceleration_exponential_half_life_;
120120

121121
// Member functions
122-
PredictedPath generateStraightPath(const TrackedObject & object, const double duration) const;
122+
PredictedPath generateStraightPath(const TrackedObject & object) const;
123123

124124
PredictedPath generatePolynomialPath(
125-
const TrackedObject & object, const PosePath & ref_path, const double duration,
126-
const double lateral_duration, const double speed_limit = 0.0);
125+
const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0);
127126

128127
FrenetPath generateFrenetPath(
129-
const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length,
130-
const double duration, const double lateral_duration);
128+
const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length);
131129
Eigen::Vector3d calcLatCoefficients(
132130
const FrenetPoint & current_point, const FrenetPoint & target_point, const double T);
133131
Eigen::Vector2d calcLonCoefficients(
@@ -141,8 +139,7 @@ class PathGenerator
141139
const PosePath & ref_path);
142140

143141
FrenetPoint getFrenetPoint(
144-
const TrackedObject & object, const PosePath & ref_path, const double duration,
145-
const double speed_limit = 0.0);
142+
const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0);
146143
};
147144
} // namespace map_based_prediction
148145

perception/map_based_prediction/schema/map_based_prediction.schema.json

+3-17
Original file line numberDiff line numberDiff line change
@@ -12,23 +12,9 @@
1212
"description": "flag to enable the time delay compensation for the position of the object"
1313
},
1414
"prediction_time_horizon": {
15-
"properties": {
16-
"vehicle": {
17-
"type": "number",
18-
"default": "15.0",
19-
"description": "predict time duration for predicted path of vehicle"
20-
},
21-
"pedestrian": {
22-
"type": "number",
23-
"default": "10.0",
24-
"description": "predict time duration for predicted path of pedestrian"
25-
},
26-
"unknown": {
27-
"type": "number",
28-
"default": "10.0",
29-
"description": "predict time duration for predicted path of unknown"
30-
}
31-
}
15+
"type": "number",
16+
"default": "10.0",
17+
"description": "predict time duration for predicted path"
3218
},
3319
"lateral_control_time_horizon": {
3420
"type": "number",

perception/map_based_prediction/src/map_based_prediction_node.cpp

+26-31
Original file line numberDiff line numberDiff line change
@@ -720,10 +720,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
720720
google::InstallFailureSignalHandler();
721721
}
722722
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");
727724
lateral_control_time_horizon_ =
728725
declare_parameter<double>("lateral_control_time_horizon"); // [s] for lateral control point
729726
prediction_sampling_time_interval_ = declare_parameter<double>("prediction_sampling_delta_time");
@@ -794,7 +791,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
794791
"crosswalk_with_signal.timeout_set_for_no_intention_to_walk");
795792

796793
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_);
798796

799797
path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_);
800798
path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_);
@@ -971,8 +969,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
971969

972970
// For off lane obstacles
973971
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);
976974
predicted_path.confidence = 1.0;
977975
if (predicted_path.path.empty()) break;
978976

@@ -987,8 +985,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
987985
transformed_object.kinematics.twist_with_covariance.twist.linear.x,
988986
transformed_object.kinematics.twist_with_covariance.twist.linear.y);
989987
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);
992990
predicted_path.confidence = 1.0;
993991
if (predicted_path.path.empty()) break;
994992

@@ -1000,14 +998,13 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
1000998

1001999
// Get Predicted Reference Path for Each Maneuver and current lanelets
10021000
// 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);
10061003

10071004
// If predicted reference path is empty, assume this object is out of the lane
10081005
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);
10111008
predicted_path.confidence = 1.0;
10121009
if (predicted_path.path.empty()) break;
10131010

@@ -1042,8 +1039,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
10421039

10431040
for (const auto & ref_path : ref_paths) {
10441041
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);
10471043
if (predicted_path.path.empty()) continue;
10481044

10491045
if (!check_lateral_acceleration_constraints_) {
@@ -1106,8 +1102,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
11061102
}
11071103
default: {
11081104
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);
11111107
predicted_path.confidence = 1.0;
11121108

11131109
predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path);
@@ -1174,8 +1170,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
11741170
{
11751171
auto predicted_object = convertToPredictedObject(object);
11761172
{
1177-
PredictedPath predicted_path =
1178-
path_generator_->generatePathForNonVehicleObject(object, prediction_time_horizon_.pedestrian);
1173+
PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(object);
11791174
predicted_path.confidence = 1.0;
11801175

11811176
predicted_object.kinematics.predicted_paths.push_back(predicted_path);
@@ -1226,7 +1221,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
12261221

12271222
if (hasPotentialToReach(
12281223
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,
12301225
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
12311226
PredictedPath predicted_path =
12321227
path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point);
@@ -1236,7 +1231,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
12361231

12371232
if (hasPotentialToReach(
12381233
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,
12401235
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
12411236
PredictedPath predicted_path =
12421237
path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point);
@@ -1260,27 +1255,27 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
12601255

12611256
const auto reachable_first = hasPotentialToReach(
12621257
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_);
12651260
const auto reachable_second = hasPotentialToReach(
12661261
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_);
12691264

12701265
if (!reachable_first && !reachable_second) {
12711266
continue;
12721267
}
12731268

12741269
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_,
12761271
min_crosswalk_user_velocity_);
12771272

12781273
if (!reachable_crosswalk) {
12791274
continue;
12801275
}
12811276

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());
12841279
predicted_path.confidence = 1.0;
12851280

12861281
if (predicted_path.path.empty()) {
@@ -1618,7 +1613,7 @@ void MapBasedPredictionNode::updateObjectsHistory(
16181613

16191614
std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
16201615
const TrackedObject & object, const LaneletsData & current_lanelets_data,
1621-
const double object_detected_time, const double time_horizon)
1616+
const double object_detected_time)
16221617
{
16231618
const double obj_vel = std::hypot(
16241619
object.kinematics.twist_with_covariance.twist.linear.x,
@@ -1630,7 +1625,7 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
16301625
object.kinematics.acceleration_with_covariance.accel.linear.x,
16311626
object.kinematics.acceleration_with_covariance.accel.linear.y)
16321627
: 0.0;
1633-
const double t_h = time_horizon;
1628+
const double t_h = prediction_time_horizon_;
16341629
const double λ = std::log(2) / acceleration_exponential_half_life_;
16351630

16361631
auto get_search_distance_with_decaying_acc = [&]() -> double {

0 commit comments

Comments
 (0)