From 6988233252205bc06d70494e3e3498f4da38dbde Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 22 Aug 2024 14:00:09 +0900 Subject: [PATCH] feat(simple_planning_simulator): add VGR sim model (#8415) * feat(simple_planning_simulator): add VGR sim model Signed-off-by: kosuke55 * Update simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp * move to interface Signed-off-by: kosuke55 * add const Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- simulator/simple_planning_simulator/README.md | 31 +- .../simple_planning_simulator_core.hpp | 9 + .../vehicle_model/sim_model_actuation_cmd.hpp | 137 +++- .../vehicle_model/sim_model_interface.hpp | 15 + .../simple_planning_simulator.launch.py | 3 +- .../media/vgr_sim.drawio.svg | 691 ++++++++++++++++++ .../simple_planning_simulator_core.cpp | 60 +- .../vehicle_model/sim_model_actuation_cmd.cpp | 167 ++++- .../test/test_simple_planning_simulator.cpp | 53 +- 9 files changed, 1127 insertions(+), 39 deletions(-) create mode 100644 simulator/simple_planning_simulator/media/vgr_sim.drawio.svg diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index d44264035ecc4..1d91d5e6149b4 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -127,17 +127,30 @@ default, 0.00, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 1 The simple_planning_simulator usually operates by receiving Control commands, but when the `ACTUATION_CMD` model is selected, it receives Actuation commands instead of Control commands. This model can simulate the motion using the vehicle command that is actually sent to the real vehicle. Therefore, when this model is selected, the `raw_vehicle_cmd_converter` is also launched. +`convert_steer_cmd_method` has two options: "vgr" and "steer_map". If you choose "vgr" (variable gear ratio), it is assumed that the steering wheel angle is sent as the actuation command, and the value is converted to the steering tire angle to move the model. If you choose "steer_map", it is assumed that an arbitrary value is sent as the actuation command, and the value is converted to the steering tire rate to move the model. An arbitrary value is like EPS (Electric Power Steering) Voltage . `enable_pub_steer` determines whether to publish the steering tire angle. If false, it is expected to be converted and published from actuation_status in other nodes (e.g. raw_vehicle_cmd_converter). + +![vgr_sim](./media/vgr_sim.drawio.svg) + +```yaml + +``` + The parameters used in the ACTUATION_CMD are as follows. -| Name | Type | Description | unit | -| :------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--- | -| accel_time_delay | double | dead time for the acceleration input | [s] | -| accel_time_constant | double | time constant of the 1st-order acceleration dynamics | [s] | -| brake_time_delay | double | dead time for the brake input | [s] | -| brake_time_constant | double | time constant of the 1st-order brake dynamics | [s] | -| convert_accel_cmd | bool | If true, it is assumed that the command is received converted to an accel actuation value, and it is converted back to acceleration value inside the simulator. | [-] | -| convert_brake_cmd | bool | If true, it is assumed that the command is received converted to a brake actuation value, and it is converted back to acceleration value inside the simulator. | [-] | -| convert_steer_cmd | bool | If true, it is assumed that the command is received converted to a steer actuation value, and it is converted back to steer rate value inside the simulator. | [-] | +| Name | Type | Description | unit | +| :----------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--- | +| accel_time_delay | double | dead time for the acceleration input | [s] | +| accel_time_constant | double | time constant of the 1st-order acceleration dynamics | [s] | +| brake_time_delay | double | dead time for the brake input | [s] | +| brake_time_constant | double | time constant of the 1st-order brake dynamics | [s] | +| convert_accel_cmd | bool | If true, it is assumed that the command is received converted to an accel actuation value, and it is converted back to acceleration value inside the simulator. | [-] | +| convert_brake_cmd | bool | If true, it is assumed that the command is received converted to a brake actuation value, and it is converted back to acceleration value inside the simulator. | [-] | +| convert_steer_cmd | bool | If true, it is assumed that the command is received converted to a steer actuation value, and it is converted back to steer rate value inside the simulator. | [-] | +| convert_steer_cmd_method | bool | method to convert steer command. You can choose from "vgr" and "steer_map". | [-] | +| vgr_coef_a | double | the value of the coefficient a of the variable gear ratio | [-] | +| vgr_coef_b | double | the value of the coefficient b of the variable gear ratio | [-] | +| vgr_coef_c | double | the value of the coefficient c of the variable gear ratio | [-] | +| enable_pub_steer | bool | whether to publish the steering tire angle. if false, it is expected to be converted and published from actuation_status in other nodes (e.g. raw_vehicle_cmd_converter) | [-] | diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 547689dc847a0..76fdf5bdda6c9 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -45,6 +45,7 @@ #include "sensor_msgs/msg/imu.hpp" #include "tier4_external_api_msgs/srv/initialize_pose.hpp" #include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp" +#include "tier4_vehicle_msgs/msg/actuation_status_stamped.hpp" #include #include @@ -86,6 +87,7 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using tier4_external_api_msgs::srv::InitializePose; using tier4_vehicle_msgs::msg::ActuationCommandStamped; +using tier4_vehicle_msgs::msg::ActuationStatusStamped; class DeltaTime { @@ -138,6 +140,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_hazard_lights_report_; rclcpp::Publisher::SharedPtr pub_tf_; rclcpp::Publisher::SharedPtr pub_current_pose_; + rclcpp::Publisher::SharedPtr pub_actuation_status_; rclcpp::Subscription::SharedPtr sub_gear_cmd_; rclcpp::Subscription::SharedPtr sub_manual_gear_cmd_; @@ -189,6 +192,10 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node ControlModeReport current_control_mode_{}; bool enable_road_slope_simulation_ = true; + // if false, it is expected to be converted and published from actuation_status in other nodes + // (e.g. raw_vehicle_cmd_converter) + bool enable_pub_steer_ = true; //!< @brief flag to publish steering report. + /* frame_id */ std::string simulated_frame_id_ = ""; //!< @brief simulated vehicle frame id std::string origin_frame_id_ = ""; //!< @brief map frame_id @@ -388,6 +395,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ void publish_hazard_lights_report(); + void publish_actuation_status(); + /** * @brief publish tf * @param [in] state The kinematic state to publish as a TF diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp index 1385a509b8dca..5615f5c118165 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp @@ -23,13 +23,22 @@ #include #include +#include #include #include #include /** * @class ActuationMap - * @brief class to convert actuation command + * @brief class to convert from actuation command to control command + * + * ------- state -------> + * | + * | + * actuation control + * | + * | + * V */ class ActuationMap { @@ -43,12 +52,48 @@ class ActuationMap bool readActuationMapFromCSV(const std::string & csv_path, const bool validation = false); double getControlCommand(const double actuation, const double state) const; -private: +protected: std::vector state_index_; // e.g. velocity, steering std::vector actuation_index_; std::vector> actuation_map_; }; +/** + * @class AccelMap + * @brief class to get throttle from acceleration + * + * ------- vel ------> + * | + * | + * throttle acc + * | + * | + * V + */ +class AccelMap : public ActuationMap +{ +public: + std::optional getThrottle(const double acc, const double vel) const; +}; + +/** + * @class BrakeMap + * @brief class to get brake from acceleration + * + * ------- vel ------> + * | + * | + * brake acc + * | + * | + * V + */ +class BrakeMap : public ActuationMap +{ +public: + double getBrake(const double acc, const double vel) const; +}; + /** * @class SimModelActuationCmd * @brief class to handle vehicle model with actuation command @@ -56,8 +101,42 @@ class ActuationMap class SimModelActuationCmd : public SimModelInterface { public: + enum class ActuationSimType { VGR, STEER_MAP }; + /** - * @brief constructor + * @brief constructor (adaptive gear ratio conversion model) + * @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] accel_delay time delay for accel command [s] + * @param [in] acc_time_constant time constant for 1D model of accel dynamics + * @param [in] brake_delay time delay for brake command [s] + * @param [in] brake_time_constant time constant for 1D model of brake 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_bias steering bias [rad] + * @param [in] convert_accel_cmd flag to convert accel command + * @param [in] convert_brake_cmd flag to convert brake command + * @param [in] convert_steer_cmd flag to convert steer command + * @param [in] accel_map_path path to csv file for accel conversion map + * @param [in] brake_map_path path to csv file for brake conversion map + * @param [in] vgr_coef_a coefficient for variable gear ratio + * @param [in] vgr_coef_b coefficient for variable gear ratio + * @param [in] vgr_coef_c coefficient for variable gear ratio + */ + SimModelActuationCmd( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double accel_delay, double accel_time_constant, double brake_delay, + double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, + bool convert_accel_cmd, bool convert_brake_cmd, bool convert_steer_cmd, + std::string accel_map_path, std::string brake_map_path, double vgr_coef_a, double vgr_coef_b, + double vgr_coef_c); + + /** + * @brief constructor (steer map conversion model) * @param [in] vx_lim velocity limit [m/s] * @param [in] steer_lim steering limit [rad] * @param [in] vx_rate_lim acceleration limit [m/ss] @@ -90,13 +169,15 @@ class SimModelActuationCmd : public SimModelInterface */ ~SimModelActuationCmd() = default; - ActuationMap accel_map_; - ActuationMap brake_map_; - ActuationMap steer_map_; + /* + * @brief get actuation status + */ + std::optional getActuationStatus() const override; - bool convert_accel_cmd_; - bool convert_brake_cmd_; - bool convert_steer_cmd_; + /** + * @brief is publish actuation status enabled + */ + bool shouldPublishActuationStatus() const override { return true; } private: const double MIN_TIME_CONSTANT; //!< @brief minimum time constant @@ -133,7 +214,23 @@ class SimModelActuationCmd : public SimModelInterface 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_bias_; //!< @brief steering angle bias [rad] - const std::string path_; //!< @brief conversion map path + + bool convert_accel_cmd_; + bool convert_brake_cmd_; + bool convert_steer_cmd_; + + AccelMap accel_map_; + BrakeMap brake_map_; + ActuationMap steer_map_; + + // steer map conversion model + + // adaptive gear ratio conversion model + double vgr_coef_a_; + double vgr_coef_b_; + double vgr_coef_c_; + + ActuationSimType actuation_sim_type_{ActuationSimType::VGR}; /** * @brief set queue buffer for input command @@ -204,6 +301,26 @@ class SimModelActuationCmd : public SimModelInterface void updateStateWithGear( Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt); + + /** + * @brief calculate steering tire command + * @param [in] vel current velocity [m/s] + * @param [in] steer current steering angle [rad] + * @param [in] steer_wheel_des desired steering wheel angle [rad] + * @return steering tire command + */ + double calculateSteeringTireCommand( + const double vel, const double steer, const double steer_wheel_des) const; + + double calculateSteeringWheelState(const double target_tire_angle, const double vel) const; + + /** + * @brief calculate variable gear ratio + * @param [in] vel current velocity [m/s] + * @param [in] steer_wheel current steering wheel angle [rad] + * @return variable gear ratio + */ + double calculateVariableGearRatio(const double vel, const double steer_wheel) const; }; #endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index 1274a1ec28a07..f8d682a6e851c 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -18,6 +18,9 @@ #include #include "autoware_vehicle_msgs/msg/gear_command.hpp" +#include "tier4_vehicle_msgs/msg/actuation_status_stamped.hpp" + +#include /** * @class SimModelInterface @@ -26,6 +29,8 @@ class SimModelInterface { protected: + using ActuationStatusStamped = tier4_vehicle_msgs::msg::ActuationStatusStamped; + const int dim_x_; //!< @brief dimension of state x const int dim_u_; //!< @brief dimension of input u Eigen::VectorXd state_; //!< @brief vehicle state vector @@ -152,6 +157,16 @@ class SimModelInterface */ inline int getDimU() { return dim_u_; } + /** + * @brief is publish actuation status enabled + */ + virtual bool shouldPublishActuationStatus() const { return false; } + + /* + * @brief get actuation status + */ + virtual std::optional getActuationStatus() const { return std::nullopt; } + /** * @brief calculate derivative of states with vehicle model * @param [in] state current model state diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index e6a35bd420bf3..9b31926a53d57 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -62,6 +62,7 @@ def launch_setup(context, *args, **kwargs): ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"), ("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"), ("output/control_mode_report", "/vehicle/status/control_mode"), + ("output/actuation_status", "/vehicle/status/actuation_status"), ] # Additional remappings @@ -110,8 +111,6 @@ def launch_setup(context, *args, **kwargs): == "ACTUATION_CMD" ) - # launch_vehicle_cmd_converter = False # tmp - # 1) Launch only simple_planning_simulator_node if not launch_vehicle_cmd_converter: return [simple_planning_simulator_node] diff --git a/simulator/simple_planning_simulator/media/vgr_sim.drawio.svg b/simulator/simple_planning_simulator/media/vgr_sim.drawio.svg new file mode 100644 index 0000000000000..dea871dd73797 --- /dev/null +++ b/simulator/simple_planning_simulator/media/vgr_sim.drawio.svg @@ -0,0 +1,691 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ sim +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ actuation cmd +
+
    +
  • + accel_des +
    • +
  • brake_des
    • +
  • steer_wheel_des
    • +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ MAP/VGR +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ sim model +
+
+
+ + + + + + + + + + + + + + +
+
+
+
    +
  • acc_des
    • +
  • steer_des
    • +
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ MAP/VGR +
+
+
+ + + + + + + + + + + + + + +
+
+
+
+
    +
  • acc_stater
    • +
  • steer_state
    • +
+
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ actuation status +
+
    +
  • accel_status
    • +
  • brake_status
    • +
  • steer_wheel_status
    • +
+
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ controller +
+
+
+ + + + + + + + + + + + + + +
+
+
+ MAP/VGR +
+
+
+ + + + + + + + + + + + + + +
+
+
+ raw_vehicle_cmd_converter +
+
+
+ + + + + + + + + + + + + + +
+
+
+ (MAP)/VGR +
+
+
+ + + + + + + + + + + + + + +
+
+
+ control cmd +
+
+
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ + steering_state + +
+
+
+ + + + + + + + + + + + + + + +
+
+
+ + steering_state + +
+
+
+ + + + + + + + + + + + + + +
+
+
+ ( + convert_actuation_to_steering_status + =true) +
+
+
+ + + + + + + + + + diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 4252ace6c1920..96cfa1587e646 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -103,6 +103,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt add_measurement_noise_ = declare_parameter("add_measurement_noise", false); simulate_motion_ = declare_parameter("initial_engage_state"); enable_road_slope_simulation_ = declare_parameter("enable_road_slope_simulation", false); + enable_pub_steer_ = declare_parameter("enable_pub_steer", true); using rclcpp::QoS; using std::placeholders::_1; @@ -168,10 +169,14 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt pub_current_pose_ = create_publisher("output/pose", QoS{1}); pub_velocity_ = create_publisher("output/twist", QoS{1}); pub_odom_ = create_publisher("output/odometry", QoS{1}); - pub_steer_ = create_publisher("output/steering", QoS{1}); pub_acc_ = create_publisher("output/acceleration", QoS{1}); pub_imu_ = create_publisher("output/imu", QoS{1}); pub_tf_ = create_publisher("/tf", QoS{1}); + pub_actuation_status_ = + create_publisher("output/actuation_status", QoS{1}); + if (enable_pub_steer_) { + pub_steer_ = create_publisher("output/steering", QoS{1}); + } /* set param callback */ set_param_res_ = this->add_on_set_parameters_callback( @@ -323,13 +328,33 @@ void SimplePlanningSimulator::initialize_vehicle_model(const std::string & vehic // actuation conversion map const std::string accel_map_path = declare_parameter("accel_map_path"); const std::string brake_map_path = declare_parameter("brake_map_path"); - const std::string steer_map_path = declare_parameter("steer_map_path"); - vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - accel_time_delay, accel_time_constant, brake_time_delay, brake_time_constant, - steer_time_delay, steer_time_constant, steer_bias, convert_accel_cmd, convert_brake_cmd, - convert_steer_cmd, accel_map_path, brake_map_path, steer_map_path); + // init vehicle model depending on convert_steer_cmd_method + if (convert_steer_cmd) { + const std::string convert_steer_cmd_method = + declare_parameter("convert_steer_cmd_method"); + if (convert_steer_cmd_method == "vgr") { + const double vgr_coef_a = declare_parameter("vgr_coef_a"); + const double vgr_coef_b = declare_parameter("vgr_coef_b"); + const double vgr_coef_c = declare_parameter("vgr_coef_c"); + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, + timer_sampling_time_ms_ / 1000.0, accel_time_delay, accel_time_constant, brake_time_delay, + brake_time_constant, steer_time_delay, steer_time_constant, steer_bias, convert_accel_cmd, + convert_brake_cmd, convert_steer_cmd, accel_map_path, brake_map_path, vgr_coef_a, + vgr_coef_b, vgr_coef_c); + } else if (convert_steer_cmd_method == "steer_map") { + const std::string steer_map_path = declare_parameter("steer_map_path"); + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, + timer_sampling_time_ms_ / 1000.0, accel_time_delay, accel_time_constant, brake_time_delay, + brake_time_constant, steer_time_delay, steer_time_constant, steer_bias, convert_accel_cmd, + convert_brake_cmd, convert_steer_cmd, accel_map_path, brake_map_path, steer_map_path); + } else { + throw std::invalid_argument( + "Invalid convert_steer_cmd_method: " + convert_steer_cmd_method); + } + } } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } @@ -446,7 +471,6 @@ void SimplePlanningSimulator::on_timer() publish_odometry(current_odometry_); publish_pose(current_odometry_); publish_velocity(current_velocity_); - publish_steering(current_steer_); publish_acceleration(); publish_imu(); @@ -455,6 +479,14 @@ void SimplePlanningSimulator::on_timer() publish_turn_indicators_report(); publish_hazard_lights_report(); publish_tf(current_odometry_); + + if (enable_pub_steer_) { + publish_steering(current_steer_); + } + + if (vehicle_model_ptr_->shouldPublishActuationStatus()) { + publish_actuation_status(); + } } void SimplePlanningSimulator::on_map(const LaneletMapBin::ConstSharedPtr msg) @@ -871,6 +903,18 @@ void SimplePlanningSimulator::publish_tf(const Odometry & odometry) tf_msg.transforms.emplace_back(std::move(tf)); pub_tf_->publish(tf_msg); } + +void SimplePlanningSimulator::publish_actuation_status() +{ + auto actuation_status = vehicle_model_ptr_->getActuationStatus(); + if (!actuation_status.has_value()) { + return; + } + + actuation_status.value().header.stamp = get_clock()->now(); + actuation_status.value().header.frame_id = simulated_frame_id_; + pub_actuation_status_->publish(actuation_status.value()); +} } // namespace simple_planning_simulator } // namespace simulation 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 d550e335e2076..19794c04750fd 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 @@ -50,6 +50,60 @@ double ActuationMap::getControlCommand(const double actuation, const double stat return interpolation::lerp(actuation_index_, interpolated_control_vec, clamped_actuation); } +std::optional AccelMap::getThrottle(const double acc, double vel) const +{ + const std::vector & vel_indices = state_index_; + const std::vector & throttle_indices = actuation_index_; + const std::vector> & accel_map = actuation_map_; + + std::vector interpolated_acc_vec; + const double clamped_vel = CSVLoader::clampValue(vel, vel_indices); + + // (throttle, vel, acc) map => (throttle, acc) map by fixing vel + for (std::vector accelerations : accel_map) { + interpolated_acc_vec.push_back(interpolation::lerp(vel_indices, accelerations, clamped_vel)); + } + + // calculate throttle + // When the desired acceleration is smaller than the throttle area, return null => brake sequence + // When the desired acceleration is greater than the throttle area, return max throttle + if (acc < interpolated_acc_vec.front()) { + return std::nullopt; + } else if (interpolated_acc_vec.back() < acc) { + return throttle_indices.back(); + } + + return interpolation::lerp(interpolated_acc_vec, throttle_indices, acc); +} + +double BrakeMap::getBrake(const double acc, double vel) const +{ + const std::vector & vel_indices = state_index_; + const std::vector & brake_indices = actuation_index_; + const std::vector> & brake_map = actuation_map_; + + std::vector interpolated_acc_vec; + const double clamped_vel = CSVLoader::clampValue(vel, vel_indices); + + // (brake, vel, acc) map => (brake, acc) map by fixing vel + for (std::vector accelerations : brake_map) { + interpolated_acc_vec.push_back(interpolation::lerp(vel_indices, accelerations, clamped_vel)); + } + + // calculate brake + // When the desired acceleration is smaller than the brake area, return max brake on the map + // When the desired acceleration is greater than the brake area, return min brake on the map + if (acc < interpolated_acc_vec.back()) { + return brake_indices.back(); + } else if (interpolated_acc_vec.front() < acc) { + return brake_indices.front(); + } + + std::reverse(std::begin(interpolated_acc_vec), std::end(interpolated_acc_vec)); + return interpolation::lerp(interpolated_acc_vec, brake_indices, acc); +} + +// steer map sim model SimModelActuationCmd::SimModelActuationCmd( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double accel_delay, double accel_time_constant, double brake_delay, @@ -75,6 +129,40 @@ SimModelActuationCmd::SimModelActuationCmd( convert_accel_cmd_ = convert_accel_cmd && accel_map_.readActuationMapFromCSV(accel_map_path); convert_brake_cmd_ = convert_brake_cmd && brake_map_.readActuationMapFromCSV(brake_map_path); convert_steer_cmd_ = convert_steer_cmd && steer_map_.readActuationMapFromCSV(steer_map_path); + actuation_sim_type_ = ActuationSimType::STEER_MAP; +} + +// VGR sim model +SimModelActuationCmd::SimModelActuationCmd( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double accel_delay, double accel_time_constant, double brake_delay, + double brake_time_constant, double steer_delay, double steer_time_constant, double steer_bias, + bool convert_accel_cmd, bool convert_brake_cmd, bool convert_steer_cmd, + std::string accel_map_path, std::string brake_map_path, double vgr_coef_a, double vgr_coef_b, + double vgr_coef_c) +: SimModelInterface(6 /* dim x */, 5 /* dim u */), + MIN_TIME_CONSTANT(0.03), + vx_lim_(vx_lim), + vx_rate_lim_(vx_rate_lim), + steer_lim_(steer_lim), + steer_rate_lim_(steer_rate_lim), + wheelbase_(wheelbase), + accel_delay_(accel_delay), + accel_time_constant_(std::max(accel_time_constant, MIN_TIME_CONSTANT)), + brake_delay_(brake_delay), + brake_time_constant_(std::max(brake_time_constant, MIN_TIME_CONSTANT)), + steer_delay_(steer_delay), + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + steer_bias_(steer_bias), + convert_steer_cmd_(convert_steer_cmd), + vgr_coef_a_(vgr_coef_a), + vgr_coef_b_(vgr_coef_b), + vgr_coef_c_(vgr_coef_c) +{ + initializeInputQueue(dt); + convert_accel_cmd_ = convert_accel_cmd && accel_map_.readActuationMapFromCSV(accel_map_path); + convert_brake_cmd_ = convert_brake_cmd && brake_map_.readActuationMapFromCSV(brake_map_path); + actuation_sim_type_ = ActuationSimType::VGR; } double SimModelActuationCmd::getX() @@ -131,8 +219,9 @@ void SimModelActuationCmd::update(const double & dt) const auto prev_state = state_; updateRungeKutta(dt, delayed_input); - // take velocity limit explicitly - state_(IDX::VX) = std::max(-vx_lim_, std::min(state_(IDX::VX), vx_lim_)); + // take velocity/steer limit explicitly + state_(IDX::VX) = std::clamp(state_(IDX::VX), -vx_lim_, vx_lim_); + state_(IDX::STEER) = std::clamp(state_(IDX::STEER), -steer_lim_, steer_lim_); // consider gear // update position and velocity first, and then acceleration is calculated naturally @@ -201,8 +290,15 @@ Eigen::VectorXd SimModelActuationCmd::calcModel( std::invoke([&]() -> double { // if conversion is enabled, calculate steering rate from steer command if (convert_steer_cmd_) { - // convert steer command to steer rate - return steer_map_.getControlCommand(input(IDX_U::STEER_DES), steer) / steer_time_constant_; + if (actuation_sim_type_ == ActuationSimType::VGR) { + // convert steer wheel command to steer rate + const double steer_des = + calculateSteeringTireCommand(vel, steer, input(IDX_U::STEER_DES)); + return -(getSteer() - steer_des) / steer_time_constant_; + } else if (actuation_sim_type_ == ActuationSimType::STEER_MAP) { + // convert steer command to steer rate + return steer_map_.getControlCommand(input(IDX_U::STEER_DES), vel) / steer_time_constant_; + } } // otherwise, steer command is desired steering angle, so calculate steering rate from the // difference between the desired steering angle and the current steering angle. @@ -258,3 +354,66 @@ void SimModelActuationCmd::updateStateWithGear( state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); } } + +std::optional +SimModelActuationCmd::getActuationStatus() const +{ + if (!convert_accel_cmd_ && !convert_brake_cmd_ && !convert_steer_cmd_) { + return std::nullopt; + } + + tier4_vehicle_msgs::msg::ActuationStatusStamped actuation_status; + + const double acc_state = std::clamp(state_(IDX::ACCX), -vx_rate_lim_, vx_rate_lim_); + const double vel_state = std::clamp(state_(IDX::VX), -vx_lim_, vx_lim_); + + if (convert_accel_cmd_) { + const auto throttle = accel_map_.getThrottle(acc_state, vel_state); + if (throttle.has_value()) { + actuation_status.status.accel_status = throttle.value(); + } + } + + if (convert_brake_cmd_) { + actuation_status.status.brake_status = brake_map_.getBrake(acc_state, vel_state); + } + + if (convert_steer_cmd_) { + if (actuation_sim_type_ == ActuationSimType::VGR) { + actuation_status.status.steer_status = + calculateSteeringWheelState(vel_state, state_(IDX::STEER)); + } + // NOTE: Conversion by steer map is not supported + // else if (actuation_sim_type_ == ActuationSimType::STEER_MAP) {} + } + + return actuation_status; +} + +/* ------ Functions for VGR sim model ----- */ +double SimModelActuationCmd::calculateSteeringTireCommand( + const double vel, const double steer, const double steer_wheel_des) const +{ + // steer_tire_state -> steer_wheel_state + const double steer_wheel = calculateSteeringWheelState(vel, steer); + + // steer_wheel_des -> steer_tire_des + const double adaptive_gear_ratio = calculateVariableGearRatio(vel, steer_wheel); + + return steer_wheel_des / adaptive_gear_ratio; +} + +double SimModelActuationCmd::calculateSteeringWheelState( + const double vel, const double steer_state) const +{ + return (vgr_coef_a_ + vgr_coef_b_ * vel * vel) * steer_state / + (1.0 + vgr_coef_c_ * std::abs(steer_state)); +} + +double SimModelActuationCmd::calculateVariableGearRatio( + const double vel, const double steer_wheel) const +{ + return std::max( + 1e-5, vgr_coef_a_ + vgr_coef_b_ * vel * vel - vgr_coef_c_ * std::fabs(steer_wheel)); +} +/* ---------------------------------------- */ diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index 73762150f8e1d..cb58628121de2 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -265,10 +265,31 @@ void declareVehicleInfoParams(rclcpp::NodeOptions & node_options) node_options.append_parameter_override("max_steer_angle", 0.7); } +using DefaultParamType = std::tuple; +using ActuationCmdParamType = std::tuple; +using ParamType = std::variant; +std::unordered_map vehicle_model_type_map = { + {"IDEAL_STEER_VEL", typeid(DefaultParamType)}, + {"IDEAL_STEER_ACC", typeid(DefaultParamType)}, + {"IDEAL_STEER_ACC_GEARED", typeid(DefaultParamType)}, + {"DELAY_STEER_VEL", typeid(DefaultParamType)}, + {"DELAY_STEER_ACC", typeid(DefaultParamType)}, + {"DELAY_STEER_ACC_GEARED", typeid(DefaultParamType)}, + {"DELAY_STEER_ACC_GEARED_WO_FALL_GUARD", typeid(DefaultParamType)}, + {"ACTUATION_CMD", typeid(ActuationCmdParamType)}}; + +std::pair get_common_params(const ParamType & params) +{ + return std::visit( + [](const auto & param) -> std::pair { + return std::make_pair(std::get<0>(param), std::get<1>(param)); + }, + params); +} + // Send a control command and run the simulation. // Then check if the vehicle is moving in the desired direction. -class TestSimplePlanningSimulator -: public ::testing::TestWithParam> +class TestSimplePlanningSimulator : public ::testing::TestWithParam { }; @@ -277,10 +298,23 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) rclcpp::init(0, nullptr); const auto params = GetParam(); - const auto command_type = std::get<0>(params); - const auto vehicle_model_type = std::get<1>(params); - + // common parameters + const auto common_params = get_common_params(params); + const auto command_type = common_params.first; + const auto vehicle_model_type = common_params.second; std::cout << "\n\n vehicle model = " << vehicle_model_type << std::endl << std::endl; + // optional parameters + std::optional conversion_type{}; // for ActuationCmdParamType + + // Determine the ParamType corresponding to vehicle_model_type and get the specific parameters. + const auto iter = vehicle_model_type_map.find(vehicle_model_type); + if (iter == vehicle_model_type_map.end()) { + throw std::invalid_argument("Unexpected vehicle_model_type."); + } + if (iter->second == typeid(ActuationCmdParamType)) { + conversion_type = std::get<2>(std::get(params)); + } + rclcpp::NodeOptions node_options; node_options.append_parameter_override("initialize_source", "INITIAL_POSE_TOPIC"); node_options.append_parameter_override("vehicle_model_type", vehicle_model_type); @@ -300,6 +334,12 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) node_options.append_parameter_override("accel_map_path", accel_map_path); node_options.append_parameter_override("brake_map_path", brake_map_path); node_options.append_parameter_override("steer_map_path", steer_map_path); + node_options.append_parameter_override("vgr_coef_a", 15.713); + node_options.append_parameter_override("vgr_coef_b", 0.053); + node_options.append_parameter_override("vgr_coef_c", 0.042); + if (conversion_type.has_value()) { + node_options.append_parameter_override("convert_steer_cmd_method", conversion_type.value()); + } declareVehicleInfoParams(node_options); const auto sim_node = std::make_shared(node_options); @@ -393,4 +433,5 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple(CommandType::Ackermann, "DELAY_STEER_ACC_GEARED"), std::make_tuple(CommandType::Ackermann, "DELAY_STEER_ACC_GEARED_WO_FALL_GUARD"), /* Actuation type */ - std::make_tuple(CommandType::Actuation, "ACTUATION_CMD"))); + std::make_tuple(CommandType::Actuation, "ACTUATION_CMD", "steer_map"), + std::make_tuple(CommandType::Actuation, "ACTUATION_CMD", "vgr")));