Merge branch 'main' into feat/autoware-utils-update

Author: isamu-takagi

common/autoware_universe_utils/src/geometry/alt_geometry.cpp

@@ -263,28 +263,30 @@ bool covered_by(const alt::Point2d & point, const alt::ConvexPolygon2d & poly)
     return false;
   }
 
-  double cross;
   for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) {
     const auto & p1 = *it;
     const auto & p2 = *std::next(it);
 
-    if (p1.y() <= point.y() && p2.y() >= point.y()) {  // upward edge
-      cross = (p2 - p1).cross(point - p1);
-      if (cross > 0) {  // point is to the left of edge
-        winding_number++;
-        continue;
-      }
-    } else if (p1.y() >= point.y() && p2.y() <= point.y()) {  // downward edge
-      cross = (p2 - p1).cross(point - p1);
-      if (cross < 0) {  // point is to the left of edge
-        winding_number--;
-        continue;
-      }
-    } else {
+    const auto is_upward_edge = p1.y() <= point.y() && p2.y() >= point.y();
+    const auto is_downward_edge = p1.y() >= point.y() && p2.y() <= point.y();
+
+    if (!is_upward_edge && !is_downward_edge) {
+      continue;
+    }
+
+    const auto start_vec = point - p1;
+    const auto end_vec = point - p2;
+    const auto cross = start_vec.cross(end_vec);
+
+    if (is_upward_edge && cross > 0) {  // point is to the left of edge
+      winding_number++;
+      continue;
+    } else if (is_downward_edge && cross < 0) {  // point is to the left of edge
+      winding_number--;
       continue;
     }
 
-    if (std::abs(cross) < epsilon) {  // point is on edge
+    if (std::abs(cross) < epsilon && start_vec.dot(end_vec) <= 0.) {  // point is on edge
       return true;
     }
   }
@@ -600,28 +602,30 @@ bool within(const alt::Point2d & point, const alt::ConvexPolygon2d & poly)
     return false;
   }
 
-  double cross;
   for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) {
     const auto & p1 = *it;
     const auto & p2 = *std::next(it);
 
-    if (p1.y() < point.y() && p2.y() > point.y()) {  // upward edge
-      cross = (p2 - p1).cross(point - p1);
-      if (cross > 0) {  // point is to the left of edge
-        winding_number++;
-        continue;
-      }
-    } else if (p1.y() > point.y() && p2.y() < point.y()) {  // downward edge
-      cross = (p2 - p1).cross(point - p1);
-      if (cross < 0) {  // point is to the left of edge
-        winding_number--;
-        continue;
-      }
-    } else {
+    const auto is_upward_edge = p1.y() < point.y() && p2.y() > point.y();
+    const auto is_downward_edge = p1.y() > point.y() && p2.y() < point.y();
+
+    if (!is_upward_edge && !is_downward_edge) {
+      continue;
+    }
+
+    const auto start_vec = point - p1;
+    const auto end_vec = point - p2;
+    const auto cross = start_vec.cross(end_vec);
+
+    if (is_upward_edge && cross > 0) {  // point is to the left of edge
+      winding_number++;
+      continue;
+    } else if (is_downward_edge && cross < 0) {  // point is to the left of edge
+      winding_number--;
       continue;
     }
 
-    if (std::abs(cross) < epsilon) {  // point is on edge
+    if (std::abs(cross) < epsilon && start_vec.dot(end_vec) <= 0.) {  // point is on edge
       return false;
     }
   }

common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp

@@ -164,6 +164,17 @@ TEST(alt_geometry, coveredBy)
 
     EXPECT_TRUE(result);
   }
+
+  {  // The point is on the extended line of an edge of the polygon
+    const Point2d point = {0.0, 0.0};
+    const Point2d p1 = {3.0, 0.0};
+    const Point2d p2 = {3.0, 1.0};
+    const Point2d p3 = {4.0, 1.0};
+    const Point2d p4 = {4.0, 0.0};
+    const auto result = covered_by(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+    EXPECT_FALSE(result);
+  }
 }
 
 TEST(alt_geometry, disjoint)
@@ -674,6 +685,17 @@ TEST(alt_geometry, within)
     EXPECT_FALSE(result);
   }
 
+  {  // The point is on the extended line of an edge of the polygon
+    const Point2d point = {0.0, 0.0};
+    const Point2d p1 = {3.0, 0.0};
+    const Point2d p2 = {3.0, 1.0};
+    const Point2d p3 = {4.0, 1.0};
+    const Point2d p4 = {4.0, 0.0};
+    const auto result = within(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+    EXPECT_FALSE(result);
+  }
+
   {  // One polygon is within the other
     const Point2d p1 = {1.0, 1.0};
     const Point2d p2 = {1.0, -1.0};

control/autoware_control_validator/README.md

@@ -57,9 +57,10 @@ The following parameters can be set for the `control_validator`:
 
 The input trajectory is detected as invalid if the index exceeds the following thresholds.
 
-| Name                                | Type   | Description                                                                                                 | Default value |
-| :---------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ |
-| `thresholds.max_distance_deviation` | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0           |
-| `thresholds.rolling_back_velocity`  | double | threshold velocity to valid the vehicle velocity [m/s]                                                      | 0.5           |
-| `thresholds.over_velocity_offset`   | double | threshold velocity offset to valid the vehicle velocity [m/s]                                               | 2.0           |
-| `thresholds.over_velocity_ratio`    | double | threshold ratio to valid the vehicle velocity [*]                                                           | 0.2           |
+| Name                                 | Type   | Description                                                                                                 | Default value |
+| :----------------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ |
+| `thresholds.max_distance_deviation`  | double | invalid threshold of the max distance deviation between the predicted path and the reference trajectory [m] | 1.0           |
+| `thresholds.rolling_back_velocity`   | double | threshold velocity to valid the vehicle velocity [m/s]                                                      | 0.5           |
+| `thresholds.over_velocity_offset`    | double | threshold velocity offset to valid the vehicle velocity [m/s]                                               | 2.0           |
+| `thresholds.over_velocity_ratio`     | double | threshold ratio to valid the vehicle velocity [*]                                                           | 0.2           |
+| `thresholds.overrun_stop_point_dist` | double | threshold distance to overrun stop point [m]                                                                | 0.8           |

control/autoware_control_validator/config/control_validator.param.yaml

@@ -12,6 +12,7 @@
       rolling_back_velocity: 0.5
       over_velocity_offset: 2.0
       over_velocity_ratio: 0.2
+      overrun_stop_point_dist: 0.8
       nominal_latency: 0.01
 
     vel_lpf_gain: 0.9 # Time constant 0.33

control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp

@@ -53,6 +53,7 @@ struct ValidationParams
   double rolling_back_velocity;
   double over_velocity_ratio;
   double over_velocity_offset;
+  double overrun_stop_point_dist;
   double nominal_latency_threshold;
 };
 
@@ -95,6 +96,14 @@ class ControlValidator : public rclcpp::Node
   void calc_velocity_deviation_status(
     const Trajectory & reference_trajectory, const Odometry & kinematics);
 
+  /**
+   * @brief Calculate whether the vehicle has overrun a stop point in the trajectory.
+   * @param reference_trajectory Reference trajectory
+   * @param kinematics Current vehicle odometry including pose and twist
+   */
+  void calc_stop_point_overrun_status(
+    const Trajectory & reference_trajectory, const Odometry & kinematics);
+
 private:
   /**
    * @brief Setup diagnostic updater

control/autoware_control_validator/msg/ControlValidatorStatus.msg

@@ -4,12 +4,15 @@ builtin_interfaces/Time stamp
 bool is_valid_max_distance_deviation
 bool is_rolling_back
 bool is_over_velocity
+bool has_overrun_stop_point
 bool is_valid_latency
 
 # values
 float64 max_distance_deviation
 float64 target_vel
 float64 vehicle_vel
+float64 dist_to_stop
+float64 nearest_trajectory_vel
 float64 latency
 
 int64 invalid_count

control/autoware_control_validator/src/control_validator.cpp

@@ -16,10 +16,12 @@
 
 #include "autoware/control_validator/utils.hpp"
 #include "autoware/motion_utils/trajectory/interpolation.hpp"
+#include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
 
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <algorithm>
 #include <cstdint>
 #include <memory>
 #include <string>
@@ -72,6 +74,7 @@ void ControlValidator::setup_parameters()
     p.rolling_back_velocity = declare_parameter<double>(t + "rolling_back_velocity");
     p.over_velocity_offset = declare_parameter<double>(t + "over_velocity_offset");
     p.over_velocity_ratio = declare_parameter<double>(t + "over_velocity_ratio");
+    p.overrun_stop_point_dist = declare_parameter<double>(t + "overrun_stop_point_dist");
     p.nominal_latency_threshold = declare_parameter<double>(t + "nominal_latency");
   }
   const auto lpf_gain = declare_parameter<double>("vel_lpf_gain");
@@ -129,6 +132,11 @@ void ControlValidator::setup_diag()
       stat, !validation_status_.is_over_velocity,
       "The vehicle is over-speeding against the target.");
   });
+  d.add(ns + "overrun_stop_point", [&](auto & stat) {
+    set_status(
+      stat, !validation_status_.has_overrun_stop_point,
+      "The vehicle has overrun the front stop point on the trajectory.");
+  });
   d.add(ns + "latency", [&](auto & stat) {
     set_status(
       stat, validation_status_.is_valid_latency, "The latency is larger than expected value.");
@@ -219,12 +227,14 @@ void ControlValidator::validate(
   }
 
   validation_status_.stamp = get_clock()->now();
+  validation_status_.vehicle_vel = vehicle_vel_.filter(kinematics.twist.twist.linear.x);
 
   std::tie(
     validation_status_.max_distance_deviation, validation_status_.is_valid_max_distance_deviation) =
     calc_lateral_deviation_status(predicted_trajectory, *current_reference_trajectory_);
 
   calc_velocity_deviation_status(*current_reference_trajectory_, kinematics);
+  calc_stop_point_overrun_status(*current_reference_trajectory_, kinematics);
 
   validation_status_.invalid_count =
     is_all_valid(validation_status_) ? 0 : validation_status_.invalid_count + 1;
@@ -245,7 +255,6 @@ void ControlValidator::calc_velocity_deviation_status(
 {
   auto & status = validation_status_;
   const auto & params = validation_params_;
-  status.vehicle_vel = vehicle_vel_.filter(kinematics.twist.twist.linear.x);
   status.target_vel = target_vel_.filter(
     autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose)
       .longitudinal_velocity_mps);
@@ -268,10 +277,41 @@ void ControlValidator::calc_velocity_deviation_status(
   }
 }
 
+void ControlValidator::calc_stop_point_overrun_status(
+  const Trajectory & reference_trajectory, const Odometry & kinematics)
+{
+  auto & status = validation_status_;
+  const auto & params = validation_params_;
+
+  status.dist_to_stop = [](const Trajectory & traj, const geometry_msgs::msg::Pose & pose) {
+    const auto stop_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(traj.points);
+
+    const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1;
+    const size_t seg_idx =
+      autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(traj.points, pose);
+    const double signed_length_on_traj = autoware::motion_utils::calcSignedArcLength(
+      traj.points, pose.position, seg_idx, traj.points.at(end_idx).pose.position,
+      std::min(end_idx, traj.points.size() - 2));
+
+    if (std::isnan(signed_length_on_traj)) {
+      return 0.0;
+    }
+    return signed_length_on_traj;
+  }(reference_trajectory, kinematics.pose.pose);
+
+  status.nearest_trajectory_vel =
+    autoware::motion_utils::calcInterpolatedPoint(reference_trajectory, kinematics.pose.pose)
+      .longitudinal_velocity_mps;
+
+  // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity
+  status.has_overrun_stop_point = status.dist_to_stop < -params.overrun_stop_point_dist &&
+                                  status.nearest_trajectory_vel < 1e-3 && status.vehicle_vel > 1e-3;
+}
+
 bool ControlValidator::is_all_valid(const ControlValidatorStatus & s)
 {
   return s.is_valid_max_distance_deviation && !s.is_rolling_back && !s.is_over_velocity &&
-         s.is_valid_latency;
+         !s.has_overrun_stop_point;
 }
 
 void ControlValidator::display_status()