Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(simple_planning_simulator): add new vehicle model with falling down #7651

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions simulator/simple_planning_simulator/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp
src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp
src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp
src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp
src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp
src/simple_planning_simulator/utils/csv_loader.cpp
)
Expand Down
41 changes: 21 additions & 20 deletions simulator/simple_planning_simulator/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -62,33 +62,34 @@ The purpose of this simulator is for the integration test of planning and contro
- `DELAY_STEER_VEL`
- `DELAY_STEER_ACC`
- `DELAY_STEER_ACC_GEARED`
- `DELAY_STEER_ACC_GEARED_WO_FALL_GUARD`
- `DELAY_STEER_MAP_ACC_GEARED`: applies 1D dynamics and time delay to the steering and acceleration commands. The simulated acceleration is determined by a value converted through the provided acceleration map. This model is valuable for an accurate simulation with acceleration deviations in a real vehicle.
- `LEARNED_STEER_VEL`: launches learned python models. More about this [here](../learning_based_vehicle_model).

The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves based on a 1st-order with time delay model. The `STEER` means the model receives the steer command. The `VEL` means the model receives the target velocity command, while the `ACC` model receives the target acceleration command. The `GEARED` suffix means that the motion will consider the gear command: the vehicle moves only one direction following the gear.

The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V).

| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | D_ST_M_ACC_G | L_S_V | Default value | unit |
| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :----------- | :---- | :------------ | :------ |
| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | o | x | 0.1 | [s] |
| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | o | x | 0.24 | [s] |
| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | x | x | 0.25 | [s] |
| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | o | x | 0.1 | [s] |
| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | o | x | 0.27 | [s] |
| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | x | x | 0.0 | [rad] |
| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | x | x | 0.5 | [s] |
| vel_lim | double | limit of velocity | x | x | x | o | o | o | o | x | 50.0 | [m/s] |
| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | x | 7.0 | [m/ss] |
| steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | x | 1.0 | [rad] |
| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | x | 5.0 | [rad/s] |
| steer_bias | double | bias for steering angle | x | x | x | o | o | o | o | x | 0.0 | [rad] |
| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | x | x | 1.0 | [-] |
| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | x | x | 1.0 | [-] |
| acceleration_map_path | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x | x | x | x | x | x | o | x | - | [-] |
| model_module_paths | string | path to a python module where the model is implemented | x | x | x | x | x | x | x | o | - | [-] |
| model_param_paths | string | path to the file where model parameters are stored (can be empty string if no parameter file is required) | x | x | x | x | x | x | x | o | - | [-] |
| model_class_names | string | name of the class that implements the model | x | x | x | x | x | x | x | o | - | [-] |
| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | D_ST_A_G_WO_FG | D_ST_M_ACC_G | L_S_V | Default value | unit |
| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------- | :----------- | :---- | :------------ | :------ |
| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | o | o | x | 0.1 | [s] |
| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | o | o | x | 0.24 | [s] |
| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | x | x | x | 0.25 | [s] |
| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | o | o | x | 0.1 | [s] |
| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | o | o | x | 0.27 | [s] |
| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | o | x | x | 0.0 | [rad] |
| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | x | x | x | 0.5 | [s] |
| vel_lim | double | limit of velocity | x | x | x | o | o | o | o | o | x | 50.0 | [m/s] |
| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | o | x | 7.0 | [m/ss] |
| steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | o | x | 1.0 | [rad] |
| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | o | x | 5.0 | [rad/s] |
| steer_bias | double | bias for steering angle | x | x | x | o | o | o | o | o | x | 0.0 | [rad] |
| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | o | x | x | 1.0 | [-] |
| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | o | x | x | 1.0 | [-] |
| acceleration_map_path | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x | x | x | x | x | x | x | o | x | - | [-] |
| model_module_paths | string | path to a python module where the model is implemented | x | x | x | x | x | x | x | x | o | - | [-] |
| model_param_paths | string | path to the file where model parameters are stored (can be empty string if no parameter file is required) | x | x | x | x | x | x | x | x | o | - | [-] |
| model_class_names | string | name of the class that implements the model | x | x | x | x | x | x | x | x | o | - | [-] |

_Note:_ Parameters `model_module_paths`, `model_param_paths`, and `model_class_names` need to have the same length.

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -205,7 +205,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
IDEAL_STEER_VEL = 4,
DELAY_STEER_VEL = 5,
DELAY_STEER_MAP_ACC_GEARED = 6,
LEARNED_STEER_VEL = 7
LEARNED_STEER_VEL = 7,
DELAY_STEER_ACC_GEARED_WO_FALL_GUARD = 8
} vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics
std::shared_ptr<SimModelInterface> vehicle_model_ptr_; //!< @brief vehicle model pointer

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp"
#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp"
#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp"
#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp"
#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp"
#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp"
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,149 @@
// Copyright 2024 The Autoware Foundation.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_ // NOLINT
#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_ // NOLINT

#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp"

#include <Eigen/Core>
#include <Eigen/LU>

#include <deque>
#include <iostream>
#include <queue>

class SimModelDelaySteerAccGearedWoFallGuard : public SimModelInterface
{
public:
/**
* @param [in] vx_lim velocity limit [m/s]
* @param [in] steer_lim steering limit [rad]
* @param [in] vx_rate_lim acceleration limit [m/ss]
* @param [in] steer_rate_lim steering angular velocity limit [rad/ss]
* @param [in] wheelbase vehicle wheelbase length [m]
* @param [in] dt delta time information to set input buffer for delay
* @param [in] acc_delay time delay for accel command [s]
* @param [in] acc_time_constant time constant for 1D model of accel dynamics
* @param [in] steer_delay time delay for steering command [s]
* @param [in] steer_time_constant time constant for 1D model of steering dynamics
* @param [in] steer_dead_band dead band for steering angle [rad]
* @param [in] steer_bias steering bias [rad]
* @param [in] debug_acc_scaling_factor scaling factor for accel command
* @param [in] debug_steer_scaling_factor scaling factor for steering command
*/
SimModelDelaySteerAccGearedWoFallGuard(
double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase,
double dt, double acc_delay, double acc_time_constant, double steer_delay,
double steer_time_constant, double steer_dead_band, double steer_bias,
double debug_acc_scaling_factor, double debug_steer_scaling_factor);

/**
* @brief default destructor
*/
~SimModelDelaySteerAccGearedWoFallGuard() = default;

private:
const double MIN_TIME_CONSTANT; //!< @brief minimum time constant

enum IDX {
X = 0,
Y,
YAW,
VX,
STEER,
PEDAL_ACCX,
};
enum IDX_U { PEDAL_ACCX_DES = 0, GEAR, SLOPE_ACCX, STEER_DES };

const double vx_lim_; //!< @brief velocity limit [m/s]
const double vx_rate_lim_; //!< @brief acceleration limit [m/ss]
const double steer_lim_; //!< @brief steering limit [rad]
const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s]
const double wheelbase_; //!< @brief vehicle wheelbase length [m]

std::deque<double> acc_input_queue_; //!< @brief buffer for accel command
std::deque<double> steer_input_queue_; //!< @brief buffer for steering command
const double acc_delay_; //!< @brief time delay for accel command [s]
const double acc_time_constant_; //!< @brief time constant for accel dynamics
const double steer_delay_; //!< @brief time delay for steering command [s]
const double steer_time_constant_; //!< @brief time constant for steering dynamics
const double steer_dead_band_; //!< @brief dead band for steering angle [rad]
const double steer_bias_; //!< @brief steering angle bias [rad]
const double debug_acc_scaling_factor_; //!< @brief scaling factor for accel command
const double debug_steer_scaling_factor_; //!< @brief scaling factor for steering command

/**
* @brief set queue buffer for input command
* @param [in] dt delta time
*/
void initializeInputQueue(const double & dt);

/**
* @brief get vehicle position x
*/
double getX() override;

/**
* @brief get vehicle position y
*/
double getY() override;

/**
* @brief get vehicle angle yaw
*/
double getYaw() override;

/**
* @brief get vehicle velocity vx
*/
double getVx() override;

/**
* @brief get vehicle lateral velocity
*/
double getVy() override;

/**
* @brief get vehicle longitudinal acceleration
*/
double getAx() override;

/**
* @brief get vehicle angular-velocity wz
*/
double getWz() override;

/**
* @brief get vehicle steering angle
*/
double getSteer() override;

/**
* @brief update vehicle states
* @param [in] dt delta time [s]
*/
void update(const double & dt) override;

/**
* @brief calculate derivative of states with time delay steering model
* @param [in] state current model state
* @param [in] input input vector to model
*/
Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override;
};

// clang-format off
#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_ // NOLINT
// clang-format on
Loading
Loading