From b407468b1c18e7d52271ea29c50351d292965ef0 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 24 Jul 2024 17:44:13 +0900 Subject: [PATCH] fix(simple_planning_simulator): fix publised acc of actuation simulator (#8169) Signed-off-by: kosuke55 --- .../vehicle_model/sim_model_actuation_cmd.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp index 16eb6e86d778c..d550e335e2076 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp @@ -160,6 +160,7 @@ Eigen::VectorXd SimModelActuationCmd::calcModel( using autoware_vehicle_msgs::msg::GearCommand; const double vel = std::clamp(state(IDX::VX), -vx_lim_, vx_lim_); + const double acc = std::clamp(state(IDX::ACCX), -vx_rate_lim_, vx_rate_lim_); const double yaw = state(IDX::YAW); const double steer = state(IDX::STEER); @@ -168,7 +169,7 @@ Eigen::VectorXd SimModelActuationCmd::calcModel( const auto gear = input(IDX_U::GEAR); // 1) calculate acceleration by accel and brake command - const double acc_des = std::clamp( + const double acc_des_wo_slope = std::clamp( std::invoke([&]() -> double { // Select the non-zero value between accel and brake and calculate the acceleration if (convert_accel_cmd_ && accel > 0.0) { @@ -185,13 +186,13 @@ Eigen::VectorXd SimModelActuationCmd::calcModel( -vx_rate_lim_, vx_rate_lim_); // add slope acceleration considering the gear state const double acc_by_slope = input(IDX_U::SLOPE_ACCX); - const double acc = std::invoke([&]() -> double { + const double acc_des = std::invoke([&]() -> double { if (gear == GearCommand::NONE || gear == GearCommand::PARK) { return 0.0; } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { - return -acc_des + acc_by_slope; + return -acc_des_wo_slope + acc_by_slope; } - return acc_des + acc_by_slope; + return acc_des_wo_slope + acc_by_slope; }); const double acc_time_constant = accel > 0.0 ? accel_time_constant_ : brake_time_constant_;