From 5a0e1411b0d5e6565185459ac15351cfd8a81698 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Thu, 11 Jan 2024 15:09:38 +0900 Subject: [PATCH] fix(map_based_prediction): yaw rate can be overwritten by max function (#6039) * fix problem with negative yaw being replaced in max function Signed-off-by: Daniel Sanchez * add check for delta time value Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../map_based_prediction/map_based_prediction_node.hpp | 9 +++++---- .../src/map_based_prediction_node.cpp | 8 ++++---- perception/map_based_prediction/src/path_generator.cpp | 2 +- 3 files changed, 10 insertions(+), 9 deletions(-) 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 02ca62a61717c..02db91b989116 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 @@ -316,8 +316,11 @@ class MapBasedPredictionNode : public rclcpp::Node }; inline bool isLateralAccelerationConstraintSatisfied( - const TrajectoryPoints & trajectory [[maybe_unused]], const double delta_time) + const TrajectoryPoints & trajectory, const double delta_time) { + constexpr double epsilon = 1E-6; + if (delta_time < epsilon) throw std::invalid_argument("delta_time must be a positive value"); + if (trajectory.size() < 3) return true; const double max_lateral_accel_abs = std::fabs(max_lateral_accel_); @@ -343,19 +346,17 @@ class MapBasedPredictionNode : public rclcpp::Node delta_theta += 2.0 * M_PI; } - const double yaw_rate = std::max(delta_theta / delta_time, 1.0E-5); + const double yaw_rate = std::max(std::abs(delta_theta / delta_time), 1.0E-5); const double current_speed = std::abs(trajectory.at(i).longitudinal_velocity_mps); // Compute lateral acceleration const double lateral_acceleration = std::abs(current_speed * yaw_rate); if (lateral_acceleration < max_lateral_accel_abs) continue; - const double v_curvature_max = std::sqrt(max_lateral_accel_abs / yaw_rate); const double t = (v_curvature_max - current_speed) / min_acceleration_before_curve_; // acc is negative const double distance_to_slow_down = current_speed * t + 0.5 * min_acceleration_before_curve_ * std::pow(t, 2); - if (distance_to_slow_down > arc_length) return false; } return true; 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 75c1d61e0a19c..6a6f24081655e 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -613,16 +613,16 @@ ObjectClassification::_label_type changeLabelForPrediction( case ObjectClassification::BICYCLE: { // if object is within road lanelet and satisfies yaw // constraints const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); - const float high_speed_threshold = + // if the object is within lanelet, do the same estimation with vehicle + if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; + + constexpr float high_speed_threshold = tier4_autoware_utils::kmph2mps(25.0); // High speed bicycle 25 km/h // calc abs speed from x and y velocity const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); const bool high_speed_object = abs_speed > high_speed_threshold; - - // if the object is within lanelet, do the same estimation with vehicle - if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; // high speed object outside road lanelet will move like unknown object // return ObjectClassification::UNKNOWN; // temporary disabled if (high_speed_object) return label; // Do nothing for now diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 5cb7e186b7cc4..d3f610583577c 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -162,7 +162,7 @@ PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) { const auto & object_pose = object.kinematics.pose_with_covariance.pose; const auto & object_twist = object.kinematics.twist_with_covariance.twist; - const double ep = 0.001; + constexpr double ep = 0.001; const double duration = time_horizon_ + ep; PredictedPath path;