Skip to content

Commit

Permalink
refactor(simple_planning_simulator): make function for duplicated code (
Browse files Browse the repository at this point in the history 
autowarefoundation#2502)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
  • Loading branch information
@TakaHoribe
TakaHoribe committed Dec 14, 2022
1 parent ed992b1 commit 9183c4f
Show file tree
Hide file tree
Showing 2 changed files with 24 additions and 40 deletions.
32 changes: 12 additions & 20 deletions ...imulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,6 +109,14 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel(
void SimModelDelaySteerAccGeared::updateStateWithGear(
Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt)
{
const auto setStopState = [&]() {
state(IDX::VX) = 0.0;
state(IDX::X) = prev_state(IDX::X);
state(IDX::Y) = prev_state(IDX::Y);
state(IDX::YAW) = prev_state(IDX::YAW);
state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5);
};

using autoware_auto_vehicle_msgs::msg::GearCommand;
if (
gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 ||
Expand All @@ -120,31 +128,15 @@ void SimModelDelaySteerAccGeared::updateStateWithGear(
gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 ||
gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) {
if (state(IDX::VX) < 0.0) {
state(IDX::VX) = 0.0;
state(IDX::X) = prev_state(IDX::X);
state(IDX::Y) = prev_state(IDX::Y);
state(IDX::YAW) = prev_state(IDX::YAW);
state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5);
setStopState();
}
} else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) {
if (state(IDX::VX) > 0.0) {
state(IDX::VX) = 0.0;
state(IDX::X) = prev_state(IDX::X);
state(IDX::Y) = prev_state(IDX::Y);
state(IDX::YAW) = prev_state(IDX::YAW);
state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5);
setStopState();
}
} else if (gear == GearCommand::PARK) {
state(IDX::VX) = 0.0;
state(IDX::X) = prev_state(IDX::X);
state(IDX::Y) = prev_state(IDX::Y);
state(IDX::YAW) = prev_state(IDX::YAW);
state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5);
setStopState();
} else {
state(IDX::VX) = 0.0;
state(IDX::X) = prev_state(IDX::X);
state(IDX::Y) = prev_state(IDX::Y);
state(IDX::YAW) = prev_state(IDX::YAW);
state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5);
setStopState();
}
}
32 changes: 12 additions & 20 deletions ...imulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,14 @@ Eigen::VectorXd SimModelIdealSteerAccGeared::calcModel(
void SimModelIdealSteerAccGeared::updateStateWithGear(
Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt)
{
const auto setStopState = [&]() {
state(IDX::VX) = 0.0;
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;
if (
gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 ||
Expand All @@ -75,31 +83,15 @@ void SimModelIdealSteerAccGeared::updateStateWithGear(
gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 ||
gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) {
if (state(IDX::VX) < 0.0) {
state(IDX::VX) = 0.0;
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);
setStopState();
}
} else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) {
if (state(IDX::VX) > 0.0) {
state(IDX::VX) = 0.0;
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);
setStopState();
}
} else if (gear == GearCommand::PARK) {
state(IDX::VX) = 0.0;
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);
setStopState();
} else {
state(IDX::VX) = 0.0;
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);
setStopState();
}
}

0 comments on commit 9183c4f

Please sign in to comment.