fix(simple_planning_simulator): fix ideal steer acc calc (autowarefou…

…ndation#2595) Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp> Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>
sujithvemi · Dec 28, 2022 · 8704ce9 · 8704ce9
1 parent 3cd453b
commit 8704ce9
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/...imulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/...imulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp
@@ -69,7 +69,6 @@ void SimModelIdealSteerAccGeared::updateStateWithGear(
     state(IDX::X) = prev_state(IDX::X);
     state(IDX::Y) = prev_state(IDX::Y);
     state(IDX::YAW) = prev_state(IDX::YAW);
-    current_acc_ = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5);
   };
 
   using autoware_auto_vehicle_msgs::msg::GearCommand;
@@ -94,4 +93,6 @@ void SimModelIdealSteerAccGeared::updateStateWithGear(
   } else {
     setStopState();
   }
+  // calculate acc from velocity diff
+  current_acc_ = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5);
 }