feat(simple_planning_simulator): add VGR sim model (#8415)

* feat(simple_planning_simulator): add VGR sim model

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Update simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp

* move to interface

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* add const

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

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) | [-]  |
 
 <!-- deadzone_delta_steer | double | dead zone for the steering dynamics                  | x      | x      | x        | o      | o      | 0.0      | [rad]         |         | -->
 

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 <lanelet2_core/geometry/Lanelet.h>
 #include <tf2_ros/buffer.h>
@@ -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<HazardLightsReport>::SharedPtr pub_hazard_lights_report_;
   rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
   rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pub_current_pose_;
+  rclcpp::Publisher<ActuationStatusStamped>::SharedPtr pub_actuation_status_;
 
   rclcpp::Subscription<GearCommand>::SharedPtr sub_gear_cmd_;
   rclcpp::Subscription<GearCommand>::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

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp

@@ -23,13 +23,22 @@
 
 #include <deque>
 #include <iostream>
+#include <optional>
 #include <queue>
 #include <string>
 #include <vector>
 
 /**
  * @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,21 +52,91 @@ 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<double> state_index_;  // e.g. velocity, steering
   std::vector<double> actuation_index_;
   std::vector<std::vector<double>> actuation_map_;
 };
 
+/**
+ * @class AccelMap
+ * @brief class to get throttle from acceleration
+ *
+ *      ------- vel ------>
+ *     |
+ *     |
+ *  throttle    acc
+ *     |
+ *     |
+ *     V
+ */
+class AccelMap : public ActuationMap
+{
+public:
+  std::optional<double> 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
  */
 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<ActuationStatusStamped> 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_

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp

@@ -18,6 +18,9 @@
 #include <Eigen/Core>
 
 #include "autoware_vehicle_msgs/msg/gear_command.hpp"
+#include "tier4_vehicle_msgs/msg/actuation_status_stamped.hpp"
+
+#include <optional>
 
 /**
  * @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<ActuationStatusStamped> getActuationStatus() const { return std::nullopt; }
+
   /**
    * @brief calculate derivative of states with vehicle model
    * @param [in] state current model state

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]