Merge branch 'beta/v0.10.4' into vel-con-steer-converge

control/autonomous_emergency_braking/src/node.cpp

@@ -221,7 +221,7 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
   obstacle_ros_pointcloud_ptr_ = std::make_shared<PointCloud2>();
   pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_);
   obstacle_ros_pointcloud_ptr_->header = input_msg->header;
-  pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_);
+  // pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_);
 }
 
 bool AEB::isDataReady()
@@ -426,6 +426,10 @@ void AEB::generateEgoPath(
   const Trajectory & predicted_traj, Path & path,
   std::vector<tier4_autoware_utils::Polygon2d> & polygons)
 {
+  if (predicted_traj.points.empty()) {
+    return;
+  }
+
   geometry_msgs::msg::TransformStamped transform_stamped{};
   try {
     transform_stamped = tf_buffer_.lookupTransform(
@@ -442,6 +446,10 @@ void AEB::generateEgoPath(
     geometry_msgs::msg::Pose map_pose;
     tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped);
     path.at(i) = map_pose;
+
+    if (i * prediction_time_interval_ > prediction_time_horizon_) {
+      break;
+    }
   }
 
   // create polygon

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp

@@ -379,6 +379,14 @@ class MPC
     const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
     const Odometry & current_kinematics) const;
 
+  /**
+   * @brief calculate steering rate limit along with the target trajectory
+   * @param reference_trajectory The reference trajectory.
+   * @param current_velocity current velocity of ego.
+   */
+  VectorXd calcSteerRateLimitOnTrajectory(
+    const MPCTrajectory & trajectory, const double current_velocity) const;
+
   //!< @brief logging with warn and return false
   template <typename... Args>
   inline bool fail_warn_throttle(Args &&... args) const

control/mpc_lateral_controller/src/mpc.cpp

@@ -359,9 +359,6 @@ std::pair<bool, VectorXd> MPC::updateStateForDelayCompensation(
 
   MatrixXd x_curr = x0_orig;
   double mpc_curr_time = start_time;
-  // for (const auto & tt : traj.relative_time) {
-  //   std::cerr << "traj.relative_time = " << tt << std::endl;
-  // }
   for (size_t i = 0; i < m_input_buffer.size(); ++i) {
     double k, v = 0.0;
     try {
@@ -573,47 +570,16 @@ std::pair<bool, VectorXd> MPC::executeOptimization(
     A(i, i - 1) = -1.0;
   }
 
-  const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01;
-  const auto get_adaptive_steer_rate_lim = [&](const double curvature, const double velocity) {
-    if (is_vehicle_stopped) {
-      return std::numeric_limits<double>::max();
-    }
-
-    double steer_rate_lim_by_curvature = m_steer_rate_lim_map_by_curvature.back().second;
-    for (const auto & steer_rate_lim_info : m_steer_rate_lim_map_by_curvature) {
-      if (std::abs(curvature) <= steer_rate_lim_info.first) {
-        steer_rate_lim_by_curvature = steer_rate_lim_info.second;
-        break;
-      }
-    }
-
-    double steer_rate_lim_by_velocity = m_steer_rate_lim_map_by_velocity.back().second;
-    for (const auto & steer_rate_lim_info : m_steer_rate_lim_map_by_velocity) {
-      if (std::abs(velocity) <= steer_rate_lim_info.first) {
-        steer_rate_lim_by_velocity = steer_rate_lim_info.second;
-        break;
-      }
-    }
-
-    return std::min(steer_rate_lim_by_curvature, steer_rate_lim_by_velocity);
-  };
-
+  // steering angle limit
   VectorXd lb = VectorXd::Constant(DIM_U_N, -m_steer_lim);  // min steering angle
   VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim);   // max steering angle
 
-  VectorXd lbA(DIM_U_N);
-  VectorXd ubA(DIM_U_N);
-  for (int i = 0; i < DIM_U_N; ++i) {
-    const double adaptive_steer_rate_lim =
-      get_adaptive_steer_rate_lim(traj.smooth_k.at(i), traj.vx.at(i));
-    const double adaptive_delta_steer_lim = adaptive_steer_rate_lim * prediction_dt;
-    lbA(i) = -adaptive_delta_steer_lim;
-    ubA(i) = adaptive_delta_steer_lim;
-  }
-  const double adaptive_steer_rate_lim =
-    get_adaptive_steer_rate_lim(traj.smooth_k.at(0), traj.vx.at(0));
-  lbA(0, 0) = m_raw_steer_cmd_prev - adaptive_steer_rate_lim * m_ctrl_period;
-  ubA(0, 0) = m_raw_steer_cmd_prev + adaptive_steer_rate_lim * m_ctrl_period;
+  // steering angle rate limit
+  VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity);
+  VectorXd ubA = steer_rate_limits * prediction_dt;
+  VectorXd lbA = -steer_rate_limits * prediction_dt;
+  ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period;
+  lbA(0) = m_raw_steer_cmd_prev - steer_rate_limits(0) * m_ctrl_period;
 
   auto t_start = std::chrono::system_clock::now();
   bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex);
@@ -753,14 +719,65 @@ double MPC::calcDesiredSteeringRate(
 
   const size_t STEER_IDX = 2;  // for kinematics model
 
-  const auto steer_0 = x0(STEER_IDX, 0);
   const auto steer_1 = Xex(STEER_IDX, 0);
-
-  const auto steer_rate = (steer_1 - steer_0) / predict_dt;
+  const auto steer_2 = Xex(STEER_IDX + m_vehicle_model_ptr->getDimX(), 0);
+  const auto steer_rate = (steer_2 - steer_1) / predict_dt;
 
   return steer_rate;
 }
 
+VectorXd MPC::calcSteerRateLimitOnTrajectory(
+  const MPCTrajectory & trajectory, const double current_velocity) const
+{
+  const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) {
+    std::vector<double> reference, limits;
+    for (const auto & p : steer_rate_limit_map) {
+      reference.push_back(p.first);
+      limits.push_back(p.second);
+    }
+
+    // If the speed is out of range of the reference, apply zero-order hold.
+    if (current <= reference.front()) {
+      return limits.front();
+    }
+    if (current >= reference.back()) {
+      return limits.back();
+    }
+
+    // Apply linear interpolation
+    for (size_t i = 0; i < reference.size() - 1; ++i) {
+      if (reference.at(i) <= current && current <= reference.at(i + 1)) {
+        auto ratio =
+          (current - reference.at(i)) / std::max(reference.at(i + 1) - reference.at(i), 1.0e-5);
+        ratio = std::clamp(ratio, 0.0, 1.0);
+        const auto interp = limits.at(i) + ratio * (limits.at(i + 1) - limits.at(i));
+        return interp;
+      }
+    }
+
+    std::cerr << "MPC::calcSteerRateLimitOnTrajectory() interpolation logic is broken. Command "
+                 "filter is not working. Please check the code."
+              << std::endl;
+    return reference.back();
+  };
+
+  // when the vehicle is stopped, no steering rate limit.
+  const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01;
+  if (is_vehicle_stopped) {
+    return VectorXd::Zero(m_param.prediction_horizon);
+  }
+
+  // calculate steering rate limit
+  VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon);
+  for (int i = 0; i < m_param.prediction_horizon; ++i) {
+    const auto limit_by_curvature = interp(m_steer_rate_lim_map_by_curvature, trajectory.k.at(i));
+    const auto limit_by_velocity = interp(m_steer_rate_lim_map_by_velocity, trajectory.vx.at(i));
+    steer_rate_limits(i) = std::min(limit_by_curvature, limit_by_velocity);
+  }
+
+  return steer_rate_limits;
+}
+
 bool MPC::isValid(const MPCMatrix & m) const
 {
   if (

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml

@@ -77,7 +77,7 @@
       <arg name="filter/velocity_threshold" value="3.0"/>
       <arg name="split/velocity_threshold" value="4.5"/>
       <arg name="split_range" value="70.0"/>
-      <arg name="clustering/angle_threshold" value="0.174"/>
+      <arg name="clustering/angle_threshold" value="0.522"/>
       <arg name="clustering/distance_threshold" value="10.0"/>
       <arg name="clustering/velocity_threshold" value="4.0"/>
     </include>

launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml

@@ -37,7 +37,7 @@
       <arg name="filter/velocity_threshold" value="3.0"/>
       <arg name="split/velocity_threshold" value="4.5"/>
       <arg name="split_range" value="70.0"/>
-      <arg name="clustering/angle_threshold" value="0.174"/>
+      <arg name="clustering/angle_threshold" value="0.522"/>
       <arg name="clustering/distance_threshold" value="10.0"/>
       <arg name="clustering/velocity_threshold" value="4.0"/>
     </include>

perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp

@@ -52,6 +52,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node
 
   lanelet::LaneletMapPtr lanelet_map_ptr_;
   lanelet::ConstLanelets road_lanelets_;
+  lanelet::ConstLanelets shoulder_lanelets_;
   std::string lanelet_frame_id_;
 
   tf2_ros::Buffer tf_buffer_;

perception/detected_object_validation/src/object_lanelet_filter.cpp

@@ -60,6 +60,7 @@ void ObjectLaneletFilterNode::mapCallback(
   lanelet::utils::conversion::fromBinMsg(*map_msg, lanelet_map_ptr_);
   const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_);
   road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets);
+  shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets);
 }
 
 void ObjectLaneletFilterNode::objectCallback(
@@ -85,7 +86,10 @@ void ObjectLaneletFilterNode::objectCallback(
   // calculate convex hull
   const auto convex_hull = getConvexHull(transformed_objects);
   // get intersected lanelets
-  lanelet::ConstLanelets intersected_lanelets = getIntersectedLanelets(convex_hull, road_lanelets_);
+  lanelet::ConstLanelets intersected_road_lanelets =
+    getIntersectedLanelets(convex_hull, road_lanelets_);
+  lanelet::ConstLanelets intersected_shoulder_lanelets =
+    getIntersectedLanelets(convex_hull, shoulder_lanelets_);
 
   int index = 0;
   for (const auto & object : transformed_objects.objects) {
@@ -99,7 +103,9 @@ void ObjectLaneletFilterNode::objectCallback(
         polygon.outer().emplace_back(point_transformed.x, point_transformed.y);
       }
       polygon.outer().push_back(polygon.outer().front());
-      if (isPolygonOverlapLanelets(polygon, intersected_lanelets)) {
+      if (isPolygonOverlapLanelets(polygon, intersected_road_lanelets)) {
+        output_object_msg.objects.emplace_back(input_msg->objects.at(index));
+      } else if (isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets)) {
         output_object_msg.objects.emplace_back(input_msg->objects.at(index));
       }
     } else {

perception/map_based_prediction/README.md

@@ -109,6 +109,21 @@ For the additional information, here we show how we calculate lateral velocity.
 
 Currently, we use the upper method with a low-pass filter to calculate lateral velocity.
 
+### Path generation
+
+Path generation is generated on the frenet frame. The path is generated by the following steps:
+
+1. Get the frenet frame of the reference path.
+2. Generate the frenet frame of the current position of the object and the finite position of the object.
+3. Optimize the path in each longitudinal and lateral coordinate of the frenet frame. (Use the quintic polynomial to fit start and end conditions.)
+4. Convert the path to the global coordinate.
+
+See paper [2] for more details.
+
+#### Tuning lateral path shape
+
+`lateral_control_time_horizon` parameter supports the tuning of the lateral path shape. This parameter is used to calculate the time to reach the reference path. The smaller the value, the more the path will be generated to reach the reference path quickly. (Mostly the center of the lane.)
+
 ### Path prediction for crosswalk users
 
 This module treats **Pedestrians** and **Bicycles** as objects using the crosswalk, and outputs prediction path based on map and estimated object's velocity, assuming the object has intention to cross the crosswalk, if the objects satisfies at least one of the following conditions:
@@ -150,23 +165,22 @@ If the target object is inside the road or crosswalk, this module outputs one or
 
 ## Parameters
 
-| Parameter                                   | Type   | Description                                                                                                  |
-| ------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------ |
-| `enable_delay_compensation`                 | bool   | flag to enable the time delay compensation for the position of the object                                    |
-| `prediction_time_horizon`                   | double | predict time duration for predicted path [s]                                                                 |
-| `prediction_sampling_delta_time`            | double | sampling time for points in predicted path [s]                                                               |
-| `min_velocity_for_map_based_prediction`     | double | apply map-based prediction to the objects with higher velocity than this value                               |
-| `min_crosswalk_user_velocity`               | double | minimum velocity use in path prediction for crosswalk users                                                  |
-| `dist_threshold_for_searching_lanelet`      | double | The threshold of the angle used when searching for the lane to which the object belongs [rad]                |
-| `delta_yaw_threshold_for_searching_lanelet` | double | The threshold of the distance used when searching for the lane to which the object belongs [m]               |
-| `sigma_lateral_offset`                      | double | Standard deviation for lateral position of objects [m]                                                       |
-| `sigma_yaw_angle`                           | double | Standard deviation yaw angle of objects [rad]                                                                |
-| `object_buffer_time_length`                 | double | Time span of object history to store the information [s]                                                     |
-| `history_time_length`                       | double | Time span of object information used for prediction [s]                                                      |
-| `dist_ratio_threshold_to_left_bound`        | double | Conditions for using lane change detection of objects. Distance to the left bound of lanelet.                |
-| `dist_ratio_threshold_to_right_bound`       | double | Conditions for using lane change detection of objects. Distance to the right bound of lanelet.               |
-| `diff_dist_threshold_to_left_bound`         | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. |
-| `diff_dist_threshold_to_right_bound`        | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. |
+| Parameter                                                        | Unit  | Type   | Description                                                                                                                           |
+| ---------------------------------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------------------- |
+| `enable_delay_compensation`                                      | [-]   | bool   | flag to enable the time delay compensation for the position of the object                                                             |
+| `prediction_time_horizon`                                        | [s]   | double | predict time duration for predicted path                                                                                              |
+| `lateral_control_time_horizon`                                   | [s]   | double | time duration for predicted path will reach the reference path (mostly center of the lane)                                            |
+| `prediction_sampling_delta_time`                                 | [s]   | double | sampling time for points in predicted path                                                                                            |
+| `min_velocity_for_map_based_prediction`                          | [m/s] | double | apply map-based prediction to the objects with higher velocity than this value                                                        |
+| `min_crosswalk_user_velocity`                                    | [m/s] | double | minimum velocity used when crosswalk user's velocity is calculated                                                                    |
+| `max_crosswalk_user_delta_yaw_threshold_for_lanelet`             | [rad] | double | maximum yaw difference between crosswalk user and lanelet to use in path prediction for crosswalk users                               |
+| `dist_threshold_for_searching_lanelet`                           | [m]   | double | The threshold of the angle used when searching for the lane to which the object belongs                                               |
+| `delta_yaw_threshold_for_searching_lanelet`                      | [rad] | double | The threshold of the angle used when searching for the lane to which the object belongs                                               |
+| `sigma_lateral_offset`                                           | [m]   | double | Standard deviation for lateral position of objects                                                                                    |
+| `sigma_yaw_angle_deg`                                            | [deg] | double | Standard deviation yaw angle of objects                                                                                               |
+| `object_buffer_time_length`                                      | [s]   | double | Time span of object history to store the information                                                                                  |
+| `history_time_length`                                            | [s]   | double | Time span of object information used for prediction                                                                                   |
+| `prediction_time_horizon_rate_for_validate_shoulder_lane_length` | [-]   | double | prediction path will disabled when the estimated path length exceeds lanelet length. This parameter control the estimated path length |
 
 ## Assumptions / Known limits
 

perception/map_based_prediction/config/map_based_prediction.param.yaml

@@ -2,6 +2,7 @@
   ros__parameters:
     enable_delay_compensation: true
     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]
     min_crosswalk_user_velocity: 1.39 #[m/s]

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -139,6 +139,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   // Parameters
   bool enable_delay_compensation_;
   double prediction_time_horizon_;
+  double lateral_control_time_horizon_;
   double prediction_time_horizon_rate_for_validate_lane_length_;
   double prediction_sampling_time_interval_;
   double min_velocity_for_map_based_prediction_;

perception/map_based_prediction/include/map_based_prediction/path_generator.hpp

@@ -57,8 +57,8 @@ class PathGenerator
 {
 public:
   PathGenerator(
-    const double time_horizon, const double sampling_time_interval,
-    const double min_crosswalk_user_velocity);
+    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);
 
@@ -78,6 +78,7 @@ class PathGenerator
 private:
   // Parameters
   double time_horizon_;
+  double lateral_time_horizon_;
   double sampling_time_interval_;
   double min_crosswalk_user_velocity_;