[pull] main from autowarefoundation:main

.github/workflows/clang-tidy-differential.yaml

@@ -38,7 +38,7 @@ jobs:
       - name: Get modified files
         id: get-modified-files
         run: |
-          echo "changed_files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' || true)" >> $GITHUB_OUTPUT
+          echo "changed_files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | tr '\n' ' ' || true)" >> $GITHUB_OUTPUT
         shell: bash
 
       - name: Run clang-tidy

control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp

@@ -100,7 +100,9 @@ class LaneDepartureCheckerNode : public rclcpp::Node
 
   // Publisher
   autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"};
-  autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this};
+  autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{
+    this, "~/debug/processing_time_ms_diag"};
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;
 
   // Timer
   rclcpp::TimerBase::SharedPtr timer_;

control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -170,6 +170,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
   lane_departure_checker_->setParam(param_, vehicle_info);
 
   // Publisher
+  processing_time_publisher_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
   // Nothing
 
   // Diagnostic Updater
@@ -347,7 +349,11 @@ void LaneDepartureCheckerNode::onTimer()
   }
 
   processing_time_map["Total"] = stop_watch.toc("Total");
-  processing_time_publisher_.publish(processing_time_map);
+  processing_diag_publisher_.publish(processing_time_map);
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = processing_time_map["Total"];
+  processing_time_publisher_->publish(processing_time_msg);
 }
 
 rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter(

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

@@ -271,7 +271,7 @@ class MPC
    */
   std::pair<bool, VectorXd> executeOptimization(
     const MPCMatrix & mpc_matrix, const VectorXd & x0, const double prediction_dt,
-    const MPCTrajectory & trajectory, const double current_velocity);
+    const MPCTrajectory & trajectory);
 
   /**
    * @brief Resample the trajectory with the MPC resampling time.
@@ -386,8 +386,7 @@ class MPC
    * @param reference_trajectory The reference trajectory.
    * @param current_velocity current velocity of ego.
    */
-  VectorXd calcSteerRateLimitOnTrajectory(
-    const MPCTrajectory & trajectory, const double current_velocity) const;
+  VectorXd calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const;
 
   //!< @brief logging with warn and return false
   template <typename... Args>

control/autoware_mpc_lateral_controller/src/mpc.cpp

@@ -76,9 +76,8 @@ bool MPC::calculateMPC(
   const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt);
 
   // solve Optimization problem
-  const auto [success_opt, Uex] = executeOptimization(
-    mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory,
-    current_kinematics.twist.twist.linear.x);
+  const auto [success_opt, Uex] =
+    executeOptimization(mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory);
   if (!success_opt) {
     return fail_warn_throttle("optimization failed. Stop MPC.");
   }
@@ -544,8 +543,7 @@ MPCMatrix MPC::generateMPCMatrix(
  * [    -au_lim * dt    ] < [uN-uN-1] < [     au_lim * dt    ] (*N... DIM_U)
  */
 std::pair<bool, VectorXd> MPC::executeOptimization(
-  const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj,
-  const double current_velocity)
+  const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj)
 {
   VectorXd Uex;
 
@@ -578,7 +576,7 @@ std::pair<bool, VectorXd> MPC::executeOptimization(
   VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim);   // max steering angle
 
   // steering angle rate limit
-  VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity);
+  VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj);
   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;
@@ -730,8 +728,7 @@ double MPC::calcDesiredSteeringRate(
   return steer_rate;
 }
 
-VectorXd MPC::calcSteerRateLimitOnTrajectory(
-  const MPCTrajectory & trajectory, const double current_velocity) const
+VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const
 {
   const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) {
     std::vector<double> reference, limits;
@@ -765,13 +762,6 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory(
     return reference.back();
   };
 
-  // when the vehicle is stopped, no steering rate limit.
-  constexpr double steer_rate_lim = 5.0;
-  const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01;
-  if (is_vehicle_stopped) {
-    return steer_rate_lim * VectorXd::Ones(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) {

control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp

@@ -64,7 +64,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
 {
 public:
   /// \param node Reference to the node used only for the component and parameter initialization.
-  explicit PidLongitudinalController(rclcpp::Node & node);
+  explicit PidLongitudinalController(
+    rclcpp::Node & node, std::shared_ptr<diagnostic_updater::Updater> diag_updater);
 
 private:
   struct Motion
@@ -236,8 +237,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   std::shared_ptr<rclcpp::Time> m_last_running_time{std::make_shared<rclcpp::Time>(clock_->now())};
 
   // Diagnostic
-
-  diagnostic_updater::Updater diagnostic_updater_;
+  std::shared_ptr<diagnostic_updater::Updater>
+    diag_updater_{};  // Diagnostic updater for publishing diagnostic data.
   struct DiagnosticData
   {
     double trans_deviation{0.0};  // translation deviation between nearest point and current_pose

control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -27,14 +27,16 @@
 
 namespace autoware::motion::control::pid_longitudinal_controller
 {
-PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
+PidLongitudinalController::PidLongitudinalController(
+  rclcpp::Node & node, std::shared_ptr<diagnostic_updater::Updater> diag_updater)
 : node_parameters_(node.get_node_parameters_interface()),
   clock_(node.get_clock()),
-  logger_(node.get_logger().get_child("longitudinal_controller")),
-  diagnostic_updater_(&node)
+  logger_(node.get_logger().get_child("longitudinal_controller"))
 {
   using std::placeholders::_1;
 
+  diag_updater_ = diag_updater;
+
   // parameters timer
   m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double();
 
@@ -432,7 +434,7 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run(
   publishDebugData(ctrl_cmd, control_data);
 
   // diagnostic
-  diagnostic_updater_.force_update();
+  diag_updater_->force_update();
 
   return output;
 }
@@ -1150,8 +1152,8 @@ void PidLongitudinalController::updateDebugVelAcc(const ControlData & control_da
 
 void PidLongitudinalController::setupDiagnosticUpdater()
 {
-  diagnostic_updater_.setHardwareID("autoware_pid_longitudinal_controller");
-  diagnostic_updater_.add("control_state", this, &PidLongitudinalController::checkControlState);
+  diag_updater_->setHardwareID("autoware_pid_longitudinal_controller");
+  diag_updater_->add("control_state", this, &PidLongitudinalController::checkControlState);
 }
 
 void PidLongitudinalController::checkControlState(

control/autoware_trajectory_follower_node/src/controller_node.cpp

@@ -57,7 +57,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
   switch (longitudinal_controller_mode) {
     case LongitudinalControllerMode::PID: {
       longitudinal_controller_ =
-        std::make_shared<pid_longitudinal_controller::PidLongitudinalController>(*this);
+        std::make_shared<pid_longitudinal_controller::PidLongitudinalController>(
+          *this, diag_updater_);
       break;
     }
     default:

control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp

@@ -294,7 +294,12 @@ class PubSubNode : public rclcpp::Node
     const auto max_lat_acc_lim = *std::max_element(lat_acc_lim.begin(), lat_acc_lim.end());
     const auto max_lat_jerk_lim = *std::max_element(lat_jerk_lim.begin(), lat_jerk_lim.end());
 
+    // This test is designed to verify that the filter is applied correctly. However, if topic
+    // communication is delayed, the allowable range of change in the command values between the
+    // sender and receiver of the topic will vary, making the results dependent on processing time.
+    // We define the allowable error margin to account for this.
     constexpr auto threshold_scale = 1.1;
+
     // Output command must be smaller than maximum limit.
     // TODO(Horibe): check for each velocity range.
     if (std::abs(lon_vel) > 0.01) {
@@ -433,7 +438,7 @@ TEST_P(TestFixture, CheckFilterForSinCmd)
   //           << cmd_generator_.p_.steering.freq << " * dt + " << cmd_generator_.p_.steering.bias
   //           << ")" << std::endl;
 
-  for (size_t i = 0; i < 100; ++i) {
+  for (size_t i = 0; i < 30; ++i) {
     auto start_time = std::chrono::steady_clock::now();
 
     const bool reset_clock = (i == 0);
@@ -449,7 +454,13 @@ TEST_P(TestFixture, CheckFilterForSinCmd)
     std::chrono::milliseconds elapsed =
       std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
 
-    std::chrono::milliseconds sleep_duration = std::chrono::milliseconds{10} - elapsed;
+    // The value determines the period of the topic. The filter logic of vehicle_cmd_gate depends on
+    // real-time, and if the time from publishing to subscribing becomes too long, this test will
+    // fail (the test specification itself should be improved). To prevent processing bottlenecks,
+    // please set this value appropriately. It is set to 30ms because it occasionally fails at 10ms.
+    constexpr int running_ms = 30;
+
+    std::chrono::milliseconds sleep_duration = std::chrono::milliseconds{running_ms} - elapsed;
     if (sleep_duration.count() > 0) {
       std::this_thread::sleep_for(sleep_duration);
     }