Merge branch 'main' into fix/voxel_grid_covariance

Author: anhnv3991

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()

control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp

@@ -76,36 +76,41 @@ MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordin
   const MPCTrajectory & reference_trajectory, const double dt) const
 {
   // Calculate predicted state in world coordinate since there is modeling errors in Frenet
-  // Relative coordinate x = [lat_err, yaw_err, steer]
-  // World coordinate x = [x, y, yaw, steer]
+  // Relative coordinate state = [lat_err, yaw_err, steer]
+  // World coordinate state_w = [x, y, yaw]
+
+  // Note: Ideally, the first-order delay of the steering should be considered, as in the control
+  // model. However, significant accuracy degradation was observed when discretizing with a long dt,
+  // so it has been ignored here. If the accuracy of the discretization improves,an appropriate
+  // model should be considered.
 
   const auto & t = reference_trajectory;
 
   // create initial state in the world coordinate
-  Eigen::VectorXd state_w = [&]() {
-    Eigen::VectorXd state = Eigen::VectorXd::Zero(3);
+  Eigen::Vector3d state_w = [&]() {
+    Eigen::Vector3d state = Eigen::Vector3d::Zero();
     const auto lateral_error_0 = x0(0);
     const auto yaw_error_0 = x0(1);
-    state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0;  // world-x
-    state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0;  // world-y
-    state(2, 0) = t.yaw.at(0) + yaw_error_0;                            // world-yaw
+    state(0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0;  // world-x
+    state(1) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0;  // world-y
+    state(2) = t.yaw.at(0) + yaw_error_0;                            // world-yaw
     return state;
   }();
 
   // update state in the world coordinate
   const auto updateState = [&](
-                             const Eigen::VectorXd & state_w, const double & input, const double dt,
+                             const Eigen::Vector3d & state_w, const double & input, const double dt,
                              const double velocity) {
     const auto yaw = state_w(2);
 
-    Eigen::VectorXd dstate = Eigen::VectorXd::Zero(4);
+    Eigen::Vector3d dstate = Eigen::Vector3d::Zero();
     dstate(0) = velocity * std::cos(yaw);
     dstate(1) = velocity * std::sin(yaw);
     dstate(2) = velocity * std::tan(input) / m_wheelbase;
 
     // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation
     // in Eigen.
-    const Eigen::VectorXd next_state = state_w + dstate * dt;
+    const Eigen::Vector3d next_state = state_w + dstate * dt;
     return next_state;
   };
 

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -34,9 +34,18 @@
   <arg name="occupancy_grid_map_param_path"/>
   <arg name="occupancy_grid_map_updater"/>
   <arg name="occupancy_grid_map_updater_param_path"/>
-  <arg name="traffic_light_arbiter_param_path"/>
-  <arg name="traffic_light_multi_camera_fusion_param_path"/>
   <arg name="lidar_detection_model"/>
+  <arg name="each_traffic_light_map_based_detector_param_path"/>
+  <arg name="traffic_light_fine_detector_param_path"/>
+  <arg name="yolox_traffic_light_detector_param_path"/>
+  <arg name="car_traffic_light_classifier_param_path"/>
+  <arg name="pedestrian_traffic_light_classifier_param_path"/>
+  <arg name="traffic_light_roi_visualizer_param_path"/>
+  <arg name="traffic_light_selector_param_path"/>
+  <arg name="traffic_light_occlusion_predictor_param_path"/>
+  <arg name="traffic_light_multi_camera_fusion_param_path"/>
+  <arg name="traffic_light_arbiter_param_path"/>
+  <arg name="crosswalk_traffic_light_estimator_param_path"/>
 
   <!-- ML model parameters -->
   <arg name="lidar_detection_model_type" default="$(eval &quot;'$(var lidar_detection_model)'.split('/')[0]&quot;)"/>
@@ -98,31 +107,22 @@
   <arg name="use_obstacle_segmentation_single_frame_filter" description="use single frame filter at the ground segmentation"/>
   <arg name="use_obstacle_segmentation_time_series_filter" description="use time series filter at the ground segmentation"/>
 
-  <!-- Traffic light recognition parameters -->
-  <arg name="use_traffic_light_recognition" default="false"/>
-  <arg name="traffic_light_recognition/use_ml_detector" default="false" description="where ml model is used for TL detection"/>
-  <arg name="traffic_light_recognition/ml_detection_model_type" default="fine_detection_model" description="select ml model for TL detection: fine_detection_model or whole_image_detection_model"/>
-  <arg name="traffic_light_recognition/whole_image_detector_model_path" default="$(var data_path)/tensorrt_yolox"/>
-  <arg name="traffic_light_recognition/whole_image_detector_model_name" default="yolox_s_car_ped_tl_detector_960_960_batch_1.onnx"/>
-  <arg name="traffic_light_recognition/fusion_only" default="false"/>
-  <arg name="all_traffic_light_camera" default="[camera6, camera7]" description="choose camera which use for traffic light recognition"/>
-  <arg
-    name="traffic_light_fine_detector_model_path"
-    default="$(var data_path)/traffic_light_fine_detector"
-    description="options: `tlr_yolox_s_batch_**`. The batch number must be either one of 1, 4, 6"
-  />
-  <arg name="traffic_light_fine_detector_model_name" default="tlr_car_ped_yolox_s_batch_6" description="options: `tlr_car_ped_yolox_s_batch_**`. The batch number must be either one of 1, 4, 6"/>
-  <arg name="traffic_light_classifier_model_path" default="$(var data_path)/traffic_light_classifier" description="classifier onnx model path"/>
-  <arg
-    name="car_traffic_light_classifier_model_name"
-    default="traffic_light_classifier_mobilenetv2_batch_6"
-    description="options: `traffic_light_classifier_mobilenetv2_batch_*` or `traffic_light_classifier_efficientNet_b1_batch_*`. The batch number must be either one of 1, 4, 6"
-  />
-  <arg
-    name="pedestrian_traffic_light_classifier_model_name"
-    default="ped_traffic_light_classifier_mobilenetv2_batch_6"
-    description="options: `ped_traffic_light_classifier_mobilenetv2_batch_*` or `ped_traffic_light_classifier_efficientNet_b1_batch_*`. The batch number must be either one of 1, 4, 6"
-  />
+  <!-- traffic light recognition options to switch launch function/module -->
+  <arg name="use_traffic_light_recognition"/>
+  <arg name="traffic_light_recognition/fusion_only"/>
+  <arg name="traffic_light_recognition/camera_namespaces"/>
+  <arg name="traffic_light_recognition/use_high_accuracy_detection"/>
+  <arg name="traffic_light_recognition/high_accuracy_detection_type"/>
+
+  <!-- traffic light recognition parameters -->
+  <arg name="traffic_light_recognition/whole_image_detection/model_path"/>
+  <arg name="traffic_light_recognition/whole_image_detection/label_path"/>
+  <arg name="traffic_light_recognition/fine_detection/model_path"/>
+  <arg name="traffic_light_recognition/fine_detection/label_path"/>
+  <arg name="traffic_light_recognition/classification/car/model_path"/>
+  <arg name="traffic_light_recognition/classification/car/label_path"/>
+  <arg name="traffic_light_recognition/classification/pedestrian/model_path"/>
+  <arg name="traffic_light_recognition/classification/pedestrian/label_path"/>
 
   <!-- Whether to use detection by tracker -->
   <arg name="use_detection_by_tracker" default="true"/>
@@ -310,19 +310,29 @@
     <group if="$(var use_traffic_light_recognition)">
       <push-ros-namespace namespace="traffic_light_recognition"/>
       <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light.launch.xml">
-        <arg name="use_ml_detector" value="$(var traffic_light_recognition/use_ml_detector)"/>
-        <arg name="ml_detection_model_type" value="$(var traffic_light_recognition/ml_detection_model_type)"/>
-        <arg name="whole_image_detector_model_path" value="$(var traffic_light_recognition/whole_image_detector_model_path)"/>
-        <arg name="whole_image_detector_model_name" value="$(var traffic_light_recognition/whole_image_detector_model_name)"/>
         <arg name="fusion_only" value="$(var traffic_light_recognition/fusion_only)"/>
-        <arg name="all_camera_namespaces" value="$(var all_traffic_light_camera)"/>
-        <arg name="traffic_light_arbiter_param_path" value="$(var traffic_light_arbiter_param_path)"/>
+        <arg name="camera_namespaces" value="$(var traffic_light_recognition/camera_namespaces)"/>
+        <arg name="use_high_accuracy_detection" value="$(var traffic_light_recognition/use_high_accuracy_detection)"/>
+        <arg name="high_accuracy_detection_type" value="$(var traffic_light_recognition/high_accuracy_detection_type)"/>
+        <arg name="each_traffic_light_map_based_detector_param_path" value="$(var each_traffic_light_map_based_detector_param_path)"/>
+        <arg name="traffic_light_fine_detector_param_path" value="$(var traffic_light_fine_detector_param_path)"/>
+        <arg name="yolox_traffic_light_detector_param_path" value="$(var yolox_traffic_light_detector_param_path)"/>
+        <arg name="car_traffic_light_classifier_param_path" value="$(var car_traffic_light_classifier_param_path)"/>
+        <arg name="pedestrian_traffic_light_classifier_param_path" value="$(var pedestrian_traffic_light_classifier_param_path)"/>
+        <arg name="traffic_light_roi_visualizer_param_path" value="$(var traffic_light_roi_visualizer_param_path)"/>
+        <arg name="traffic_light_selector_param_path" value="$(var traffic_light_selector_param_path)"/>
+        <arg name="traffic_light_occlusion_predictor_param_path" value="$(var traffic_light_occlusion_predictor_param_path)"/>
         <arg name="traffic_light_multi_camera_fusion_param_path" value="$(var traffic_light_multi_camera_fusion_param_path)"/>
-        <arg name="traffic_light_fine_detector_model_path" value="$(var traffic_light_fine_detector_model_path)"/>
-        <arg name="traffic_light_fine_detector_model_name" value="$(var traffic_light_fine_detector_model_name)"/>
-        <arg name="traffic_light_classifier_model_path" value="$(var traffic_light_classifier_model_path)"/>
-        <arg name="car_traffic_light_classifier_model_name" value="$(var car_traffic_light_classifier_model_name)"/>
-        <arg name="pedestrian_traffic_light_classifier_model_name" value="$(var pedestrian_traffic_light_classifier_model_name)"/>
+        <arg name="traffic_light_arbiter_param_path" value="$(var traffic_light_arbiter_param_path)"/>
+        <arg name="crosswalk_traffic_light_estimator_param_path" value="$(var crosswalk_traffic_light_estimator_param_path)"/>
+        <arg name="whole_image_detection/model_path" value="$(var traffic_light_recognition/whole_image_detection/model_path)"/>
+        <arg name="whole_image_detection/label_path" value="$(var traffic_light_recognition/whole_image_detection/label_path)"/>
+        <arg name="fine_detection/model_path" value="$(var traffic_light_recognition/fine_detection/model_path)"/>
+        <arg name="fine_detection/label_path" value="$(var traffic_light_recognition/fine_detection/label_path)"/>
+        <arg name="classification/car/model_path" value="$(var traffic_light_recognition/classification/car/model_path)"/>
+        <arg name="classification/car/label_path" value="$(var traffic_light_recognition/classification/car/label_path)"/>
+        <arg name="classification/pedestrian/model_path" value="$(var traffic_light_recognition/classification/pedestrian/model_path)"/>
+        <arg name="classification/pedestrian/label_path" value="$(var traffic_light_recognition/classification/pedestrian/label_path)"/>
       </include>
     </group>
   </group>