From 69ebedf8971795981864660920e7356f8a8c6601 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 15 Apr 2024 13:25:14 +0900 Subject: [PATCH] feat(map_based_prediction): use different time horizon Signed-off-by: Mamoru Sobue --- .../config/map_based_prediction.param.yaml | 5 +- .../map_based_prediction_node.hpp | 11 +++- .../map_based_prediction/path_generator.hpp | 33 ++++++----- .../schema/map_based_prediction.schema.json | 20 ++++++- .../src/map_based_prediction_node.cpp | 57 ++++++++++-------- .../src/path_generator.cpp | 59 ++++++++++--------- .../test_path_generator.cpp | 39 +++++------- 7 files changed, 124 insertions(+), 100 deletions(-) diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index a1b776bdb6393..a5b7b0ad6be01 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -1,7 +1,10 @@ /**: ros__parameters: enable_delay_compensation: true - prediction_time_horizon: 10.0 #[s] + prediction_time_horizon: + vehicle: 15.0 #[s] + pedestrian: 10.0 #[s] + unknown: 10.0 #[s] lateral_control_time_horizon: 5.0 #[s] prediction_sampling_delta_time: 0.5 #[s] min_velocity_for_map_based_prediction: 1.39 #[m/s] diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 213f18d63ef3a..1a068cd0f2b5c 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -106,6 +106,13 @@ struct PredictedRefPath Maneuver maneuver; }; +struct PredictionTimeHorizon +{ + double vehicle; + double pedestrian; + double unknown; +}; + using LaneletsData = std::vector; using ManeuverProbability = std::unordered_map; using autoware_auto_mapping_msgs::msg::HADMapBin; @@ -170,7 +177,7 @@ class MapBasedPredictionNode : public rclcpp::Node // Parameters bool enable_delay_compensation_; - double prediction_time_horizon_; + PredictionTimeHorizon prediction_time_horizon_; double lateral_control_time_horizon_; double prediction_time_horizon_rate_for_validate_lane_length_; double prediction_sampling_time_interval_; @@ -253,7 +260,7 @@ class MapBasedPredictionNode : public rclcpp::Node const std::string & object_id, std::unordered_map & current_users); std::vector getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, - const double object_detected_time); + const double object_detected_time, const double time_horizon); Maneuver predictObjectManeuver( const TrackedObject & object, const LaneletData & current_lanelet_data, const double object_detected_time); diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 5023051da5e71..3c1a5432d3b08 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -80,21 +80,24 @@ using PosePath = std::vector; class PathGenerator { public: - PathGenerator( - const double time_horizon, const double lateral_time_horizon, - const double sampling_time_interval, const double min_crosswalk_user_velocity); + PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity); - PredictedPath generatePathForNonVehicleObject(const TrackedObject & object) const; + PredictedPath generatePathForNonVehicleObject( + const TrackedObject & object, const double duration) const; - PredictedPath generatePathForLowSpeedVehicle(const TrackedObject & object) const; + PredictedPath generatePathForLowSpeedVehicle( + const TrackedObject & object, const double duration) const; - PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object) const; + PredictedPath generatePathForOffLaneVehicle( + const TrackedObject & object, const double duration) const; PredictedPath generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0) const; + const TrackedObject & object, const PosePath & ref_paths, const double duration, + const double lateral_duration, const double speed_limit = 0.0) const; PredictedPath generatePathForCrosswalkUser( - const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const; + const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk, + const double duration) const; PredictedPath generatePathToTargetPoint( const TrackedObject & object, const Eigen::Vector2d & point) const; @@ -111,22 +114,21 @@ class PathGenerator private: // Parameters - double time_horizon_; - double lateral_time_horizon_; double sampling_time_interval_; double min_crosswalk_user_velocity_; bool use_vehicle_acceleration_; double acceleration_exponential_half_life_; // Member functions - PredictedPath generateStraightPath(const TrackedObject & object) const; + PredictedPath generateStraightPath(const TrackedObject & object, const double duration) const; PredictedPath generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0) const; + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double lateral_duration, const double speed_limit = 0.0) const; FrenetPath generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, - const double max_length) const; + const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length, + const double duration, const double lateral_duration) const; Eigen::Vector3d calcLatCoefficients( const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const; Eigen::Vector2d calcLonCoefficients( @@ -140,7 +142,8 @@ class PathGenerator const PosePath & ref_path) const; FrenetPoint getFrenetPoint( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0) const; + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double speed_limit = 0.0) const; }; } // namespace map_based_prediction diff --git a/perception/map_based_prediction/schema/map_based_prediction.schema.json b/perception/map_based_prediction/schema/map_based_prediction.schema.json index 8ddb122ebb56e..8e5ef9e542f54 100644 --- a/perception/map_based_prediction/schema/map_based_prediction.schema.json +++ b/perception/map_based_prediction/schema/map_based_prediction.schema.json @@ -12,9 +12,23 @@ "description": "flag to enable the time delay compensation for the position of the object" }, "prediction_time_horizon": { - "type": "number", - "default": "10.0", - "description": "predict time duration for predicted path" + "properties": { + "vehicle": { + "type": "number", + "default": "15.0", + "description": "predict time duration for predicted path of vehicle" + }, + "pedestrian": { + "type": "number", + "default": "10.0", + "description": "predict time duration for predicted path of pedestrian" + }, + "unknown": { + "type": "number", + "default": "10.0", + "description": "predict time duration for predicted path of unknown" + } + } }, "lateral_control_time_horizon": { "type": "number", diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index b55ea72402d74..9b527fafb30e9 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -766,7 +766,10 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ google::InstallFailureSignalHandler(); } enable_delay_compensation_ = declare_parameter("enable_delay_compensation"); - prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); + prediction_time_horizon_.vehicle = declare_parameter("prediction_time_horizon.vehicle"); + prediction_time_horizon_.pedestrian = + declare_parameter("prediction_time_horizon.pedestrian"); + prediction_time_horizon_.unknown = declare_parameter("prediction_time_horizon.unknown"); lateral_control_time_horizon_ = declare_parameter("lateral_control_time_horizon"); // [s] for lateral control point prediction_sampling_time_interval_ = declare_parameter("prediction_sampling_delta_time"); @@ -840,8 +843,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ "crosswalk_with_signal.timeout_set_for_no_intention_to_walk"); path_generator_ = std::make_shared( - prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, - min_crosswalk_user_velocity_); + prediction_sampling_time_interval_, min_crosswalk_user_velocity_); path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); @@ -1050,8 +1052,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // For off lane obstacles if (current_lanelets.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForOffLaneVehicle(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForOffLaneVehicle( + transformed_object, prediction_time_horizon_.vehicle); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1066,8 +1068,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt transformed_object.kinematics.twist_with_covariance.twist.linear.x, transformed_object.kinematics.twist_with_covariance.twist.linear.y); if (std::fabs(abs_obj_speed) < min_velocity_for_map_based_prediction_) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle( + transformed_object, prediction_time_horizon_.vehicle); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1079,13 +1081,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Get Predicted Reference Path for Each Maneuver and current lanelets // return: - const auto ref_paths = - getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); + const auto ref_paths = getPredictedReferencePath( + transformed_object, current_lanelets, objects_detected_time, + prediction_time_horizon_.vehicle); // If predicted reference path is empty, assume this object is out of the lane if (ref_paths.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle( + transformed_object, prediction_time_horizon_.vehicle); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1120,7 +1123,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt for (const auto & ref_path : ref_paths) { PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( - yaw_fixed_transformed_object, ref_path.path, ref_path.speed_limit); + yaw_fixed_transformed_object, ref_path.path, prediction_time_horizon_.vehicle, + lateral_control_time_horizon_, ref_path.speed_limit); if (predicted_path.path.empty()) continue; if (!check_lateral_acceleration_constraints_) { @@ -1183,8 +1187,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } default: { auto predicted_unknown_object = convertToPredictedObject(transformed_object); - PredictedPath predicted_path = - path_generator_->generatePathForNonVehicleObject(transformed_object); + PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject( + transformed_object, prediction_time_horizon_.unknown); predicted_path.confidence = 1.0; predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path); @@ -1337,7 +1341,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( { auto predicted_object = convertToPredictedObject(object); { - PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(object); + PredictedPath predicted_path = + path_generator_->generatePathForNonVehicleObject(object, prediction_time_horizon_.pedestrian); predicted_path.confidence = 1.0; predicted_object.kinematics.predicted_paths.push_back(predicted_path); @@ -1388,7 +1393,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( if (hasPotentialToReach( object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_ * 2.0, + edge_points.front_left_point, prediction_time_horizon_.pedestrian * 2.0, min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { PredictedPath predicted_path = path_generator_->generatePathToTargetPoint(object, edge_points.front_center_point); @@ -1398,7 +1403,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( if (hasPotentialToReach( object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_ * 2.0, + edge_points.back_left_point, prediction_time_horizon_.pedestrian * 2.0, min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_)) { PredictedPath predicted_path = path_generator_->generatePathToTargetPoint(object, edge_points.back_center_point); @@ -1422,27 +1427,27 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const auto reachable_first = hasPotentialToReach( object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + edge_points.front_left_point, prediction_time_horizon_.pedestrian, + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_); const auto reachable_second = hasPotentialToReach( object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + edge_points.back_left_point, prediction_time_horizon_.pedestrian, + min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_); if (!reachable_first && !reachable_second) { continue; } const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( - object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_, + object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_.pedestrian, min_crosswalk_user_velocity_); if (!reachable_crosswalk) { continue; } - PredictedPath predicted_path = - path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); + PredictedPath predicted_path = path_generator_->generatePathForCrosswalkUser( + object, reachable_crosswalk.get(), prediction_time_horizon_.pedestrian); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) { @@ -1740,7 +1745,7 @@ void MapBasedPredictionNode::updateRoadUsersHistory( std::vector MapBasedPredictionNode::getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, - const double object_detected_time) + const double object_detected_time, const double time_horizon) { const double obj_vel = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, @@ -1752,7 +1757,7 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( object.kinematics.acceleration_with_covariance.accel.linear.x, object.kinematics.acceleration_with_covariance.accel.linear.y) : 0.0; - const double t_h = prediction_time_horizon_; + const double t_h = time_horizon; const double λ = std::log(2) / acceleration_exponential_half_life_; auto get_search_distance_with_decaying_acc = [&]() -> double { diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 4238859535c8e..838d7b1c8e316 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -23,18 +23,16 @@ namespace map_based_prediction { PathGenerator::PathGenerator( - const double time_horizon, const double lateral_time_horizon, const double sampling_time_interval, - const double min_crosswalk_user_velocity) -: time_horizon_(time_horizon), - lateral_time_horizon_(lateral_time_horizon), - sampling_time_interval_(sampling_time_interval), + const double sampling_time_interval, const double min_crosswalk_user_velocity) +: sampling_time_interval_(sampling_time_interval), min_crosswalk_user_velocity_(min_crosswalk_user_velocity) { } -PredictedPath PathGenerator::generatePathForNonVehicleObject(const TrackedObject & object) const +PredictedPath PathGenerator::generatePathForNonVehicleObject( + const TrackedObject & object, const double duration) const { - return generateStraightPath(object); + return generateStraightPath(object, duration); } PredictedPath PathGenerator::generatePathToTargetPoint( @@ -75,7 +73,8 @@ PredictedPath PathGenerator::generatePathToTargetPoint( } PredictedPath PathGenerator::generatePathForCrosswalkUser( - const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const + const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk, + const double duration) const { PredictedPath predicted_path{}; const double ep = 0.001; @@ -99,7 +98,7 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( const auto entry_to_exit_point_orientation = tier4_autoware_utils::createQuaternionFromYaw( std::atan2(entry_to_exit_point_normalized.y(), entry_to_exit_point_normalized.x())); - for (double dt = 0.0; dt < time_horizon_ + ep; dt += sampling_time_interval_) { + for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) { geometry_msgs::msg::Pose world_frame_pose; if (dt < arrival_time) { world_frame_pose.position.x = @@ -131,39 +130,43 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( return predicted_path; } -PredictedPath PathGenerator::generatePathForLowSpeedVehicle(const TrackedObject & object) const +PredictedPath PathGenerator::generatePathForLowSpeedVehicle( + const TrackedObject & object, const double duration) const { PredictedPath path; path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); const double ep = 0.001; - for (double dt = 0.0; dt < time_horizon_ + ep; dt += sampling_time_interval_) { + for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) { path.path.push_back(object.kinematics.pose_with_covariance.pose); } return path; } -PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & object) const +PredictedPath PathGenerator::generatePathForOffLaneVehicle( + const TrackedObject & object, const double duration) const { - return generateStraightPath(object); + return generateStraightPath(object, duration); } PredictedPath PathGenerator::generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths, const double speed_limit) const + const TrackedObject & object, const PosePath & ref_paths, const double duration, + const double lateral_duration, const double speed_limit) const { if (ref_paths.size() < 2) { - return generateStraightPath(object); + return generateStraightPath(object, duration); } - return generatePolynomialPath(object, ref_paths, speed_limit); + return generatePolynomialPath(object, ref_paths, speed_limit, duration, lateral_duration); } -PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) const +PredictedPath PathGenerator::generateStraightPath( + const TrackedObject & object, const double longitudinal_duration) const { const auto & object_pose = object.kinematics.pose_with_covariance.pose; const auto & object_twist = object.kinematics.twist_with_covariance.twist; constexpr double ep = 0.001; - const double duration = time_horizon_ + ep; + const double duration = longitudinal_duration + ep; PredictedPath path; path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); @@ -178,11 +181,12 @@ PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) } PredictedPath PathGenerator::generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit) const + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double lateral_duration, const double speed_limit) const { // Get current Frenet Point const double ref_path_len = motion_utils::calcArcLength(ref_path); - const auto current_point = getFrenetPoint(object, ref_path, speed_limit); + const auto current_point = getFrenetPoint(object, ref_path, speed_limit, duration); // Step1. Set Target Frenet Point // Note that we do not set position s, @@ -196,13 +200,13 @@ PredictedPath PathGenerator::generatePolynomialPath( // Step2. Generate Predicted Path on a Frenet coordinate const auto frenet_predicted_path = - generateFrenetPath(current_point, terminal_point, ref_path_len); + generateFrenetPath(current_point, terminal_point, ref_path_len, duration, lateral_duration); // Step3. Interpolate Reference Path for converting predicted path coordinate const auto interpolated_ref_path = interpolateReferencePath(ref_path, frenet_predicted_path); if (frenet_predicted_path.size() < 2 || interpolated_ref_path.size() < 2) { - return generateStraightPath(object); + return generateStraightPath(object, duration); } // Step4. Convert predicted trajectory from Frenet to Cartesian coordinate @@ -210,12 +214,10 @@ PredictedPath PathGenerator::generatePolynomialPath( } FrenetPath PathGenerator::generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, - const double max_length) const + const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length, + const double duration, const double lateral_duration) const { FrenetPath path; - const double duration = time_horizon_; - const double lateral_duration = lateral_time_horizon_; // Compute Lateral and Longitudinal Coefficients to generate the trajectory const Eigen::Vector3d lat_coeff = @@ -387,7 +389,8 @@ PredictedPath PathGenerator::convertToPredictedPath( } FrenetPoint PathGenerator::getFrenetPoint( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit) const + const TrackedObject & object, const PosePath & ref_path, const double duration, + const double speed_limit) const { FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; @@ -413,7 +416,7 @@ FrenetPoint PathGenerator::getFrenetPoint( : 0.0; // Using a decaying acceleration model. Consult the README for more information about the model. - const double t_h = time_horizon_; + const double t_h = duration; const float λ = std::log(2) / acceleration_exponential_half_life_; auto have_same_sign = [](double a, double b) -> bool { diff --git a/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp b/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp index 555dc217fb5ed..6134f0c6a25d7 100644 --- a/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp +++ b/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp @@ -52,19 +52,17 @@ TEST(PathGenerator, test_generatePathForNonVehicleObject) { // Generate Path generator const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate pedestrian object TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); // Generate predicted path const PredictedPath predicted_path = - path_generator.generatePathForNonVehicleObject(tracked_object); + path_generator.generatePathForNonVehicleObject(tracked_object, prediction_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -77,19 +75,17 @@ TEST(PathGenerator, test_generatePathForLowSpeedVehicle) { // Generate Path generator const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); // Generate predicted path const PredictedPath predicted_path = - path_generator.generatePathForLowSpeedVehicle(tracked_object); + path_generator.generatePathForLowSpeedVehicle(tracked_object, prediction_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -102,17 +98,16 @@ TEST(PathGenerator, test_generatePathForOffLaneVehicle) { // Generate Path generator const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); - const PredictedPath predicted_path = path_generator.generatePathForOffLaneVehicle(tracked_object); + const PredictedPath predicted_path = + path_generator.generatePathForOffLaneVehicle(tracked_object, prediction_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -129,8 +124,7 @@ TEST(PathGenerator, test_generatePathForOnLaneVehicle) const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); @@ -144,8 +138,8 @@ TEST(PathGenerator, test_generatePathForOnLaneVehicle) ref_paths.push_back(pose); // Generate predicted path - const PredictedPath predicted_path = - path_generator.generatePathForOnLaneVehicle(tracked_object, ref_paths); + const PredictedPath predicted_path = path_generator.generatePathForOnLaneVehicle( + tracked_object, ref_paths, prediction_time_horizon, lateral_control_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -158,12 +152,10 @@ TEST(PathGenerator, test_generatePathForCrosswalkUser) { // Generate Path generator const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); @@ -178,8 +170,8 @@ TEST(PathGenerator, test_generatePathForCrosswalkUser) reachable_crosswalk.back_left_point << -1.0, 1.0; // Generate predicted path - const PredictedPath predicted_path = - path_generator.generatePathForCrosswalkUser(tracked_object, reachable_crosswalk); + const PredictedPath predicted_path = path_generator.generatePathForCrosswalkUser( + tracked_object, reachable_crosswalk, prediction_time_horizon); // Check EXPECT_FALSE(predicted_path.path.empty()); @@ -191,13 +183,10 @@ TEST(PathGenerator, test_generatePathForCrosswalkUser) TEST(PathGenerator, test_generatePathToTargetPoint) { // Generate Path generator - const double prediction_time_horizon = 10.0; - const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, - min_crosswalk_user_velocity); + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR);