Merge pull request autowarefoundation#555 from tier4/sync-upstream

chore: sync upstream

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp

@@ -39,6 +39,7 @@
 #include <deque>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace autoware::motion::control::mpc_lateral_controller
@@ -226,7 +227,7 @@ class MPC
    */
   bool executeOptimization(
     const MPCMatrix & mpc_matrix, const Eigen::VectorXd & x0, const double prediction_dt,
-    Eigen::VectorXd * Uex);
+    Eigen::VectorXd * Uex, const MPCTrajectory & traj, const double current_velocity);
   /**
    * @brief resample trajectory with mpc resampling time
    */
@@ -352,8 +353,10 @@ class MPC
   double m_admissible_yaw_error_rad;
   //!< @brief steering command limit [rad]
   double m_steer_lim;
-  //!< @brief steering rate limit [rad/s]
-  double m_steer_rate_lim;
+  //!< @brief steering rate limit list depending on curvature [/m], [rad/s]
+  std::vector<std::pair<double, double>> m_steer_rate_lim_map_by_curvature{};
+  //!< @brief steering rate limit list depending on velocity [m/s], [rad/s]
+  std::vector<std::pair<double, double>> m_steer_rate_lim_map_by_velocity{};
   //!< @brief control frequency [s]
   double m_ctrl_period;
   /* parameters for path smoothing */

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp

@@ -105,9 +105,6 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
   double m_new_traj_end_dist;            // check trajectory shape change
   bool m_keep_steer_control_until_converged;
 
-  /* parameter to store the actual steering rate limit */
-  double m_steer_rate_lim;
-
   // trajectory buffer for detecting new trajectory
   std::deque<autoware_auto_planning_msgs::msg::Trajectory> m_trajectory_buffer;
 

control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml

@@ -47,7 +47,10 @@
     vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics
     input_delay: 0.24 # steering input delay time for delay compensation
     vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s]
-    steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s]
+    steer_rate_lim_dps_list_by_curvature: [10.0, 20.0, 30.0]            # steering angle rate limit list depending on curvature [deg/s]
+    curvature_list_for_steer_rate_lim: [0.001, 0.002, 0.01]              # curvature list for steering angle rate limit interpolation in ascending order [/m]
+    steer_rate_lim_dps_list_by_velocity: [40.0, 30.0, 20.0]             # steering angle rate limit list depending on velocity [deg/s]
+    velocity_list_for_steer_rate_lim: [10.0, 15.0, 20.0]                  # velocity list for steering angle rate limit interpolation in ascending order [m/s]
     acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss]
     velocity_time_constant: 0.3 # velocity dynamics time constant  for trajectory velocity modification [s]
 

control/mpc_lateral_controller/src/mpc.cpp

@@ -70,7 +70,8 @@ bool MPC::calculateMPC(
 
   /* solve quadratic optimization */
   Eigen::VectorXd Uex;
-  if (!executeOptimization(mpc_matrix, x0, prediction_dt, &Uex)) {
+  if (!executeOptimization(
+        mpc_matrix, x0, prediction_dt, &Uex, mpc_resampled_ref_traj, current_velocity)) {
     RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "optimization failed.");
     return false;
   }
@@ -636,7 +637,7 @@ MPCMatrix MPC::generateMPCMatrix(
  */
 bool MPC::executeOptimization(
   const MPCMatrix & m, const Eigen::VectorXd & x0, const double prediction_dt,
-  Eigen::VectorXd * Uex)
+  Eigen::VectorXd * Uex, const MPCTrajectory & traj, const double current_velocity)
 {
   using Eigen::MatrixXd;
   using Eigen::VectorXd;
@@ -666,12 +667,47 @@ bool 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);
+  };
+
   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 = VectorXd::Constant(DIM_U_N, -m_steer_rate_lim * prediction_dt);
-  VectorXd ubA = VectorXd::Constant(DIM_U_N, m_steer_rate_lim * prediction_dt);
-  lbA(0, 0) = m_raw_steer_cmd_prev - m_steer_rate_lim * m_ctrl_period;
-  ubA(0, 0) = m_raw_steer_cmd_prev + m_steer_rate_lim * m_ctrl_period;
+
+  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;
 
   auto t_start = std::chrono::system_clock::now();
   bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, *Uex);

control/mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -75,10 +75,29 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node}
   /* mpc parameters */
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo();
   const double wheelbase = vehicle_info.wheel_base_m;
-  const double steer_rate_lim_dps = node_->declare_parameter<double>("steer_rate_lim_dps");
   constexpr double deg2rad = static_cast<double>(M_PI) / 180.0;
   m_mpc.m_steer_lim = vehicle_info.max_steer_angle_rad;
-  m_steer_rate_lim = steer_rate_lim_dps * deg2rad;
+
+  // steer rate limit depending on curvature
+  const auto steer_rate_lim_dps_list_by_curvature =
+    node_->declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_curvature");
+  const auto curvature_list_for_steer_rate_lim =
+    node_->declare_parameter<std::vector<double>>("curvature_list_for_steer_rate_lim");
+  for (size_t i = 0; i < steer_rate_lim_dps_list_by_curvature.size(); ++i) {
+    m_mpc.m_steer_rate_lim_map_by_curvature.emplace_back(
+      curvature_list_for_steer_rate_lim.at(i),
+      steer_rate_lim_dps_list_by_curvature.at(i) * deg2rad);
+  }
+
+  // steer rate limit depending on velocity
+  const auto steer_rate_lim_dps_list_by_velocity =
+    node_->declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_velocity");
+  const auto velocity_list_for_steer_rate_lim =
+    node_->declare_parameter<std::vector<double>>("velocity_list_for_steer_rate_lim");
+  for (size_t i = 0; i < steer_rate_lim_dps_list_by_velocity.size(); ++i) {
+    m_mpc.m_steer_rate_lim_map_by_velocity.emplace_back(
+      velocity_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_velocity.at(i) * deg2rad);
+  }
 
   /* vehicle model setup */
   const std::string vehicle_model_type =
@@ -204,17 +223,6 @@ trajectory_follower::LateralOutput MpcLateralController::run(
     m_is_ctrl_cmd_prev_initialized = true;
   }
 
-  const bool is_vehicle_stopped = std::fabs(m_current_kinematic_state.twist.twist.linear.x) < 0.01;
-
-  // if the vehicle is stopped, set steering angle rate limit to a large value to get a proper
-  // steering value from mpc.
-  // TODO(someone): solve mpc cannot create output in low steering rate limit problem
-  if (is_vehicle_stopped) {
-    m_mpc.m_steer_rate_lim = std::numeric_limits<double>::max();
-  } else {
-    m_mpc.m_steer_rate_lim = m_steer_rate_lim;
-  }
-
   const bool is_mpc_solved = m_mpc.calculateMPC(
     m_current_steering, m_current_kinematic_state.twist.twist.linear.x,
     m_current_kinematic_state.pose.pose, ctrl_cmd, predicted_traj, debug_values);

control/mpc_lateral_controller/test/test_mpc.cpp

@@ -168,7 +168,8 @@ class MPCTest : public ::testing::Test
     mpc.m_admissible_position_error = admissible_position_error;
     mpc.m_admissible_yaw_error_rad = admissible_yaw_error_rad;
     mpc.m_steer_lim = steer_lim;
-    mpc.m_steer_rate_lim = steer_rate_lim;
+    mpc.m_steer_rate_lim_map_by_curvature.emplace_back(0.0, steer_rate_lim);
+    mpc.m_steer_rate_lim_map_by_velocity.emplace_back(0.0, steer_rate_lim);
     mpc.m_ctrl_period = ctrl_period;
     mpc.m_use_steer_prediction = use_steer_prediction;
     // Init filters

control/trajectory_follower_node/param/lateral/mpc.param.yaml

@@ -47,7 +47,10 @@
     vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics
     input_delay: 0.24                # steering input delay time for delay compensation
     vehicle_model_steer_tau: 0.3     # steering dynamics time constant (1d approximation) [s]
-    steer_rate_lim_dps: 40.0         # steering angle rate limit [deg/s]
+    steer_rate_lim_dps_list_by_curvature: [10.0, 20.0, 30.0]            # steering angle rate limit list depending on curvature [deg/s]
+    curvature_list_for_steer_rate_lim: [0.001, 0.002, 0.01]              # curvature list for steering angle rate limit interpolation in ascending order [/m]
+    steer_rate_lim_dps_list_by_velocity: [40.0, 30.0, 20.0]             # steering angle rate limit list depending on velocity [deg/s]
+    velocity_list_for_steer_rate_lim: [10.0, 15.0, 20.0]                  # velocity list for steering angle rate limit interpolation in ascending order [m/s]
     acceleration_limit: 2.0          # acceleration limit for trajectory velocity modification [m/ss]
     velocity_time_constant: 0.3      # velocity dynamics time constant  for trajectory velocity modification [s]
 

planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp

@@ -100,10 +100,14 @@ MarkerArray createShiftLineMarkerArray(
   const float & g, const float & b, const float & w);
 
 MarkerArray createShiftLengthMarkerArray(
-  const std::vector<double> & shift_distance,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & reference, std::string && ns,
+  const std::vector<double> & shift_distance, const PathWithLaneId & reference, std::string && ns,
   const float & r, const float & g, const float & b);
 
+MarkerArray createShiftGradMarkerArray(
+  const std::vector<double> & grad, const std::vector<double> & shift_distance,
+  const PathWithLaneId & reference, std::string && ns, const float & r, const float & g,
+  const float & b);
+
 MarkerArray createLaneletsAreaMarkerArray(
   const std::vector<lanelet::ConstLanelet> & lanelets, std::string && ns, const float & r,
   const float & g, const float & b);

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -118,8 +118,8 @@ struct BehaviorPathPlannerParameters
   double turn_signal_shift_length_threshold;
   bool turn_signal_on_swerving;
 
-  double enable_akima_spline_first;
-  double enable_cog_on_centerline;
+  bool enable_akima_spline_first;
+  bool enable_cog_on_centerline;
   double input_path_interval;
   double output_path_interval;
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -496,6 +496,14 @@ class AvoidanceModule : public SceneModuleInterface
    */
   void addNewShiftLines(PathShifter & path_shifter, const AvoidLineArray & shift_lines) const;
 
+  /**
+   * @brief validate shift lines.
+   * @param new shift lines.
+   * @param path shifter.
+   * @return result. if there is huge gap between the ego position and candidate path, return false.
+   */
+  bool isValidShiftLine(const AvoidLineArray & shift_lines, const PathShifter & shifter) const;
+
   // generate output data
 
   /**

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -514,11 +514,22 @@ struct DebugData
   AvoidLineArray trim_similar_grad_shift_third;
   AvoidLineArray trim_momentary_return;
   AvoidLineArray trim_too_sharp_shift;
+
+  // shift length
   std::vector<double> pos_shift;
   std::vector<double> neg_shift;
   std::vector<double> total_shift;
   std::vector<double> output_shift;
 
+  // shift grad
+  std::vector<double> pos_shift_grad;
+  std::vector<double> neg_shift_grad;
+  std::vector<double> total_forward_grad;
+  std::vector<double> total_backward_grad;
+
+  // shift path
+  std::vector<double> proposed_spline_shift;
+
   bool exist_adjacent_objects{false};
 
   // future pose

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp

@@ -104,6 +104,12 @@ void filterTargetObjects(
   ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug,
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters);
+
+double extendToRoadShoulderDistanceWithPolygon(
+  const std::shared_ptr<route_handler::RouteHandler> & rh,
+  const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance,
+  const geometry_msgs::msg::Point & overhang_pos,
+  const lanelet::BasicPoint3d & overhang_basic_pose);
 }  // namespace behavior_path_planner::utils::avoidance
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_

planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp

@@ -281,46 +281,77 @@ MarkerArray createAvoidLineMarkerArray(
   const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g,
   const float & b, const double & w)
 {
-  AvoidLineArray shift_lines_local = shift_lines;
-  if (shift_lines_local.empty()) {
-    shift_lines_local.push_back(AvoidLine());
+  MarkerArray msg;
+
+  if (shift_lines.empty()) {
+    return msg;
   }
 
-  int32_t id{0};
-  const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();
-  MarkerArray msg;
+  auto marker = createDefaultMarker(
+    "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::CUBE,
+    createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(r, g, b, 0.9));
 
-  for (const auto & sl : shift_lines_local) {
-    // ROS_ERROR("sl: s = (%f, %f), g = (%f, %f)", sl.start.x, sl.start.y, sl.end.x, sl.end.y);
-    auto basic_marker = createDefaultMarker(
-      "map", current_time, ns, 0L, Marker::CUBE, createMarkerScale(0.5, 0.5, 0.5),
-      createMarkerColor(r, g, b, 0.9));
-    basic_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
+  int32_t shift_line_id{0};
+  int32_t marker_id{0};
+  for (const auto & s : shift_lines) {
+    // shift line
     {
-      // start point
-      auto marker_s = basic_marker;
-      marker_s.id = id++;
-      marker_s.pose = sl.start;
-      marker_s.pose = calcOffsetPose(marker_s.pose, 0.0, sl.start_shift_length, 0.0);
-      msg.markers.push_back(marker_s);
-
-      // end point
-      auto marker_e = basic_marker;
-      marker_e.id = id++;
-      marker_e.pose = sl.end;
-      marker_e.pose = calcOffsetPose(marker_e.pose, 0.0, sl.end_shift_length, 0.0);
-      msg.markers.push_back(marker_e);
-
-      // start-to-end line
-      auto marker_l = basic_marker;
-      marker_l.id = id++;
-      marker_l.type = Marker::LINE_STRIP;
-      marker_l.scale = tier4_autoware_utils::createMarkerScale(w, 0.0, 0.0);
-      marker_l.points.push_back(marker_s.pose.position);
-      marker_l.points.push_back(marker_e.pose.position);
-      msg.markers.push_back(marker_l);
+      auto m = marker;
+      m.id = marker_id++;
+      m.type = Marker::LINE_STRIP;
+      m.scale = createMarkerScale(w, 0.0, 0.0);
+      m.points.push_back(calcOffsetPose(s.start, 0.0, s.start_shift_length, 0.0).position);
+      m.points.push_back(calcOffsetPose(s.end, 0.0, s.end_shift_length, 0.0).position);
+      msg.markers.push_back(m);
     }
-    // current_shift = sp.length;
+
+    // start point
+    {
+      auto m = marker;
+      m.id = marker_id++;
+      m.type = Marker::CUBE;
+      m.scale = createMarkerScale(0.2, 0.2, 0.2);
+      m.pose = calcOffsetPose(s.start, 0.0, s.start_shift_length, 0.0);
+      msg.markers.push_back(m);
+    }
+
+    // end point
+    {
+      auto m = marker;
+      m.id = marker_id++;
+      m.type = Marker::CUBE;
+      m.scale = createMarkerScale(0.2, 0.2, 0.2);
+      m.pose = calcOffsetPose(s.end, 0.0, s.end_shift_length, 0.0);
+      msg.markers.push_back(m);
+    }
+
+    // start text
+    {
+      auto m = marker;
+      std::ostringstream string_stream;
+      string_stream << "(S):" << shift_line_id;
+      m.id = marker_id++;
+      m.type = Marker::TEXT_VIEW_FACING;
+      m.scale = createMarkerScale(0.3, 0.3, 0.3);
+      m.pose = calcOffsetPose(s.start, 0.0, s.start_shift_length + 0.3, 0.0);
+      m.text = string_stream.str();
+      msg.markers.push_back(m);
+    }
+
+    // end text
+    {
+      auto m = marker;
+      std::ostringstream string_stream;
+      string_stream << "(E):" << shift_line_id;
+      m.id = marker_id++;
+      m.type = Marker::TEXT_VIEW_FACING;
+      m.scale = createMarkerScale(0.3, 0.3, 0.3);
+      m.pose = calcOffsetPose(s.end, 0.0, s.end_shift_length - 0.3, 0.0);
+      m.text = string_stream.str();
+      msg.markers.push_back(m);
+    }
+
+    shift_line_id++;
   }
 
   return msg;