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 <[email protected]> * add check for delta time value Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]>
autowarefoundation · Jan 11, 2024 · 5a0e141 · 5a0e141
1 parent 6928fff
commit 5a0e141
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 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
@@ -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
@@ -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
@@ -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;