From 7c5aa8ca45a05e897b874d480bf028e6e6acf896 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 10 May 2024 14:57:26 +0900 Subject: [PATCH] Revert "feat(map_based_prediction): use different time horizon (#6877)" This reverts commit d5aa5d61c5d346aa97ea12065dd75a6f7b1e6b5c. Signed-off-by: yoshiri --- .../config/map_based_prediction.param.yaml | 5 +- .../map_based_prediction_node.hpp | 11 +--- .../map_based_prediction/path_generator.hpp | 31 +++++----- .../schema/map_based_prediction.schema.json | 20 +------ .../src/map_based_prediction_node.cpp | 57 +++++++++--------- .../src/path_generator.cpp | 58 +++++++++---------- 6 files changed, 73 insertions(+), 109 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 803933ef4dbfd..f8ad371ab92a6 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -1,10 +1,7 @@ /**: ros__parameters: enable_delay_compensation: true - prediction_time_horizon: - vehicle: 15.0 #[s] - pedestrian: 10.0 #[s] - unknown: 10.0 #[s] + prediction_time_horizon: 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 c9d0ac84c4f91..829c4d6e4a114 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 @@ -99,13 +99,6 @@ 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; @@ -168,7 +161,7 @@ class MapBasedPredictionNode : public rclcpp::Node // Parameters bool enable_delay_compensation_; - PredictionTimeHorizon prediction_time_horizon_; + double prediction_time_horizon_; double lateral_control_time_horizon_; double prediction_time_horizon_rate_for_validate_lane_length_; double prediction_sampling_time_interval_; @@ -245,7 +238,7 @@ class MapBasedPredictionNode : public rclcpp::Node std::vector getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, - const double object_detected_time, const double time_horizon); + const double object_detected_time); 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 443a7c671ec83..6dfc4a8db9e20 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,23 +80,21 @@ using PosePath = std::vector; class PathGenerator { public: - PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity); + PathGenerator( + const double time_horizon, const double lateral_time_horizon, + const double sampling_time_interval, const double min_crosswalk_user_velocity); - PredictedPath generatePathForNonVehicleObject( - const TrackedObject & object, const double duration); + PredictedPath generatePathForNonVehicleObject(const TrackedObject & object); - PredictedPath generatePathForLowSpeedVehicle( - const TrackedObject & object, const double duration) const; + PredictedPath generatePathForLowSpeedVehicle(const TrackedObject & object) const; - PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object, const double duration); + PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object); PredictedPath generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths, const double duration, - const double lateral_duration, const double speed_limit = 0.0); + const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0); PredictedPath generatePathForCrosswalkUser( - const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk, - const double duration) const; + const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const; PredictedPath generatePathToTargetPoint( const TrackedObject & object, const Eigen::Vector2d & point) const; @@ -113,21 +111,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 double duration) const; + PredictedPath generateStraightPath(const TrackedObject & object) const; PredictedPath generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path, const double duration, - const double lateral_duration, const double speed_limit = 0.0); + const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0); FrenetPath generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length, - const double duration, const double lateral_duration); + const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length); Eigen::Vector3d calcLatCoefficients( const FrenetPoint & current_point, const FrenetPoint & target_point, const double T); Eigen::Vector2d calcLonCoefficients( @@ -141,8 +139,7 @@ class PathGenerator const PosePath & ref_path); FrenetPoint getFrenetPoint( - const TrackedObject & object, const PosePath & ref_path, const double duration, - const double speed_limit = 0.0); + const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0); }; } // 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 8e5ef9e542f54..8ddb122ebb56e 100644 --- a/perception/map_based_prediction/schema/map_based_prediction.schema.json +++ b/perception/map_based_prediction/schema/map_based_prediction.schema.json @@ -12,23 +12,9 @@ "description": "flag to enable the time delay compensation for the position of the object" }, "prediction_time_horizon": { - "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" - } - } + "type": "number", + "default": "10.0", + "description": "predict time duration for predicted path" }, "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 e717109b94cd7..8f080ba0edc35 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -720,10 +720,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ google::InstallFailureSignalHandler(); } enable_delay_compensation_ = declare_parameter("enable_delay_compensation"); - 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"); + prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); 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"); @@ -794,7 +791,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ "crosswalk_with_signal.timeout_set_for_no_intention_to_walk"); path_generator_ = std::make_shared( - prediction_sampling_time_interval_, min_crosswalk_user_velocity_); + prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, + min_crosswalk_user_velocity_); path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_); path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_); @@ -971,8 +969,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // For off lane obstacles if (current_lanelets.empty()) { - PredictedPath predicted_path = path_generator_->generatePathForOffLaneVehicle( - transformed_object, prediction_time_horizon_.vehicle); + PredictedPath predicted_path = + path_generator_->generatePathForOffLaneVehicle(transformed_object); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -987,8 +985,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, prediction_time_horizon_.vehicle); + PredictedPath predicted_path = + path_generator_->generatePathForLowSpeedVehicle(transformed_object); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1000,14 +998,13 @@ 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, - prediction_time_horizon_.vehicle); + const auto ref_paths = + getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); // 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, prediction_time_horizon_.vehicle); + PredictedPath predicted_path = + path_generator_->generatePathForLowSpeedVehicle(transformed_object); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) break; @@ -1042,8 +1039,7 @@ 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, prediction_time_horizon_.vehicle, - lateral_control_time_horizon_, ref_path.speed_limit); + yaw_fixed_transformed_object, ref_path.path, ref_path.speed_limit); if (predicted_path.path.empty()) continue; if (!check_lateral_acceleration_constraints_) { @@ -1106,8 +1102,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt } default: { auto predicted_unknown_object = convertToPredictedObject(transformed_object); - PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject( - transformed_object, prediction_time_horizon_.unknown); + PredictedPath predicted_path = + path_generator_->generatePathForNonVehicleObject(transformed_object); predicted_path.confidence = 1.0; predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path); @@ -1174,8 +1170,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( { auto predicted_object = convertToPredictedObject(object); { - PredictedPath predicted_path = - path_generator_->generatePathForNonVehicleObject(object, prediction_time_horizon_.pedestrian); + PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(object); predicted_path.confidence = 1.0; predicted_object.kinematics.predicted_paths.push_back(predicted_path); @@ -1226,7 +1221,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_.pedestrian * 2.0, + edge_points.front_left_point, prediction_time_horizon_ * 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); @@ -1236,7 +1231,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_.pedestrian * 2.0, + edge_points.back_left_point, prediction_time_horizon_ * 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); @@ -1260,27 +1255,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_.pedestrian, - min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + edge_points.front_left_point, prediction_time_horizon_, 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_.pedestrian, - min_crosswalk_user_velocity_, max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + edge_points.back_left_point, prediction_time_horizon_, 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_.pedestrian, + object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_, min_crosswalk_user_velocity_); if (!reachable_crosswalk) { continue; } - PredictedPath predicted_path = path_generator_->generatePathForCrosswalkUser( - object, reachable_crosswalk.get(), prediction_time_horizon_.pedestrian); + PredictedPath predicted_path = + path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); predicted_path.confidence = 1.0; if (predicted_path.path.empty()) { @@ -1618,7 +1613,7 @@ void MapBasedPredictionNode::updateObjectsHistory( std::vector MapBasedPredictionNode::getPredictedReferencePath( const TrackedObject & object, const LaneletsData & current_lanelets_data, - const double object_detected_time, const double time_horizon) + const double object_detected_time) { const double obj_vel = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, @@ -1630,7 +1625,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 = time_horizon; + const double t_h = prediction_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 562fa966c40a9..83fbc16feb7fa 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -23,16 +23,18 @@ namespace map_based_prediction { PathGenerator::PathGenerator( - const double sampling_time_interval, const double min_crosswalk_user_velocity) -: sampling_time_interval_(sampling_time_interval), + 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), min_crosswalk_user_velocity_(min_crosswalk_user_velocity) { } -PredictedPath PathGenerator::generatePathForNonVehicleObject( - const TrackedObject & object, const double duration) +PredictedPath PathGenerator::generatePathForNonVehicleObject(const TrackedObject & object) { - return generateStraightPath(object, duration); + return generateStraightPath(object); } PredictedPath PathGenerator::generatePathToTargetPoint( @@ -73,8 +75,7 @@ PredictedPath PathGenerator::generatePathToTargetPoint( } PredictedPath PathGenerator::generatePathForCrosswalkUser( - const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk, - const double duration) const + const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const { PredictedPath predicted_path{}; const double ep = 0.001; @@ -98,7 +99,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 < duration + ep; dt += sampling_time_interval_) { + for (double dt = 0.0; dt < time_horizon_ + ep; dt += sampling_time_interval_) { geometry_msgs::msg::Pose world_frame_pose; if (dt < arrival_time) { world_frame_pose.position.x = @@ -130,43 +131,39 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser( return predicted_path; } -PredictedPath PathGenerator::generatePathForLowSpeedVehicle( - const TrackedObject & object, const double duration) const +PredictedPath PathGenerator::generatePathForLowSpeedVehicle(const TrackedObject & object) const { PredictedPath path; path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); const double ep = 0.001; - for (double dt = 0.0; dt < duration + ep; dt += sampling_time_interval_) { + for (double dt = 0.0; dt < time_horizon_ + ep; dt += sampling_time_interval_) { path.path.push_back(object.kinematics.pose_with_covariance.pose); } return path; } -PredictedPath PathGenerator::generatePathForOffLaneVehicle( - const TrackedObject & object, const double duration) +PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & object) { - return generateStraightPath(object, duration); + return generateStraightPath(object); } PredictedPath PathGenerator::generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths, const double duration, - const double lateral_duration, const double speed_limit) + const TrackedObject & object, const PosePath & ref_paths, const double speed_limit) { if (ref_paths.size() < 2) { - return generateStraightPath(object, duration); + return generateStraightPath(object); } - return generatePolynomialPath(object, ref_paths, speed_limit, duration, lateral_duration); + return generatePolynomialPath(object, ref_paths, speed_limit); } -PredictedPath PathGenerator::generateStraightPath( - const TrackedObject & object, const double longitudinal_duration) const +PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) 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 = longitudinal_duration + ep; + const double duration = time_horizon_ + ep; PredictedPath path; path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); @@ -181,12 +178,11 @@ PredictedPath PathGenerator::generateStraightPath( } PredictedPath PathGenerator::generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path, const double duration, - const double lateral_duration, const double speed_limit) + const TrackedObject & object, const PosePath & ref_path, const double speed_limit) { // Get current Frenet Point const double ref_path_len = motion_utils::calcArcLength(ref_path); - const auto current_point = getFrenetPoint(object, ref_path, speed_limit, duration); + const auto current_point = getFrenetPoint(object, ref_path, speed_limit); // Step1. Set Target Frenet Point // Note that we do not set position s, @@ -200,13 +196,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, duration, lateral_duration); + generateFrenetPath(current_point, terminal_point, ref_path_len); // 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, duration); + return generateStraightPath(object); } // Step4. Convert predicted trajectory from Frenet to Cartesian coordinate @@ -214,10 +210,11 @@ PredictedPath PathGenerator::generatePolynomialPath( } FrenetPath PathGenerator::generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length, - const double duration, const double lateral_duration) + const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length) { 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 = @@ -388,8 +385,7 @@ PredictedPath PathGenerator::convertToPredictedPath( } FrenetPoint PathGenerator::getFrenetPoint( - const TrackedObject & object, const PosePath & ref_path, const double duration, - const double speed_limit) + const TrackedObject & object, const PosePath & ref_path, const double speed_limit) { FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; @@ -415,7 +411,7 @@ FrenetPoint PathGenerator::getFrenetPoint( : 0.0; // Using a decaying acceleration model. Consult the README for more information about the model. - const double t_h = duration; + const double t_h = time_horizon_; const float λ = std::log(2) / acceleration_exponential_half_life_; auto have_same_sign = [](double a, double b) -> bool {