From d20d901e019bddea7d811c4404f99e62ab4bf10b Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Tue, 25 Jun 2024 22:49:38 +0900 Subject: [PATCH 1/5] fix(lidar_centerpoint): fix constexpr related bugs (#7686) Signed-off-by: Ryuta Kambe --- .../test/test_postprocess_kernel.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp b/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp index fde4fcbee3d6c..e75aff416b8aa 100644 --- a/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp +++ b/perception/lidar_centerpoint/test/test_postprocess_kernel.cpp @@ -94,12 +94,12 @@ TEST_F(PostprocessKernelTest, SingleDetectionTest) constexpr float detection_x = 70.f; constexpr float detection_y = -38.4f; constexpr float detection_z = 1.0; - constexpr float detection_log_w = std::log(7.0); - constexpr float detection_log_l = std::log(1.0); - constexpr float detection_log_h = std::log(2.0); + const float detection_log_w = std::log(7.0); + const float detection_log_l = std::log(1.0); + const float detection_log_h = std::log(2.0); constexpr float detection_yaw = M_PI_4; - constexpr float detection_yaw_sin = std::sin(detection_yaw); - constexpr float detection_yaw_cos = std::sin(detection_yaw); + const float detection_yaw_sin = std::sin(detection_yaw); + const float detection_yaw_cos = std::sin(detection_yaw); constexpr float detection_vel_x = 5.0; constexpr float detection_vel_y = -5.0; @@ -240,9 +240,9 @@ TEST_F(PostprocessKernelTest, CircleNMSTest) constexpr float detection_x = 70.f; constexpr float detection_y = -38.4f; constexpr float detection_z = 1.0; - constexpr float detection_log_w = std::log(7.0); - constexpr float detection_log_l = std::log(1.0); - constexpr float detection_log_h = std::log(2.0); + const float detection_log_w = std::log(7.0); + const float detection_log_l = std::log(1.0); + const float detection_log_h = std::log(2.0); constexpr float detection_yaw1_sin = 0.0; constexpr float detection_yaw1_cos = 1.0; constexpr float detection_yaw2_sin = 1.0; From 1d1ee46f55f8c494f03bd0c1b2779e8bd0182bec Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 25 Jun 2024 22:50:59 +0900 Subject: [PATCH 2/5] chore(ci): fix error in `clang-tidy-differential` with diffs in multiple files (#7685) * chore(ci): output modified file list without newlines Signed-off-by: Kotaro Yoshimoto * chore(ci): test diffs Signed-off-by: Kotaro Yoshimoto * chore(ci): test diffs Signed-off-by: Kotaro Yoshimoto * chore(ci): revert test diff Signed-off-by: Kotaro Yoshimoto * chore(ci): revert test diff Signed-off-by: Kotaro Yoshimoto --------- Signed-off-by: Kotaro Yoshimoto --- .github/workflows/clang-tidy-differential.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml index e2e4fc478e0ee..60337cc572e7a 100644 --- a/.github/workflows/clang-tidy-differential.yaml +++ b/.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 From 1cff7ad1ff4d99e88f6257cf01e79aeef0b5e739 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 26 Jun 2024 09:45:01 +0900 Subject: [PATCH 3/5] fix(static_obstacle_avoidance): fix json schema (#7692) Signed-off-by: satoshi-ota --- .../schema/static_obstacle_avoidance.schema.json | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index 3e6f0ef1f61a3..266a4cf8a23ab 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -17,6 +17,7 @@ "default": "4.0" }, "path_generation_method": { + "type": "string", "enum": ["shift_line_base", "optimization_base", "both"], "description": "Path generation method.", "default": "shift_line_base" From cd86f0aa4c2f8d52e69ee3836236e8afd0b30c43 Mon Sep 17 00:00:00 2001 From: Zhe Shen <80969277+HansOersted@users.noreply.github.com> Date: Wed, 26 Jun 2024 10:48:50 +0900 Subject: [PATCH 4/5] fix(autoware_pid_longitudinal_controller, autoware_trajectory_follower_node): unite diagnostic_updater_ in PID and MPC. (#7674) * diag_updater_ added in PID Signed-off-by: Zhe Shen * correct the pointer form Signed-off-by: Zhe Shen * pre-commit Signed-off-by: Zhe Shen --------- Signed-off-by: Zhe Shen --- .../pid_longitudinal_controller.hpp | 7 ++++--- .../src/pid_longitudinal_controller.cpp | 14 ++++++++------ .../src/controller_node.cpp | 3 ++- 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 9291d538b022f..5805ef7db9f86 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/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 diag_updater); private: struct Motion @@ -236,8 +237,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro std::shared_ptr m_last_running_time{std::make_shared(clock_->now())}; // Diagnostic - - diagnostic_updater::Updater diagnostic_updater_; + std::shared_ptr + diag_updater_{}; // Diagnostic updater for publishing diagnostic data. struct DiagnosticData { double trans_deviation{0.0}; // translation deviation between nearest point and current_pose diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 78c7ccf832514..93496c85c741b 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/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 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( diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp index c1e5fe646cdaa..d5167f5096786 100644 --- a/control/autoware_trajectory_follower_node/src/controller_node.cpp +++ b/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(*this); + std::make_shared( + *this, diag_updater_); break; } default: From 69258bd92cb8a0ff8320df9b2302db72975e027f Mon Sep 17 00:00:00 2001 From: Zhe Shen <80969277+HansOersted@users.noreply.github.com> Date: Wed, 26 Jun 2024 11:00:20 +0900 Subject: [PATCH 5/5] fix(autoware_mpc_lateral_controller): delete the zero speed constraint (#7673) * delete steer rate limit when vel = 0 Signed-off-by: Zhe Shen * delete unnecessary variable Signed-off-by: Zhe Shen * pre-commit Signed-off-by: Zhe Shen --------- Signed-off-by: Zhe Shen --- .../autoware/mpc_lateral_controller/mpc.hpp | 5 ++--- .../src/mpc.cpp | 20 +++++-------------- 2 files changed, 7 insertions(+), 18 deletions(-) diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 058eb45bfaaff..902790f260e5e 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -271,7 +271,7 @@ class MPC */ std::pair 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 diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index c8e2da6daf7f4..ea97e9e6d5f39 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/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 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 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 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) {