Skip to content

Commit 3323c21

Browse files
authored
feat(map_based_prediction): use different time horizon (#7157)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 6e5d597 commit 3323c21

File tree

7 files changed

+125
-100
lines changed

7 files changed

+125
-100
lines changed

perception/map_based_prediction/config/map_based_prediction.param.yaml

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

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

+10-2
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,14 @@ struct PredictedRefPath
106106
Maneuver maneuver;
107107
};
108108

109+
struct PredictionTimeHorizon
110+
{
111+
// NOTE(Mamoru Sobue): motorcycle belongs to "vehicle" and bicycle to "pedestrian"
112+
double vehicle;
113+
double pedestrian;
114+
double unknown;
115+
};
116+
109117
using LaneletsData = std::vector<LaneletData>;
110118
using ManeuverProbability = std::unordered_map<Maneuver, float>;
111119
using autoware_auto_mapping_msgs::msg::HADMapBin;
@@ -170,7 +178,7 @@ class MapBasedPredictionNode : public rclcpp::Node
170178

171179
// Parameters
172180
bool enable_delay_compensation_;
173-
double prediction_time_horizon_;
181+
PredictionTimeHorizon prediction_time_horizon_;
174182
double lateral_control_time_horizon_;
175183
double prediction_time_horizon_rate_for_validate_lane_length_;
176184
double prediction_sampling_time_interval_;
@@ -253,7 +261,7 @@ class MapBasedPredictionNode : public rclcpp::Node
253261
const std::string & object_id, std::unordered_map<std::string, TrackedObject> & current_users);
254262
std::vector<PredictedRefPath> getPredictedReferencePath(
255263
const TrackedObject & object, const LaneletsData & current_lanelets_data,
256-
const double object_detected_time);
264+
const double object_detected_time, const double time_horizon);
257265
Maneuver predictObjectManeuver(
258266
const TrackedObject & object, const LaneletData & current_lanelet_data,
259267
const double object_detected_time);

perception/map_based_prediction/include/map_based_prediction/path_generator.hpp

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

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

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

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

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

9698
PredictedPath generatePathForCrosswalkUser(
97-
const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const;
99+
const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk,
100+
const double duration) const;
98101

99102
PredictedPath generatePathToTargetPoint(
100103
const TrackedObject & object, const Eigen::Vector2d & point) const;
@@ -111,22 +114,21 @@ class PathGenerator
111114

112115
private:
113116
// Parameters
114-
double time_horizon_;
115-
double lateral_time_horizon_;
116117
double sampling_time_interval_;
117118
double min_crosswalk_user_velocity_;
118119
bool use_vehicle_acceleration_;
119120
double acceleration_exponential_half_life_;
120121

121122
// Member functions
122-
PredictedPath generateStraightPath(const TrackedObject & object) const;
123+
PredictedPath generateStraightPath(const TrackedObject & object, const double duration) const;
123124

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

127129
FrenetPath generateFrenetPath(
128-
const FrenetPoint & current_point, const FrenetPoint & target_point,
129-
const double max_length) const;
130+
const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length,
131+
const double duration, const double lateral_duration) const;
130132
Eigen::Vector3d calcLatCoefficients(
131133
const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const;
132134
Eigen::Vector2d calcLonCoefficients(
@@ -140,7 +142,8 @@ class PathGenerator
140142
const PosePath & ref_path) const;
141143

142144
FrenetPoint getFrenetPoint(
143-
const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0) const;
145+
const TrackedObject & object, const PosePath & ref_path, const double duration,
146+
const double speed_limit = 0.0) const;
144147
};
145148
} // namespace map_based_prediction
146149

perception/map_based_prediction/schema/map_based_prediction.schema.json

+17-3
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,23 @@
1212
"description": "flag to enable the time delay compensation for the position of the object"
1313
},
1414
"prediction_time_horizon": {
15-
"type": "number",
16-
"default": "10.0",
17-
"description": "predict time duration for predicted path"
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+
}
1832
},
1933
"lateral_control_time_horizon": {
2034
"type": "number",

perception/map_based_prediction/src/map_based_prediction_node.cpp

+31-26
Original file line numberDiff line numberDiff line change
@@ -766,7 +766,10 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
766766
google::InstallFailureSignalHandler();
767767
}
768768
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");
770773
lateral_control_time_horizon_ =
771774
declare_parameter<double>("lateral_control_time_horizon"); // [s] for lateral control point
772775
prediction_sampling_time_interval_ = declare_parameter<double>("prediction_sampling_delta_time");
@@ -840,8 +843,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
840843
"crosswalk_with_signal.timeout_set_for_no_intention_to_walk");
841844

842845
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_);
845847

846848
path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_);
847849
path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_);
@@ -1050,8 +1052,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
10501052

10511053
// For off lane obstacles
10521054
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);
10551057
predicted_path.confidence = 1.0;
10561058
if (predicted_path.path.empty()) break;
10571059

@@ -1066,8 +1068,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
10661068
transformed_object.kinematics.twist_with_covariance.twist.linear.x,
10671069
transformed_object.kinematics.twist_with_covariance.twist.linear.y);
10681070
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);
10711073
predicted_path.confidence = 1.0;
10721074
if (predicted_path.path.empty()) break;
10731075

@@ -1079,13 +1081,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
10791081

10801082
// Get Predicted Reference Path for Each Maneuver and current lanelets
10811083
// 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);
10841087

10851088
// If predicted reference path is empty, assume this object is out of the lane
10861089
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);
10891092
predicted_path.confidence = 1.0;
10901093
if (predicted_path.path.empty()) break;
10911094

@@ -1120,7 +1123,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
11201123

11211124
for (const auto & ref_path : ref_paths) {
11221125
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);
11241128
if (predicted_path.path.empty()) continue;
11251129

11261130
if (!check_lateral_acceleration_constraints_) {
@@ -1183,8 +1187,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
11831187
}
11841188
default: {
11851189
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);
11881192
predicted_path.confidence = 1.0;
11891193

11901194
predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path);
@@ -1337,7 +1341,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
13371341
{
13381342
auto predicted_object = convertToPredictedObject(object);
13391343
{
1340-
PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(object);
1344+
PredictedPath predicted_path =
1345+
path_generator_->generatePathForNonVehicleObject(object, prediction_time_horizon_.pedestrian);
13411346
predicted_path.confidence = 1.0;
13421347

13431348
predicted_object.kinematics.predicted_paths.push_back(predicted_path);
@@ -1388,7 +1393,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
13881393

13891394
if (hasPotentialToReach(
13901395
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,
13921397
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
13931398
PredictedPath predicted_path =
13941399
path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point);
@@ -1398,7 +1403,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
13981403

13991404
if (hasPotentialToReach(
14001405
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,
14021407
min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) {
14031408
PredictedPath predicted_path =
14041409
path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point);
@@ -1422,27 +1427,27 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
14221427

14231428
const auto reachable_first = hasPotentialToReach(
14241429
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_);
14271432
const auto reachable_second = hasPotentialToReach(
14281433
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_);
14311436

14321437
if (!reachable_first && !reachable_second) {
14331438
continue;
14341439
}
14351440

14361441
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,
14381443
min_crosswalk_user_velocity_);
14391444

14401445
if (!reachable_crosswalk) {
14411446
continue;
14421447
}
14431448

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);
14461451
predicted_path.confidence = 1.0;
14471452

14481453
if (predicted_path.path.empty()) {
@@ -1740,7 +1745,7 @@ void MapBasedPredictionNode::updateRoadUsersHistory(
17401745

17411746
std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
17421747
const TrackedObject & object, const LaneletsData & current_lanelets_data,
1743-
const double object_detected_time)
1748+
const double object_detected_time, const double time_horizon)
17441749
{
17451750
const double obj_vel = std::hypot(
17461751
object.kinematics.twist_with_covariance.twist.linear.x,
@@ -1752,7 +1757,7 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
17521757
object.kinematics.acceleration_with_covariance.accel.linear.x,
17531758
object.kinematics.acceleration_with_covariance.accel.linear.y)
17541759
: 0.0;
1755-
const double t_h = prediction_time_horizon_;
1760+
const double t_h = time_horizon;
17561761
const double λ = std::log(2) / acceleration_exponential_half_life_;
17571762

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

0 commit comments

Comments
 (0)