Merge pull request #1810 from tier4/tmp/beta/v0.29.0-2

feat: cherry-pick change of beta/v4.0.2

control/autoware_mpc_lateral_controller/README.md

@@ -95,6 +95,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 | use_steer_prediction      | boolean | flag for using steer prediction (do not use steer measurement)              | false         |
 | admissible_position_error | double  | stop vehicle when following position error is larger than this value [m]    | 5.0           |
 | admissible_yaw_error_rad  | double  | stop vehicle when following yaw angle error is larger than this value [rad] | 1.57          |
+| use_delayed_initial_state | boolean | flag to use x0_delayed as initial state for predicted trajectory            | true          |
 
 #### Path Smoothing
 
@@ -202,6 +203,12 @@ Defined in the `steering_offset` namespace. This logic is designed as simple as
 | cf            | double | front cornering power [N/rad]               | 155494.663    |
 | cr            | double | rear cornering power [N/rad]                | 155494.663    |
 
+#### Debug
+
+| Name                       | Type    | Description                                                                       | Default value |
+| :------------------------- | :------ | :-------------------------------------------------------------------------------- | :------------ |
+| publish_debug_trajectories | boolean | publish predicted trajectory and resampled reference trajectory for debug purpose | true          |
+
 ### How to tune MPC parameters
 
 #### Set kinematics information

control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp

@@ -222,6 +222,7 @@ class MPC
   double m_min_prediction_length = 5.0;  // Minimum prediction distance.
 
   rclcpp::Publisher<Trajectory>::SharedPtr m_debug_frenet_predicted_trajectory_pub;
+  rclcpp::Publisher<Trajectory>::SharedPtr m_debug_resampled_reference_trajectory_pub;
   /**
    * @brief Get variables for MPC calculation.
    * @param trajectory The reference trajectory.
@@ -341,11 +342,14 @@ class MPC
    * @param Uex optimized input.
    * @param mpc_resampled_ref_traj reference trajectory resampled in the mpc time-step
    * @param dt delta time used in the mpc problem.
+   * @param coordinate String specifying the coordinate system ("world" or "frenet", default is
+   * "world")
    * @return predicted path
    */
   Trajectory calculatePredictedTrajectory(
     const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
-    const MPCTrajectory & mpc_resampled_ref_traj, const double dt) const;
+    const MPCTrajectory & reference_trajectory, const double dt,
+    const std::string & coordinate = "world") const;
 
   /**
    * @brief Check if the MPC matrix has any invalid values.
@@ -426,7 +430,11 @@ class MPC
   double ego_nearest_dist_threshold = 3.0;  // Threshold for nearest index search based on distance.
   double ego_nearest_yaw_threshold = M_PI_2;  // Threshold for nearest index search based on yaw.
 
-  bool m_debug_publish_predicted_trajectory = false;  // Flag to publish debug predicted trajectory
+  bool m_use_delayed_initial_state =
+    true;  // Flag to use x0_delayed as initial state for predicted trajectory
+
+  bool m_publish_debug_trajectories = false;  // Flag to publish predicted trajectory and
+                                              // resampled reference trajectory for debug purpose
 
   //!< Constructor.
   explicit MPC(rclcpp::Node & node);

control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml

@@ -5,6 +5,7 @@
     use_steer_prediction: false     # flag for using steer prediction (do not use steer measurement)
     admissible_position_error: 5.0  # stop mpc calculation when error is larger than the following value
     admissible_yaw_error_rad: 1.57  # stop mpc calculation when error is larger than the following value
+    use_delayed_initial_state: true # flag to use x0_delayed as initial state for predicted trajectory
 
     # -- path smoothing --
     enable_path_smoothing: false   # flag for path smoothing
@@ -84,4 +85,4 @@
     cf: 155494.663
     cr: 155494.663
 
-    debug_publish_predicted_trajectory: false  # publish debug predicted trajectory in Frenet coordinate
+    publish_debug_trajectories: true # flag to publish predicted trajectory and resampled reference trajectory for debug purpose

control/autoware_mpc_lateral_controller/src/mpc.cpp

@@ -33,6 +33,8 @@ MPC::MPC(rclcpp::Node & node)
 {
   m_debug_frenet_predicted_trajectory_pub = node.create_publisher<Trajectory>(
     "~/debug/predicted_trajectory_in_frenet_coordinate", rclcpp::QoS(1));
+  m_debug_resampled_reference_trajectory_pub =
+    node.create_publisher<Trajectory>("~/debug/resampled_reference_trajectory", rclcpp::QoS(1));
 }
 
 bool MPC::calculateMPC(
@@ -104,8 +106,19 @@ bool MPC::calculateMPC(
   m_raw_steer_cmd_prev = Uex(0);
 
   /* calculate predicted trajectory */
-  predicted_trajectory =
-    calculatePredictedTrajectory(mpc_matrix, x0, Uex, mpc_resampled_ref_trajectory, prediction_dt);
+  Eigen::VectorXd initial_state = m_use_delayed_initial_state ? x0_delayed : x0;
+  predicted_trajectory = calculatePredictedTrajectory(
+    mpc_matrix, initial_state, Uex, mpc_resampled_ref_trajectory, prediction_dt, "world");
+
+  // Publish predicted trajectories in different coordinates for debugging purposes
+  if (m_publish_debug_trajectories) {
+    // Calculate and publish predicted trajectory in Frenet coordinate
+    auto predicted_trajectory_frenet = calculatePredictedTrajectory(
+      mpc_matrix, initial_state, Uex, mpc_resampled_ref_trajectory, prediction_dt, "frenet");
+    predicted_trajectory_frenet.header.stamp = m_clock->now();
+    predicted_trajectory_frenet.header.frame_id = "map";
+    m_debug_frenet_predicted_trajectory_pub->publish(predicted_trajectory_frenet);
+  }
 
   // prepare diagnostic message
   diagnostic =
@@ -310,6 +323,13 @@ std::pair<bool, MPCTrajectory> MPC::resampleMPCTrajectoryByTime(
     warn_throttle("calculateMPC: mpc resample error. stop mpc calculation. check code!");
     return {false, {}};
   }
+  // Publish resampled reference trajectory for debug purpose.
+  if (m_publish_debug_trajectories) {
+    auto converted_output = MPCUtils::convertToAutowareTrajectory(output);
+    converted_output.header.stamp = m_clock->now();
+    converted_output.header.frame_id = "map";
+    m_debug_resampled_reference_trajectory_pub->publish(converted_output);
+  }
   return {true, output};
 }
 
@@ -785,12 +805,21 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory(
 
 Trajectory MPC::calculatePredictedTrajectory(
   const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
-  const MPCTrajectory & reference_trajectory, const double dt) const
+  const MPCTrajectory & reference_trajectory, const double dt, const std::string & coordinate) const
 {
-  const auto predicted_mpc_trajectory =
-    m_vehicle_model_ptr->calculatePredictedTrajectoryInWorldCoordinate(
+  MPCTrajectory predicted_mpc_trajectory;
+
+  if (coordinate == "world") {
+    predicted_mpc_trajectory = m_vehicle_model_ptr->calculatePredictedTrajectoryInWorldCoordinate(
+      mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory,
+      dt);
+  } else if (coordinate == "frenet") {
+    predicted_mpc_trajectory = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate(
       mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory,
       dt);
+  } else {
+    throw std::invalid_argument("Invalid coordinate system specified. Use 'world' or 'frenet'.");
+  }
 
   // do not over the reference trajectory
   const auto predicted_length = MPCUtils::calcMPCTrajectoryArcLength(reference_trajectory);
@@ -799,18 +828,6 @@ Trajectory MPC::calculatePredictedTrajectory(
 
   const auto predicted_trajectory = MPCUtils::convertToAutowareTrajectory(clipped_trajectory);
 
-  // Publish trajectory in relative coordinate for debug purpose.
-  if (m_debug_publish_predicted_trajectory) {
-    const auto frenet = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate(
-      mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory,
-      dt);
-    auto frenet_clipped = MPCUtils::convertToAutowareTrajectory(
-      MPCUtils::clipTrajectoryByLength(frenet, predicted_length));
-    frenet_clipped.header.stamp = m_clock->now();
-    frenet_clipped.header.frame_id = "map";
-    m_debug_frenet_predicted_trajectory_pub->publish(frenet_clipped);
-  }
-
   return predicted_trajectory;
 }
 

control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -134,7 +134,9 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node)
   m_mpc->ego_nearest_dist_threshold = m_ego_nearest_dist_threshold;
   m_mpc->ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold;
 
-  m_mpc->m_debug_publish_predicted_trajectory = dp_bool("debug_publish_predicted_trajectory");
+  m_mpc->m_use_delayed_initial_state = dp_bool("use_delayed_initial_state");
+
+  m_mpc->m_publish_debug_trajectories = dp_bool("publish_debug_trajectories");
 
   m_pub_predicted_traj = node.create_publisher<Trajectory>("~/output/predicted_trajectory", 1);
   m_pub_debug_values =

control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml

@@ -5,6 +5,7 @@
     use_steer_prediction: false     # flag for using steer prediction (do not use steer measurement)
     admissible_position_error: 5.0  # stop mpc calculation when error is larger than the following value
     admissible_yaw_error_rad: 1.57  # stop mpc calculation when error is larger than the following value
+    use_delayed_initial_state: true # flag to use x0_delayed as initial state for predicted trajectory
 
     # -- path smoothing --
     enable_path_smoothing: false   # flag for path smoothing
@@ -75,4 +76,4 @@
       average_num: 1000
       steering_offset_limit: 0.02
 
-    debug_publish_predicted_trajectory: false  # publish debug predicted trajectory in Frenet coordinate
+    publish_debug_trajectories: true # flag to publish predicted trajectory and resampled reference trajectory for debug purpose

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -209,6 +209,11 @@ void MultiObjectTracker::onTrigger()
   const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list);
   if (!is_objects_ready) return;
   onMessage(objects_list);
+  // Publish without delay compensation
+  if (!publish_timer_) {
+    const auto latest_object_time = rclcpp::Time(objects_list.back().second.header.stamp);
+    checkAndPublish(latest_object_time);
+  }
 }
 
 void MultiObjectTracker::onTimer()

planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp

@@ -324,6 +324,9 @@ class RouteHandler
   lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const;
   lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const;
 
+  Pose get_pose_from_2d_arc_length(
+    const lanelet::ConstLanelets & lanelet_sequence, const double s) const;
+
 private:
   // MUST
   lanelet::routing::RoutingGraphPtr routing_graph_ptr_;

planning/autoware_route_handler/src/route_handler.cpp

@@ -46,6 +46,8 @@ namespace autoware::route_handler
 {
 namespace
 {
+using autoware::universe_utils::createPoint;
+using autoware::universe_utils::createQuaternionFromYaw;
 using autoware_planning_msgs::msg::LaneletPrimitive;
 using autoware_planning_msgs::msg::Path;
 using geometry_msgs::msg::Pose;
@@ -2225,4 +2227,29 @@ std::optional<lanelet::routing::LaneletPath> RouteHandler::findDrivableLanePath(
   if (route) return route->shortestPath();
   return {};
 }
+
+Pose RouteHandler::get_pose_from_2d_arc_length(
+  const lanelet::ConstLanelets & lanelet_sequence, const double s) const
+{
+  double accumulated_distance2d = 0;
+  for (const auto & llt : lanelet_sequence) {
+    const auto & centerline = llt.centerline();
+    for (auto it = centerline.begin(); std::next(it) != centerline.end(); ++it) {
+      const auto pt = *it;
+      const auto next_pt = *std::next(it);
+      const double distance2d = lanelet::geometry::distance2d(to2D(pt), to2D(next_pt));
+      if (accumulated_distance2d + distance2d > s) {
+        const double ratio = (s - accumulated_distance2d) / distance2d;
+        const auto interpolated_pt = pt.basicPoint() * (1 - ratio) + next_pt.basicPoint() * ratio;
+        const auto yaw = std::atan2(next_pt.y() - pt.y(), next_pt.x() - pt.x());
+        Pose pose;
+        pose.position = createPoint(interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z());
+        pose.orientation = createQuaternionFromYaw(yaw);
+        return pose;
+      }
+      accumulated_distance2d += distance2d;
+    }
+  }
+  return Pose{};
+}
 }  // namespace autoware::route_handler

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp

@@ -44,11 +44,14 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface(
 {
 }
 
+// cppcheck-suppress unusedFunction
 bool AvoidanceByLaneChangeInterface::isExecutionRequested() const
 {
   return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() &&
          module_type_->isValidPath();
 }
+
+// cppcheck-suppress unusedFunction
 void AvoidanceByLaneChangeInterface::processOnEntry()
 {
   waitApproval();

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp

@@ -187,8 +187,8 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
   avoidance_parameters_ = std::make_shared<AvoidanceByLCParameters>(p);
 }
 
-std::unique_ptr<SceneModuleInterface>
-AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance()
+// cppcheck-suppress unusedFunction
+SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance()
 {
   return std::make_unique<AvoidanceByLaneChangeInterface>(
     name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_,

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp

@@ -32,6 +32,8 @@ using autoware::behavior_path_planner::LaneChangeModuleManager;
 using autoware::behavior_path_planner::LaneChangeModuleType;
 using autoware::behavior_path_planner::SceneModuleInterface;
 
+using SMIPtr = std::unique_ptr<SceneModuleInterface>;
+
 class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager
 {
 public:
@@ -44,7 +46,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager
 
   void init(rclcpp::Node * node) override;
 
-  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
+  SMIPtr createNewSceneModuleInstance() override;
 
 private:
   std::shared_ptr<AvoidanceByLCParameters> avoidance_parameters_;

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -20,6 +20,7 @@
 #include "autoware/behavior_path_planner_common/utils/utils.hpp"
 #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp"
 
+#include <autoware/behavior_path_lane_change_module/utils/calculation.hpp>
 #include <autoware/behavior_path_lane_change_module/utils/utils.hpp>
 #include <autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
@@ -33,14 +34,55 @@
 #include <optional>
 #include <utility>
 
+namespace
+{
+geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose)
+{
+  geometry_msgs::msg::Point32 p;
+  p.x = static_cast<float>(pose.position.x);
+  p.y = static_cast<float>(pose.position.y);
+  p.z = static_cast<float>(pose.position.z);
+  return p;
+};
+
+geometry_msgs::msg::Polygon create_execution_area(
+  const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
+  const geometry_msgs::msg::Pose & pose, double additional_lon_offset, double additional_lat_offset)
+{
+  const double & base_to_front = vehicle_info.max_longitudinal_offset_m;
+  const double & width = vehicle_info.vehicle_width_m;
+  const double & base_to_rear = vehicle_info.rear_overhang_m;
+
+  // if stationary object, extend forward and backward by the half of lon length
+  const double forward_lon_offset = base_to_front + additional_lon_offset;
+  const double backward_lon_offset = -base_to_rear;
+  const double lat_offset = width / 2.0 + additional_lat_offset;
+
+  const auto p1 =
+    autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0);
+  const auto p2 =
+    autoware::universe_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0);
+  const auto p3 =
+    autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0);
+  const auto p4 =
+    autoware::universe_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0);
+  geometry_msgs::msg::Polygon polygon;
+
+  polygon.points.push_back(create_point32(p1));
+  polygon.points.push_back(create_point32(p2));
+  polygon.points.push_back(create_point32(p3));
+  polygon.points.push_back(create_point32(p4));
+
+  return polygon;
+}
+}  // namespace
+
 namespace autoware::behavior_path_planner
 {
 using autoware::behavior_path_planner::Direction;
 using autoware::behavior_path_planner::LaneChangeModuleType;
 using autoware::behavior_path_planner::ObjectInfo;
 using autoware::behavior_path_planner::Point2d;
-using autoware::behavior_path_planner::utils::lane_change::debug::createExecutionArea;
-namespace utils = autoware::behavior_path_planner::utils;
 
 AvoidanceByLaneChange::AvoidanceByLaneChange(
   const std::shared_ptr<LaneChangeParameters> & parameters,
@@ -82,9 +124,9 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
 
   const auto & nearest_object = data.target_objects.front();
   const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object);
-  const auto minimum_lane_change_length = calcMinimumLaneChangeLength();
+  const auto minimum_lane_change_length = calc_minimum_dist_buffer();
 
-  lane_change_debug_.execution_area = createExecutionArea(
+  lane_change_debug_.execution_area = create_execution_area(
     getCommonParam().vehicle_info, getEgoPose(),
     std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset());
 
@@ -273,16 +315,11 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_
   return avoidance_helper_->getMinAvoidanceDistance(shift_length);
 }
 
-double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
+double AvoidanceByLaneChange::calc_minimum_dist_buffer() const
 {
-  const auto current_lanes = get_current_lanes();
-  if (current_lanes.empty()) {
-    RCLCPP_DEBUG(logger_, "no empty lanes");
-    return std::numeric_limits<double>::infinity();
-  }
-
-  return utils::lane_change::calcMinimumLaneChangeLength(
-    getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_);
+  const auto [_, dist_buffer] = utils::lane_change::calculation::calc_lc_length_and_dist_buffer(
+    common_data_ptr_, get_current_lanes());
+  return dist_buffer.min;
 }
 
 double AvoidanceByLaneChange::calcLateralOffset() const

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp

@@ -63,7 +63,7 @@ class AvoidanceByLaneChange : public NormalLaneChange
   void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const;
 
   double calcMinAvoidanceLength(const ObjectData & nearest_object) const;
-  double calcMinimumLaneChangeLength() const;
+  double calc_minimum_dist_buffer() const;
   double calcLateralOffset() const;
 };
 }  // namespace autoware::behavior_path_planner