diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp index 24d737a154e86..fa9d491cd9c5f 100644 --- a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp +++ b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp @@ -15,7 +15,6 @@ #ifndef OSQP_INTERFACE__OSQP_INTERFACE_HPP_ #define OSQP_INTERFACE__OSQP_INTERFACE_HPP_ -#include "common/types.hpp" #include "eigen3/Eigen/Core" #include "osqp/osqp.h" #include "osqp_interface/csc_matrix_conv.hpp" @@ -36,8 +35,6 @@ namespace common namespace osqp { constexpr c_float INF = 1e30; -using autoware::common::types::bool8_t; -using autoware::common::types::float64_t; /** * Implementation of a native C++ interface for the OSQP solver. @@ -54,19 +51,19 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface // Number of parameters to optimize int64_t m_param_n; // Flag to check if the current work exists - bool8_t m_work_initialized = false; + bool m_work_initialized = false; // Exitflag int64_t m_exitflag; // Runs the solver on the stored problem. - std::tuple, std::vector, int64_t, int64_t, int64_t> solve(); + std::tuple, std::vector, int64_t, int64_t, int64_t> solve(); static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; public: /// \brief Constructor without problem formulation explicit OSQPInterface( - const c_float eps_abs = std::numeric_limits::epsilon(), const bool8_t polish = true); + const c_float eps_abs = std::numeric_limits::epsilon(), const bool polish = true); /// \brief Constructor with problem setup /// \param P: (n,n) matrix defining relations between parameters. /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. @@ -75,11 +72,11 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface /// \param u: (m) vector defining the upper bound problem constraint. /// \param eps_abs: Absolute convergence tolerance. OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs); + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u, const c_float eps_abs); OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs); + const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, + const std::vector & l, const std::vector & u, const c_float eps_abs); ~OSQPInterface(); /**************** @@ -97,13 +94,13 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface /// \details 2. Initialize the interface and set up the problem. /// \details osqp_interface = OSQPInterface(P, A, q, l, u, 1e-6); /// \details 3. Call the optimization function. - /// \details std::tuple, std::vector> result; + /// \details std::tuple, std::vector> result; /// \details result = osqp_interface.optimize(); /// \details 4. Access the optimized parameters. /// \details std::vector param = std::get<0>(result); - /// \details float64_t x_0 = param[0]; - /// \details float64_t x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t, int64_t> optimize(); + /// \details double x_0 = param[0]; + /// \details double x_1 = param[1]; + std::tuple, std::vector, int64_t, int64_t, int64_t> optimize(); /// \brief Solves convex quadratic programs (QPs) using the OSQP solver. /// \return The function returns a tuple containing the solution as two float vectors. @@ -115,15 +112,15 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface /// \details 2. Initialize the interface. /// \details osqp_interface = OSQPInterface(1e-6); /// \details 3. Call the optimization function with the problem formulation. - /// \details std::tuple, std::vector> result; + /// \details std::tuple, std::vector> result; /// \details result = osqp_interface.optimize(P, A, q, l, u, 1e-6); /// \details 4. Access the optimized parameters. /// \details std::vector param = std::get<0>(result); - /// \details float64_t x_0 = param[0]; - /// \details float64_t x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t, int64_t> optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); + /// \details double x_0 = param[0]; + /// \details double x_1 = param[1]; + std::tuple, std::vector, int64_t, int64_t, int64_t> optimize( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u); /// \brief Converts the input data and sets up the workspace object. /// \param P (n,n) matrix defining relations between parameters. @@ -132,11 +129,11 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface /// \param l (m) vector defining the lower bound problem constraint. /// \param u (m) vector defining the upper bound problem constraint. int64_t initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u); int64_t initializeProblem( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); + CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, + const std::vector & u); // Setter functions for warm start bool setWarmStart( @@ -187,9 +184,9 @@ class OSQP_INTERFACE_PUBLIC OSQPInterface return static_cast(m_latest_work_info.status_polish); } /// \brief Get the runtime of the latest problem solved - inline float64_t getRunTime() const { return m_latest_work_info.run_time; } + inline double getRunTime() const { return m_latest_work_info.run_time; } /// \brief Get the objective value the latest problem solved - inline float64_t getObjVal() const { return m_latest_work_info.obj_val; } + inline double getObjVal() const { return m_latest_work_info.obj_val; } /// \brief Returns flag asserting interface condition (Healthy condition: 0). inline int64_t getExitFlag() const { return m_exitflag; } diff --git a/common/osqp_interface/package.xml b/common/osqp_interface/package.xml index 7c11ac6b31ae4..643611ca48448 100644 --- a/common/osqp_interface/package.xml +++ b/common/osqp_interface/package.xml @@ -14,7 +14,6 @@ autoware_cmake - autoware_auto_common eigen osqp_vendor rclcpp diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp index 085faf8f14bd6..972a586b164e9 100644 --- a/common/osqp_interface/src/osqp_interface.cpp +++ b/common/osqp_interface/src/osqp_interface.cpp @@ -31,7 +31,7 @@ namespace common { namespace osqp { -OSQPInterface::OSQPInterface(const c_float eps_abs, const bool8_t polish) +OSQPInterface::OSQPInterface(const c_float eps_abs, const bool polish) : m_work{nullptr, OSQPWorkspaceDeleter} { m_settings = std::make_unique(); @@ -53,16 +53,16 @@ OSQPInterface::OSQPInterface(const c_float eps_abs, const bool8_t polish) } OSQPInterface::OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs) + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u, const c_float eps_abs) : OSQPInterface(eps_abs) { initializeProblem(P, A, q, l, u); } OSQPInterface::OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs) + const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, + const std::vector & l, const std::vector & u, const c_float eps_abs) : OSQPInterface(eps_abs) { initializeProblem(P, A, q, l, u); @@ -281,8 +281,8 @@ bool OSQPInterface::setDualVariables(const std::vector & dual_variables) } int64_t OSQPInterface::initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) { // check if arguments are valid std::stringstream ss; @@ -318,16 +318,16 @@ int64_t OSQPInterface::initializeProblem( } int64_t OSQPInterface::initializeProblem( - CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, - const std::vector & l, const std::vector & u) + CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, + const std::vector & u) { // Dynamic float arrays - std::vector q_tmp(q.begin(), q.end()); - std::vector l_tmp(l.begin(), l.end()); - std::vector u_tmp(u.begin(), u.end()); - float64_t * q_dyn = q_tmp.data(); - float64_t * l_dyn = l_tmp.data(); - float64_t * u_dyn = u_tmp.data(); + std::vector q_tmp(q.begin(), q.end()); + std::vector l_tmp(l.begin(), l.end()); + std::vector u_tmp(u.begin(), u.end()); + double * q_dyn = q_tmp.data(); + double * l_dyn = l_tmp.data(); + double * u_dyn = u_tmp.data(); /********************** * OBJECTIVE FUNCTION @@ -360,7 +360,7 @@ int64_t OSQPInterface::initializeProblem( return m_exitflag; } -std::tuple, std::vector, int64_t, int64_t, int64_t> +std::tuple, std::vector, int64_t, int64_t, int64_t> OSQPInterface::solve() { // Solve Problem @@ -369,17 +369,17 @@ OSQPInterface::solve() /******************** * EXTRACT SOLUTION ********************/ - float64_t * sol_x = m_work->solution->x; - float64_t * sol_y = m_work->solution->y; - std::vector sol_primal(sol_x, sol_x + m_param_n); - std::vector sol_lagrange_multiplier(sol_y, sol_y + m_data->m); + double * sol_x = m_work->solution->x; + double * sol_y = m_work->solution->y; + std::vector sol_primal(sol_x, sol_x + m_param_n); + std::vector sol_lagrange_multiplier(sol_y, sol_y + m_data->m); int64_t status_polish = m_work->info->status_polish; int64_t status_solution = m_work->info->status_val; int64_t status_iteration = m_work->info->iter; // Result tuple - std::tuple, std::vector, int64_t, int64_t, int64_t> result = + std::tuple, std::vector, int64_t, int64_t, int64_t> result = std::make_tuple( sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration); @@ -388,26 +388,24 @@ OSQPInterface::solve() return result; } -std::tuple, std::vector, int64_t, int64_t, int64_t> +std::tuple, std::vector, int64_t, int64_t, int64_t> OSQPInterface::optimize() { // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t, int64_t> result = - solve(); + std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); return result; } -std::tuple, std::vector, int64_t, int64_t, int64_t> +std::tuple, std::vector, int64_t, int64_t, int64_t> OSQPInterface::optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) { // Allocate memory for problem initializeProblem(P, A, q, l, u); // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t, int64_t> result = - solve(); + std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); m_work.reset(); m_work_initialized = false; diff --git a/common/osqp_interface/test/test_osqp_interface.cpp b/common/osqp_interface/test/test_osqp_interface.cpp index 6ef6d131a6cbe..cf96f0164bafc 100644 --- a/common/osqp_interface/test/test_osqp_interface.cpp +++ b/common/osqp_interface/test/test_osqp_interface.cpp @@ -21,7 +21,6 @@ namespace { -using autoware::common::osqp::float64_t; // Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py // // min 1/2 * x' * P * x + q' * x @@ -45,7 +44,7 @@ TEST(TestOsqpInterface, BasicQp) using autoware::common::osqp::CSC_Matrix; auto check_result = - [](const std::tuple, std::vector, int, int, int> & result) { + [](const std::tuple, std::vector, int, int, int> & result) { EXPECT_EQ(std::get<2>(result), 1); // polish succeeded EXPECT_EQ(std::get<3>(result), 1); // solution succeeded @@ -66,14 +65,14 @@ TEST(TestOsqpInterface, BasicQp) const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); - const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -autoware::common::osqp::INF}; - const std::vector u = {1.0, 0.7, 0.7, autoware::common::osqp::INF}; + const std::vector q = {1.0, 1.0}; + const std::vector l = {1.0, 0.0, 0.0, -autoware::common::osqp::INF}; + const std::vector u = {1.0, 0.7, 0.7, autoware::common::osqp::INF}; { // Define problem during optimization autoware::common::osqp::OSQPInterface osqp; - std::tuple, std::vector, int, int, int> result = + std::tuple, std::vector, int, int, int> result = osqp.optimize(P, A, q, l, u); check_result(result); } @@ -81,19 +80,18 @@ TEST(TestOsqpInterface, BasicQp) { // Define problem during initialization autoware::common::osqp::OSQPInterface osqp(P, A, q, l, u, 1e-6); - std::tuple, std::vector, int, int, int> result = - osqp.optimize(); + std::tuple, std::vector, int, int, int> result = osqp.optimize(); check_result(result); } { - std::tuple, std::vector, int, int, int> result; + std::tuple, std::vector, int, int, int> result; // Dummy initial problem Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); autoware::common::osqp::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6); osqp.optimize(); @@ -108,19 +106,18 @@ TEST(TestOsqpInterface, BasicQp) CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); CSC_Matrix A_csc = calCSCMatrix(A); autoware::common::osqp::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6); - std::tuple, std::vector, int, int, int> result = - osqp.optimize(); + std::tuple, std::vector, int, int, int> result = osqp.optimize(); check_result(result); } { - std::tuple, std::vector, int, int, int> result; + std::tuple, std::vector, int, int, int> result; // Dummy initial problem with csc matrix CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); autoware::common::osqp::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); osqp.optimize(); @@ -134,13 +131,13 @@ TEST(TestOsqpInterface, BasicQp) // add warm startup { - std::tuple, std::vector, int, int, int> result; + std::tuple, std::vector, int, int, int> result; // Dummy initial problem with csc matrix CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); autoware::common::osqp::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); osqp.optimize(); 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 9bbaabf61d68f..b9ae9d9804b57 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 @@ -15,7 +15,6 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ #define SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ -#include "common/types.hpp" #include "rclcpp/rclcpp.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include "simple_planning_simulator/visibility_control.hpp" @@ -58,9 +57,6 @@ namespace simulation { namespace simple_planning_simulator { -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_geometry_msgs::msg::Complex32; @@ -90,13 +86,13 @@ class DeltaTime { public: DeltaTime() : prev_updated_time_ptr_(nullptr) {} - float64_t get_dt(const rclcpp::Time & now) + double get_dt(const rclcpp::Time & now) { if (prev_updated_time_ptr_ == nullptr) { prev_updated_time_ptr_ = std::make_shared(now); return 0.0; } - const float64_t dt = (now - *prev_updated_time_ptr_).seconds(); + const double dt = (now - *prev_updated_time_ptr_).seconds(); *prev_updated_time_ptr_ = now; return dt; } @@ -181,15 +177,15 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node std::string origin_frame_id_; //!< @brief map frame_id /* flags */ - bool8_t is_initialized_; //!< @brief flag to check the initial position is set - bool8_t add_measurement_noise_; //!< @brief flag to add measurement noise + bool is_initialized_; //!< @brief flag to check the initial position is set + bool add_measurement_noise_; //!< @brief flag to add measurement noise DeltaTime delta_time_; //!< @brief to calculate delta time MeasurementNoiseGenerator measurement_noise_; //!< @brief for measurement noise - float64_t x_stddev_; //!< @brief x standard deviation for dummy covariance in map coordinate - float64_t y_stddev_; //!< @brief y standard deviation for dummy covariance in map coordinate + double x_stddev_; //!< @brief x standard deviation for dummy covariance in map coordinate + double y_stddev_; //!< @brief y standard deviation for dummy covariance in map coordinate /* vehicle model */ enum class VehicleModelType { diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp index ee9f79a17e2f7..057b2418450e1 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp @@ -40,9 +40,9 @@ class SimModelDelaySteerAcc : public SimModelInterface * @param [in] steer_time_constant time constant for 1D model of steering dynamics */ SimModelDelaySteerAcc( - float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, - float64_t wheelbase, float64_t dt, float64_t acc_delay, float64_t acc_time_constant, - float64_t steer_delay, float64_t steer_time_constant); + 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); /** * @brief default destructor @@ -50,7 +50,7 @@ class SimModelDelaySteerAcc : public SimModelInterface ~SimModelDelaySteerAcc() = default; private: - const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant enum IDX { X = 0, @@ -66,70 +66,70 @@ class SimModelDelaySteerAcc : public SimModelInterface DRIVE_SHIFT, }; - const float64_t vx_lim_; //!< @brief velocity limit [m/s] - const float64_t vx_rate_lim_; //!< @brief acceleration limit [m/ss] - const float64_t steer_lim_; //!< @brief steering limit [rad] - const float64_t steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] - const float64_t wheelbase_; //!< @brief vehicle wheelbase length [m] + 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 acc_input_queue_; //!< @brief buffer for accel command - std::deque steer_input_queue_; //!< @brief buffer for steering command - const float64_t acc_delay_; //!< @brief time delay for accel command [s] - const float64_t acc_time_constant_; //!< @brief time constant for accel dynamics - const float64_t steer_delay_; //!< @brief time delay for steering command [s] - const float64_t steer_time_constant_; //!< @brief time constant for steering dynamics + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque 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 /** * @brief set queue buffer for input command * @param [in] dt delta time */ - void initializeInputQueue(const float64_t & dt); + void initializeInputQueue(const double & dt); /** * @brief get vehicle position x */ - float64_t getX() override; + double getX() override; /** * @brief get vehicle position y */ - float64_t getY() override; + double getY() override; /** * @brief get vehicle angle yaw */ - float64_t getYaw() override; + double getYaw() override; /** * @brief get vehicle velocity vx */ - float64_t getVx() override; + double getVx() override; /** * @brief get vehicle lateral velocity */ - float64_t getVy() override; + double getVy() override; /** * @brief get vehicle longitudinal acceleration */ - float64_t getAx() override; + double getAx() override; /** * @brief get vehicle angular-velocity wz */ - float64_t getWz() override; + double getWz() override; /** * @brief get vehicle steering angle */ - float64_t getSteer() override; + double getSteer() override; /** * @brief update vehicle states * @param [in] dt delta time [s] */ - void update(const float64_t & dt) override; + void update(const double & dt) override; /** * @brief calculate derivative of states with time delay steering model diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp index 7ba18d870635a..bc6e807144b52 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -40,9 +40,9 @@ class SimModelDelaySteerAccGeared : public SimModelInterface * @param [in] steer_time_constant time constant for 1D model of steering dynamics */ SimModelDelaySteerAccGeared( - float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, - float64_t wheelbase, float64_t dt, float64_t acc_delay, float64_t acc_time_constant, - float64_t steer_delay, float64_t steer_time_constant); + 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); /** * @brief default destructor @@ -50,7 +50,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface ~SimModelDelaySteerAccGeared() = default; private: - const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant enum IDX { X = 0, @@ -66,70 +66,70 @@ class SimModelDelaySteerAccGeared : public SimModelInterface DRIVE_SHIFT, }; - const float64_t vx_lim_; //!< @brief velocity limit [m/s] - const float64_t vx_rate_lim_; //!< @brief acceleration limit [m/ss] - const float64_t steer_lim_; //!< @brief steering limit [rad] - const float64_t steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] - const float64_t wheelbase_; //!< @brief vehicle wheelbase length [m] + 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 acc_input_queue_; //!< @brief buffer for accel command - std::deque steer_input_queue_; //!< @brief buffer for steering command - const float64_t acc_delay_; //!< @brief time delay for accel command [s] - const float64_t acc_time_constant_; //!< @brief time constant for accel dynamics - const float64_t steer_delay_; //!< @brief time delay for steering command [s] - const float64_t steer_time_constant_; //!< @brief time constant for steering dynamics + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque 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 /** * @brief set queue buffer for input command * @param [in] dt delta time */ - void initializeInputQueue(const float64_t & dt); + void initializeInputQueue(const double & dt); /** * @brief get vehicle position x */ - float64_t getX() override; + double getX() override; /** * @brief get vehicle position y */ - float64_t getY() override; + double getY() override; /** * @brief get vehicle angle yaw */ - float64_t getYaw() override; + double getYaw() override; /** * @brief get vehicle velocity vx */ - float64_t getVx() override; + double getVx() override; /** * @brief get vehicle lateral velocity */ - float64_t getVy() override; + double getVy() override; /** * @brief get vehicle longitudinal acceleration */ - float64_t getAx() override; + double getAx() override; /** * @brief get vehicle angular-velocity wz */ - float64_t getWz() override; + double getWz() override; /** * @brief get vehicle steering angle */ - float64_t getSteer() override; + double getSteer() override; /** * @brief update vehicle states * @param [in] dt delta time [s] */ - void update(const float64_t & dt) override; + void update(const double & dt) override; /** * @brief calculate derivative of states with time delay steering model diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp index 6303dfc0d546a..047742a2e8fa2 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp @@ -43,9 +43,9 @@ class SimModelDelaySteerVel : public SimModelInterface * @param [in] steer_time_constant time constant for 1D model of steering dynamics */ SimModelDelaySteerVel( - float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, - float64_t wheelbase, float64_t dt, float64_t vx_delay, float64_t vx_time_constant, - float64_t steer_delay, float64_t steer_time_constant); + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double vx_delay, double vx_time_constant, double steer_delay, + double steer_time_constant); /** * @brief destructor @@ -53,7 +53,7 @@ class SimModelDelaySteerVel : public SimModelInterface ~SimModelDelaySteerVel() = default; private: - const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant enum IDX { X = 0, @@ -67,74 +67,74 @@ class SimModelDelaySteerVel : public SimModelInterface STEER_DES, }; - const float64_t vx_lim_; //!< @brief velocity limit - const float64_t vx_rate_lim_; //!< @brief acceleration limit - const float64_t steer_lim_; //!< @brief steering limit [rad] - const float64_t steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] - const float64_t wheelbase_; //!< @brief vehicle wheelbase length [m] - float64_t prev_vx_ = 0.0; - float64_t current_ax_ = 0.0; - - std::deque vx_input_queue_; //!< @brief buffer for velocity command - std::deque steer_input_queue_; //!< @brief buffer for angular velocity command - const float64_t vx_delay_; //!< @brief time delay for velocity command [s] - const float64_t vx_time_constant_; + const double vx_lim_; //!< @brief velocity limit + const double vx_rate_lim_; //!< @brief acceleration limit + 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] + double prev_vx_ = 0.0; + double current_ax_ = 0.0; + + std::deque vx_input_queue_; //!< @brief buffer for velocity command + std::deque steer_input_queue_; //!< @brief buffer for angular velocity command + const double vx_delay_; //!< @brief time delay for velocity command [s] + const double vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics - const float64_t steer_delay_; //!< @brief time delay for angular-velocity command [s] - const float64_t + const double steer_delay_; //!< @brief time delay for angular-velocity command [s] + const double steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics /** * @brief set queue buffer for input command * @param [in] dt delta time */ - void initializeInputQueue(const float64_t & dt); + void initializeInputQueue(const double & dt); /** * @brief get vehicle position x */ - float64_t getX() override; + double getX() override; /** * @brief get vehicle position y */ - float64_t getY() override; + double getY() override; /** * @brief get vehicle angle yaw */ - float64_t getYaw() override; + double getYaw() override; /** * @brief get vehicle longitudinal velocity */ - float64_t getVx() override; + double getVx() override; /** * @brief get vehicle lateral velocity */ - float64_t getVy() override; + double getVy() override; /** * @brief get vehicle longitudinal acceleration */ - float64_t getAx() override; + double getAx() override; /** * @brief get vehicle angular-velocity wz */ - float64_t getWz() override; + double getWz() override; /** * @brief get vehicle steering angle */ - float64_t getSteer() override; + double getSteer() override; /** * @brief update vehicle states * @param [in] dt delta time [s] */ - void update(const float64_t & dt) override; + void update(const double & dt) override; /** * @brief calculate derivative of states with delay steering model diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp index b28227dc28275..d48e744ee93ea 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp @@ -32,7 +32,7 @@ class SimModelIdealSteerAcc : public SimModelInterface * @brief constructor * @param [in] wheelbase vehicle wheelbase length [m] */ - explicit SimModelIdealSteerAcc(float64_t wheelbase); + explicit SimModelIdealSteerAcc(double wheelbase); /** * @brief destructor @@ -46,53 +46,53 @@ class SimModelIdealSteerAcc : public SimModelInterface STEER_DES, }; - const float64_t wheelbase_; //!< @brief vehicle wheelbase length + const double wheelbase_; //!< @brief vehicle wheelbase length /** * @brief get vehicle position x */ - float64_t getX() override; + double getX() override; /** * @brief get vehicle position y */ - float64_t getY() override; + double getY() override; /** * @brief get vehicle angle yaw */ - float64_t getYaw() override; + double getYaw() override; /** * @brief get vehicle longitudinal velocity */ - float64_t getVx() override; + double getVx() override; /** * @brief get vehicle lateral velocity */ - float64_t getVy() override; + double getVy() override; /** * @brief get vehicle longitudinal acceleration */ - float64_t getAx() override; + double getAx() override; /** * @brief get vehicle angular-velocity wz */ - float64_t getWz() override; + double getWz() override; /** * @brief get vehicle steering angle */ - float64_t getSteer() override; + double getSteer() override; /** * @brief update vehicle states * @param [in] dt delta time [s] */ - void update(const float64_t & dt) override; + void update(const double & dt) override; /** * @brief calculate derivative of states with ideal steering model diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp index f8905fd220f1f..95e105592d2ea 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp @@ -32,7 +32,7 @@ class SimModelIdealSteerAccGeared : public SimModelInterface * @brief constructor * @param [in] wheelbase vehicle wheelbase length [m] */ - explicit SimModelIdealSteerAccGeared(float64_t wheelbase); + explicit SimModelIdealSteerAccGeared(double wheelbase); /** * @brief destructor @@ -46,54 +46,54 @@ class SimModelIdealSteerAccGeared : public SimModelInterface STEER_DES, }; - const float64_t wheelbase_; //!< @brief vehicle wheelbase length - float64_t current_acc_; //!< @brief current_acc with gear consideration + const double wheelbase_; //!< @brief vehicle wheelbase length + double current_acc_; //!< @brief current_acc with gear consideration /** * @brief get vehicle position x */ - float64_t getX() override; + double getX() override; /** * @brief get vehicle position y */ - float64_t getY() override; + double getY() override; /** * @brief get vehicle angle yaw */ - float64_t getYaw() override; + double getYaw() override; /** * @brief get vehicle longitudinal velocity */ - float64_t getVx() override; + double getVx() override; /** * @brief get vehicle lateral velocity */ - float64_t getVy() override; + double getVy() override; /** * @brief get vehicle longitudinal acceleration */ - float64_t getAx() override; + double getAx() override; /** * @brief get vehicle angular-velocity wz */ - float64_t getWz() override; + double getWz() override; /** * @brief get vehicle steering angle */ - float64_t getSteer() override; + double getSteer() override; /** * @brief update vehicle states * @param [in] dt delta time [s] */ - void update(const float64_t & dt) override; + void update(const double & dt) override; /** * @brief calculate derivative of states with ideal steering model diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp index 1fbdb9f32f92f..cc4b1b4ea3651 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp @@ -31,7 +31,7 @@ class SimModelIdealSteerVel : public SimModelInterface * @brief constructor * @param [in] wheelbase vehicle wheelbase length [m] */ - explicit SimModelIdealSteerVel(float64_t wheelbase); + explicit SimModelIdealSteerVel(double wheelbase); /** * @brief destructor @@ -45,55 +45,55 @@ class SimModelIdealSteerVel : public SimModelInterface STEER_DES, }; - const float64_t wheelbase_; //!< @brief vehicle wheelbase length - float64_t prev_vx_ = 0.0; - float64_t current_ax_ = 0.0; + const double wheelbase_; //!< @brief vehicle wheelbase length + double prev_vx_ = 0.0; + double current_ax_ = 0.0; /** * @brief get vehicle position x */ - float64_t getX() override; + double getX() override; /** * @brief get vehicle position y */ - float64_t getY() override; + double getY() override; /** * @brief get vehicle angle yaw */ - float64_t getYaw() override; + double getYaw() override; /** * @brief get vehicle longitudinal velocity */ - float64_t getVx() override; + double getVx() override; /** * @brief get vehicle lateral velocity */ - float64_t getVy() override; + double getVy() override; /** * @brief get vehicle longitudinal acceleration */ - float64_t getAx() override; + double getAx() override; /** * @brief get vehicle angular-velocity wz */ - float64_t getWz() override; + double getWz() override; /** * @brief get vehicle steering angle */ - float64_t getSteer() override; + double getSteer() override; /** * @brief update vehicle states * @param [in] dt delta time [s] */ - void update(const float64_t & dt) override; + void update(const double & dt) override; /** * @brief calculate derivative of states with ideal steering model 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 4cfe4cf0090d8..40711cfe9e720 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 @@ -15,15 +15,10 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ -#include "common/types.hpp" #include "eigen3/Eigen/Core" #include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" -using autoware::common::types::bool8_t; -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; - /** * @class SimModelInterface * @brief simple_planning_simulator vehicle model class to calculate vehicle dynamics @@ -87,60 +82,60 @@ class SimModelInterface * @param [in] dt delta time [s] * @param [in] input vehicle input */ - void updateRungeKutta(const float64_t & dt, const Eigen::VectorXd & input); + void updateRungeKutta(const double & dt, const Eigen::VectorXd & input); /** * @brief update vehicle states with Euler methods * @param [in] dt delta time [s] * @param [in] input vehicle input */ - void updateEuler(const float64_t & dt, const Eigen::VectorXd & input); + void updateEuler(const double & dt, const Eigen::VectorXd & input); /** * @brief update vehicle states * @param [in] dt delta time [s] */ - virtual void update(const float64_t & dt) = 0; + virtual void update(const double & dt) = 0; /** * @brief get vehicle position x */ - virtual float64_t getX() = 0; + virtual double getX() = 0; /** * @brief get vehicle position y */ - virtual float64_t getY() = 0; + virtual double getY() = 0; /** * @brief get vehicle angle yaw */ - virtual float64_t getYaw() = 0; + virtual double getYaw() = 0; /** * @brief get vehicle velocity vx */ - virtual float64_t getVx() = 0; + virtual double getVx() = 0; /** * @brief get vehicle lateral velocity */ - virtual float64_t getVy() = 0; + virtual double getVy() = 0; /** * @brief get vehicle longitudinal acceleration */ - virtual float64_t getAx() = 0; + virtual double getAx() = 0; /** * @brief get vehicle angular-velocity wz */ - virtual float64_t getWz() = 0; + virtual double getWz() = 0; /** * @brief get vehicle steering angle */ - virtual float64_t getSteer() = 0; + virtual double getSteer() = 0; /** * @brief get vehicle gear diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 9f3913f64ec60..c731acd3e3e45 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -11,7 +11,6 @@ autoware_cmake - autoware_auto_common autoware_auto_control_msgs autoware_auto_planning_msgs autoware_auto_tf2 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 1aa28b1c150e8..02c1924d5e899 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 @@ -15,7 +15,6 @@ #include "simple_planning_simulator/simple_planning_simulator_core.hpp" #include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" -#include "common/types.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -40,9 +39,9 @@ autoware_auto_vehicle_msgs::msg::VelocityReport to_velocity_report( const std::shared_ptr vehicle_model_ptr) { autoware_auto_vehicle_msgs::msg::VelocityReport velocity; - velocity.longitudinal_velocity = static_cast(vehicle_model_ptr->getVx()); + velocity.longitudinal_velocity = static_cast(vehicle_model_ptr->getVx()); velocity.lateral_velocity = 0.0F; - velocity.heading_rate = static_cast(vehicle_model_ptr->getWz()); + velocity.heading_rate = static_cast(vehicle_model_ptr->getWz()); return velocity; } @@ -63,7 +62,7 @@ autoware_auto_vehicle_msgs::msg::SteeringReport to_steering_report( const std::shared_ptr vehicle_model_ptr) { autoware_auto_vehicle_msgs::msg::SteeringReport steer; - steer.steering_tire_angle = static_cast(vehicle_model_ptr->getSteer()); + steer.steering_tire_angle = static_cast(vehicle_model_ptr->getSteer()); return steer; } @@ -165,10 +164,10 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt std::random_device seed; auto & m = measurement_noise_; m.rand_engine_ = std::make_shared(seed()); - float64_t pos_noise_stddev = declare_parameter("pos_noise_stddev", 1e-2); - float64_t vel_noise_stddev = declare_parameter("vel_noise_stddev", 1e-2); - float64_t rpy_noise_stddev = declare_parameter("rpy_noise_stddev", 1e-4); - float64_t steer_noise_stddev = declare_parameter("steer_noise_stddev", 1e-4); + double pos_noise_stddev = declare_parameter("pos_noise_stddev", 1e-2); + double vel_noise_stddev = declare_parameter("vel_noise_stddev", 1e-2); + double rpy_noise_stddev = declare_parameter("rpy_noise_stddev", 1e-4); + double steer_noise_stddev = declare_parameter("steer_noise_stddev", 1e-4); m.pos_dist_ = std::make_shared>(0.0, pos_noise_stddev); m.vel_dist_ = std::make_shared>(0.0, vel_noise_stddev); m.rpy_dist_ = std::make_shared>(0.0, rpy_noise_stddev); @@ -189,18 +188,18 @@ void SimplePlanningSimulator::initialize_vehicle_model() RCLCPP_INFO(this->get_logger(), "vehicle_model_type = %s", vehicle_model_type_str.c_str()); - const float64_t vel_lim = declare_parameter("vel_lim", 50.0); - const float64_t vel_rate_lim = declare_parameter("vel_rate_lim", 7.0); - const float64_t steer_lim = declare_parameter("steer_lim", 1.0); - const float64_t steer_rate_lim = declare_parameter("steer_rate_lim", 5.0); - const float64_t acc_time_delay = declare_parameter("acc_time_delay", 0.1); - const float64_t acc_time_constant = declare_parameter("acc_time_constant", 0.1); - const float64_t vel_time_delay = declare_parameter("vel_time_delay", 0.25); - const float64_t vel_time_constant = declare_parameter("vel_time_constant", 0.5); - const float64_t steer_time_delay = declare_parameter("steer_time_delay", 0.24); - const float64_t steer_time_constant = declare_parameter("steer_time_constant", 0.27); + const double vel_lim = declare_parameter("vel_lim", 50.0); + const double vel_rate_lim = declare_parameter("vel_rate_lim", 7.0); + const double steer_lim = declare_parameter("steer_lim", 1.0); + const double steer_rate_lim = declare_parameter("steer_rate_lim", 5.0); + const double acc_time_delay = declare_parameter("acc_time_delay", 0.1); + const double acc_time_constant = declare_parameter("acc_time_constant", 0.1); + const double vel_time_delay = declare_parameter("vel_time_delay", 0.25); + const double vel_time_constant = declare_parameter("vel_time_constant", 0.5); + const double steer_time_delay = declare_parameter("steer_time_delay", 0.24); + const double steer_time_constant = declare_parameter("steer_time_constant", 0.27); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); - const float64_t wheelbase = vehicle_info.wheel_base_m; + const double wheelbase = vehicle_info.wheel_base_m; if (vehicle_model_type_str == "IDEAL_STEER_VEL") { vehicle_model_type_ = VehicleModelType::IDEAL_STEER_VEL; @@ -258,7 +257,7 @@ void SimplePlanningSimulator::on_timer() // update vehicle dynamics { - const float64_t dt = delta_time_.get_dt(get_clock()->now()); + const double dt = delta_time_.get_dt(get_clock()->now()); if (current_control_mode_.mode == ControlModeReport::AUTONOMOUS) { vehicle_model_ptr_->setGear(current_gear_cmd_.command); @@ -410,13 +409,13 @@ void SimplePlanningSimulator::add_measurement_noise( odom.pose.pose.position.y += (*n.pos_dist_)(*n.rand_engine_); const auto velocity_noise = (*n.vel_dist_)(*n.rand_engine_); odom.twist.twist.linear.x += velocity_noise; - float32_t yaw = tf2::getYaw(odom.pose.pose.orientation); + double yaw = tf2::getYaw(odom.pose.pose.orientation); yaw += static_cast((*n.rpy_dist_)(*n.rand_engine_)); odom.pose.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - vel.longitudinal_velocity += static_cast(velocity_noise); + vel.longitudinal_velocity += static_cast(velocity_noise); - steer.steering_tire_angle += static_cast((*n.steer_dist_)(*n.rand_engine_)); + steer.steering_tire_angle += static_cast((*n.steer_dist_)(*n.rand_engine_)); } void SimplePlanningSimulator::set_initial_state_with_transform( @@ -433,12 +432,12 @@ void SimplePlanningSimulator::set_initial_state_with_transform( void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & twist) { - const float64_t x = pose.position.x; - const float64_t y = pose.position.y; - const float64_t yaw = tf2::getYaw(pose.orientation); - const float64_t vx = twist.linear.x; - const float64_t steer = 0.0; - const float64_t accx = 0.0; + const double x = pose.position.x; + const double y = pose.position.y; + const double yaw = tf2::getYaw(pose.orientation); + const double vx = twist.linear.x; + const double steer = 0.0; + const double accx = 0.0; Eigen::VectorXd state(vehicle_model_ptr_->getDimX()); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index 8ce4425e56eba..a86a896b2dd0e 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -17,9 +17,9 @@ #include SimModelDelaySteerAcc::SimModelDelaySteerAcc( - float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, - float64_t wheelbase, float64_t dt, float64_t acc_delay, float64_t acc_time_constant, - float64_t steer_delay, float64_t steer_time_constant) + 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) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -35,18 +35,18 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( initializeInputQueue(dt); } -float64_t SimModelDelaySteerAcc::getX() { return state_(IDX::X); } -float64_t SimModelDelaySteerAcc::getY() { return state_(IDX::Y); } -float64_t SimModelDelaySteerAcc::getYaw() { return state_(IDX::YAW); } -float64_t SimModelDelaySteerAcc::getVx() { return state_(IDX::VX); } -float64_t SimModelDelaySteerAcc::getVy() { return 0.0; } -float64_t SimModelDelaySteerAcc::getAx() { return state_(IDX::ACCX); } -float64_t SimModelDelaySteerAcc::getWz() +double SimModelDelaySteerAcc::getX() { return state_(IDX::X); } +double SimModelDelaySteerAcc::getY() { return state_(IDX::Y); } +double SimModelDelaySteerAcc::getYaw() { return state_(IDX::YAW); } +double SimModelDelaySteerAcc::getVx() { return state_(IDX::VX); } +double SimModelDelaySteerAcc::getVy() { return 0.0; } +double SimModelDelaySteerAcc::getAx() { return state_(IDX::ACCX); } +double SimModelDelaySteerAcc::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; } -float64_t SimModelDelaySteerAcc::getSteer() { return state_(IDX::STEER); } -void SimModelDelaySteerAcc::update(const float64_t & dt) +double SimModelDelaySteerAcc::getSteer() { return state_(IDX::STEER); } +void SimModelDelaySteerAcc::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -62,7 +62,7 @@ void SimModelDelaySteerAcc::update(const float64_t & dt) state_(IDX::VX) = std::max(-vx_lim_, std::min(state_(IDX::VX), vx_lim_)); } -void SimModelDelaySteerAcc::initializeInputQueue(const float64_t & dt) +void SimModelDelaySteerAcc::initializeInputQueue(const double & dt) { size_t acc_input_queue_size = static_cast(round(acc_delay_ / dt)); acc_input_queue_.resize(acc_input_queue_size); @@ -76,15 +76,15 @@ void SimModelDelaySteerAcc::initializeInputQueue(const float64_t & dt) Eigen::VectorXd SimModelDelaySteerAcc::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { - auto sat = [](float64_t val, float64_t u, float64_t l) { return std::max(std::min(val, u), l); }; + auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); }; - const float64_t vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); - const float64_t acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); - const float64_t yaw = state(IDX::YAW); - const float64_t steer = state(IDX::STEER); - const float64_t acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); - const float64_t steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); - float64_t steer_rate = -(steer - steer_des) / steer_time_constant_; + const double vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); + const double acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); + const double yaw = state(IDX::YAW); + const double steer = state(IDX::STEER); + const double acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); + const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); + double steer_rate = -(steer - steer_des) / steer_time_constant_; steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index e27bb45a0b925..1af8d21bfe4d7 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -19,9 +19,9 @@ #include SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( - float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, - float64_t wheelbase, float64_t dt, float64_t acc_delay, float64_t acc_time_constant, - float64_t steer_delay, float64_t steer_time_constant) + 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) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -37,18 +37,18 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( initializeInputQueue(dt); } -float64_t SimModelDelaySteerAccGeared::getX() { return state_(IDX::X); } -float64_t SimModelDelaySteerAccGeared::getY() { return state_(IDX::Y); } -float64_t SimModelDelaySteerAccGeared::getYaw() { return state_(IDX::YAW); } -float64_t SimModelDelaySteerAccGeared::getVx() { return state_(IDX::VX); } -float64_t SimModelDelaySteerAccGeared::getVy() { return 0.0; } -float64_t SimModelDelaySteerAccGeared::getAx() { return state_(IDX::ACCX); } -float64_t SimModelDelaySteerAccGeared::getWz() +double SimModelDelaySteerAccGeared::getX() { return state_(IDX::X); } +double SimModelDelaySteerAccGeared::getY() { return state_(IDX::Y); } +double SimModelDelaySteerAccGeared::getYaw() { return state_(IDX::YAW); } +double SimModelDelaySteerAccGeared::getVx() { return state_(IDX::VX); } +double SimModelDelaySteerAccGeared::getVy() { return 0.0; } +double SimModelDelaySteerAccGeared::getAx() { return state_(IDX::ACCX); } +double SimModelDelaySteerAccGeared::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; } -float64_t SimModelDelaySteerAccGeared::getSteer() { return state_(IDX::STEER); } -void SimModelDelaySteerAccGeared::update(const float64_t & dt) +double SimModelDelaySteerAccGeared::getSteer() { return state_(IDX::STEER); } +void SimModelDelaySteerAccGeared::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -70,7 +70,7 @@ void SimModelDelaySteerAccGeared::update(const float64_t & dt) updateStateWithGear(state_, prev_state, gear_, dt); } -void SimModelDelaySteerAccGeared::initializeInputQueue(const float64_t & dt) +void SimModelDelaySteerAccGeared::initializeInputQueue(const double & dt) { size_t acc_input_queue_size = static_cast(round(acc_delay_ / dt)); acc_input_queue_.resize(acc_input_queue_size); @@ -84,15 +84,15 @@ void SimModelDelaySteerAccGeared::initializeInputQueue(const float64_t & dt) Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { - auto sat = [](float64_t val, float64_t u, float64_t l) { return std::max(std::min(val, u), l); }; - - const float64_t vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); - const float64_t acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); - const float64_t yaw = state(IDX::YAW); - const float64_t steer = state(IDX::STEER); - const float64_t acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); - const float64_t steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); - float64_t steer_rate = -(steer - steer_des) / steer_time_constant_; + auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); }; + + const double vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); + const double acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); + const double yaw = state(IDX::YAW); + const double steer = state(IDX::STEER); + const double acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); + const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); + double steer_rate = -(steer - steer_des) / steer_time_constant_; steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp index c6ba87965e5ee..82973a2e8a004 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp @@ -17,9 +17,9 @@ #include SimModelDelaySteerVel::SimModelDelaySteerVel( - float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, - float64_t wheelbase, float64_t dt, float64_t vx_delay, float64_t vx_time_constant, - float64_t steer_delay, float64_t steer_time_constant) + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double vx_delay, double vx_time_constant, double steer_delay, + double steer_time_constant) : SimModelInterface(5 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -35,18 +35,18 @@ SimModelDelaySteerVel::SimModelDelaySteerVel( initializeInputQueue(dt); } -float64_t SimModelDelaySteerVel::getX() { return state_(IDX::X); } -float64_t SimModelDelaySteerVel::getY() { return state_(IDX::Y); } -float64_t SimModelDelaySteerVel::getYaw() { return state_(IDX::YAW); } -float64_t SimModelDelaySteerVel::getVx() { return state_(IDX::VX); } -float64_t SimModelDelaySteerVel::getVy() { return 0.0; } -float64_t SimModelDelaySteerVel::getAx() { return current_ax_; } -float64_t SimModelDelaySteerVel::getWz() +double SimModelDelaySteerVel::getX() { return state_(IDX::X); } +double SimModelDelaySteerVel::getY() { return state_(IDX::Y); } +double SimModelDelaySteerVel::getYaw() { return state_(IDX::YAW); } +double SimModelDelaySteerVel::getVx() { return state_(IDX::VX); } +double SimModelDelaySteerVel::getVy() { return 0.0; } +double SimModelDelaySteerVel::getAx() { return current_ax_; } +double SimModelDelaySteerVel::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; } -float64_t SimModelDelaySteerVel::getSteer() { return state_(IDX::STEER); } -void SimModelDelaySteerVel::update(const float64_t & dt) +double SimModelDelaySteerVel::getSteer() { return state_(IDX::STEER); } +void SimModelDelaySteerVel::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -62,7 +62,7 @@ void SimModelDelaySteerVel::update(const float64_t & dt) prev_vx_ = input_(IDX_U::VX_DES); } -void SimModelDelaySteerVel::initializeInputQueue(const float64_t & dt) +void SimModelDelaySteerVel::initializeInputQueue(const double & dt) { size_t vx_input_queue_size = static_cast(round(vx_delay_ / dt)); for (size_t i = 0; i < vx_input_queue_size; i++) { @@ -77,17 +77,17 @@ void SimModelDelaySteerVel::initializeInputQueue(const float64_t & dt) Eigen::VectorXd SimModelDelaySteerVel::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { - auto sat = [](float64_t val, float64_t u, float64_t l) { return std::max(std::min(val, u), l); }; + auto sat = [](double val, double u, double l) { return std::max(std::min(val, u), l); }; - const float64_t vx = sat(state(IDX::VX), vx_lim_, -vx_lim_); - const float64_t steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); - const float64_t yaw = state(IDX::YAW); - const float64_t delay_input_vx = input(IDX_U::VX_DES); - const float64_t delay_input_steer = input(IDX_U::STEER_DES); - const float64_t delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); - const float64_t delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); - float64_t vx_rate = -(vx - delay_vx_des) / vx_time_constant_; - float64_t steer_rate = -(steer - delay_steer_des) / steer_time_constant_; + const double vx = sat(state(IDX::VX), vx_lim_, -vx_lim_); + const double steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); + const double yaw = state(IDX::YAW); + const double delay_input_vx = input(IDX_U::VX_DES); + const double delay_input_steer = input(IDX_U::STEER_DES); + const double delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); + const double delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); + double vx_rate = -(vx - delay_vx_des) / vx_time_constant_; + double steer_rate = -(steer - delay_steer_des) / steer_time_constant_; vx_rate = sat(vx_rate, vx_rate_lim_, -vx_rate_lim_); steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp index d8f1b13c0b22a..08dc3f2e34116 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp @@ -14,31 +14,31 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" -SimModelIdealSteerAcc::SimModelIdealSteerAcc(float64_t wheelbase) +SimModelIdealSteerAcc::SimModelIdealSteerAcc(double wheelbase) : SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) { } -float64_t SimModelIdealSteerAcc::getX() { return state_(IDX::X); } -float64_t SimModelIdealSteerAcc::getY() { return state_(IDX::Y); } -float64_t SimModelIdealSteerAcc::getYaw() { return state_(IDX::YAW); } -float64_t SimModelIdealSteerAcc::getVx() { return state_(IDX::VX); } -float64_t SimModelIdealSteerAcc::getVy() { return 0.0; } -float64_t SimModelIdealSteerAcc::getAx() { return input_(IDX_U::AX_DES); } -float64_t SimModelIdealSteerAcc::getWz() +double SimModelIdealSteerAcc::getX() { return state_(IDX::X); } +double SimModelIdealSteerAcc::getY() { return state_(IDX::Y); } +double SimModelIdealSteerAcc::getYaw() { return state_(IDX::YAW); } +double SimModelIdealSteerAcc::getVx() { return state_(IDX::VX); } +double SimModelIdealSteerAcc::getVy() { return 0.0; } +double SimModelIdealSteerAcc::getAx() { return input_(IDX_U::AX_DES); } +double SimModelIdealSteerAcc::getWz() { return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -float64_t SimModelIdealSteerAcc::getSteer() { return input_(IDX_U::STEER_DES); } -void SimModelIdealSteerAcc::update(const float64_t & dt) { updateRungeKutta(dt, input_); } +double SimModelIdealSteerAcc::getSteer() { return input_(IDX_U::STEER_DES); } +void SimModelIdealSteerAcc::update(const double & dt) { updateRungeKutta(dt, input_); } Eigen::VectorXd SimModelIdealSteerAcc::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { - const float64_t vx = state(IDX::VX); - const float64_t yaw = state(IDX::YAW); - const float64_t ax = input(IDX_U::AX_DES); - const float64_t steer = input(IDX_U::STEER_DES); + const double vx = state(IDX::VX); + const double yaw = state(IDX::YAW); + const double ax = input(IDX_U::AX_DES); + const double steer = input(IDX_U::STEER_DES); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vx * std::cos(yaw); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index 8ec9f13822ee4..70341dcdee102 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -18,23 +18,23 @@ #include -SimModelIdealSteerAccGeared::SimModelIdealSteerAccGeared(float64_t wheelbase) +SimModelIdealSteerAccGeared::SimModelIdealSteerAccGeared(double wheelbase) : SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase), current_acc_(0.0) { } -float64_t SimModelIdealSteerAccGeared::getX() { return state_(IDX::X); } -float64_t SimModelIdealSteerAccGeared::getY() { return state_(IDX::Y); } -float64_t SimModelIdealSteerAccGeared::getYaw() { return state_(IDX::YAW); } -float64_t SimModelIdealSteerAccGeared::getVx() { return state_(IDX::VX); } -float64_t SimModelIdealSteerAccGeared::getVy() { return 0.0; } -float64_t SimModelIdealSteerAccGeared::getAx() { return current_acc_; } -float64_t SimModelIdealSteerAccGeared::getWz() +double SimModelIdealSteerAccGeared::getX() { return state_(IDX::X); } +double SimModelIdealSteerAccGeared::getY() { return state_(IDX::Y); } +double SimModelIdealSteerAccGeared::getYaw() { return state_(IDX::YAW); } +double SimModelIdealSteerAccGeared::getVx() { return state_(IDX::VX); } +double SimModelIdealSteerAccGeared::getVy() { return 0.0; } +double SimModelIdealSteerAccGeared::getAx() { return current_acc_; } +double SimModelIdealSteerAccGeared::getWz() { return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -float64_t SimModelIdealSteerAccGeared::getSteer() { return input_(IDX_U::STEER_DES); } -void SimModelIdealSteerAccGeared::update(const float64_t & dt) +double SimModelIdealSteerAccGeared::getSteer() { return input_(IDX_U::STEER_DES); } +void SimModelIdealSteerAccGeared::update(const double & dt) { const auto prev_state = state_; updateRungeKutta(dt, input_); @@ -47,10 +47,10 @@ void SimModelIdealSteerAccGeared::update(const float64_t & dt) Eigen::VectorXd SimModelIdealSteerAccGeared::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { - const float64_t vx = state(IDX::VX); - const float64_t yaw = state(IDX::YAW); - const float64_t ax = input(IDX_U::AX_DES); - const float64_t steer = input(IDX_U::STEER_DES); + const double vx = state(IDX::VX); + const double yaw = state(IDX::YAW); + const double ax = input(IDX_U::AX_DES); + const double steer = input(IDX_U::STEER_DES); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vx * std::cos(yaw); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp index 33eead9d10128..e126f442c1e91 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp @@ -14,23 +14,23 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" -SimModelIdealSteerVel::SimModelIdealSteerVel(float64_t wheelbase) +SimModelIdealSteerVel::SimModelIdealSteerVel(double wheelbase) : SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) { } -float64_t SimModelIdealSteerVel::getX() { return state_(IDX::X); } -float64_t SimModelIdealSteerVel::getY() { return state_(IDX::Y); } -float64_t SimModelIdealSteerVel::getYaw() { return state_(IDX::YAW); } -float64_t SimModelIdealSteerVel::getVx() { return input_(IDX_U::VX_DES); } -float64_t SimModelIdealSteerVel::getVy() { return 0.0; } -float64_t SimModelIdealSteerVel::getAx() { return current_ax_; } -float64_t SimModelIdealSteerVel::getWz() +double SimModelIdealSteerVel::getX() { return state_(IDX::X); } +double SimModelIdealSteerVel::getY() { return state_(IDX::Y); } +double SimModelIdealSteerVel::getYaw() { return state_(IDX::YAW); } +double SimModelIdealSteerVel::getVx() { return input_(IDX_U::VX_DES); } +double SimModelIdealSteerVel::getVy() { return 0.0; } +double SimModelIdealSteerVel::getAx() { return current_ax_; } +double SimModelIdealSteerVel::getWz() { return input_(IDX_U::VX_DES) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -float64_t SimModelIdealSteerVel::getSteer() { return input_(IDX_U::STEER_DES); } -void SimModelIdealSteerVel::update(const float64_t & dt) +double SimModelIdealSteerVel::getSteer() { return input_(IDX_U::STEER_DES); } +void SimModelIdealSteerVel::update(const double & dt) { updateRungeKutta(dt, input_); current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; @@ -40,9 +40,9 @@ void SimModelIdealSteerVel::update(const float64_t & dt) Eigen::VectorXd SimModelIdealSteerVel::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { - const float64_t yaw = state(IDX::YAW); - const float64_t vx = input(IDX_U::VX_DES); - const float64_t steer = input(IDX_U::STEER_DES); + const double yaw = state(IDX::YAW); + const double vx = input(IDX_U::VX_DES); + const double steer = input(IDX_U::STEER_DES); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vx * std::cos(yaw); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp index 55650364d801c..0740540801f68 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp @@ -20,7 +20,7 @@ SimModelInterface::SimModelInterface(int dim_x, int dim_u) : dim_x_(dim_x), dim_ input_ = Eigen::VectorXd::Zero(dim_u_); } -void SimModelInterface::updateRungeKutta(const float64_t & dt, const Eigen::VectorXd & input) +void SimModelInterface::updateRungeKutta(const double & dt, const Eigen::VectorXd & input) { Eigen::VectorXd k1 = calcModel(state_, input); Eigen::VectorXd k2 = calcModel(state_ + k1 * 0.5 * dt, input); @@ -29,7 +29,7 @@ void SimModelInterface::updateRungeKutta(const float64_t & dt, const Eigen::Vect state_ += 1.0 / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4) * dt; } -void SimModelInterface::updateEuler(const float64_t & dt, const Eigen::VectorXd & input) +void SimModelInterface::updateEuler(const double & dt, const Eigen::VectorXd & input) { state_ += calcModel(state_, input) * dt; } 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 9e770bd52bbb1..e57a3683b97a9 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -75,8 +75,8 @@ class PubSubNode : public rclcpp::Node * @param [in] jerk [m/s3] jerk */ AckermannControlCommand cmdGen( - const builtin_interfaces::msg::Time & t, float32_t steer, float32_t steer_rate, float32_t vel, - float32_t acc, float32_t jerk) + const builtin_interfaces::msg::Time & t, double steer, double steer_rate, double vel, double acc, + double jerk) { AckermannControlCommand cmd; cmd.stamp = t; @@ -148,34 +148,34 @@ void sendCommand( // void isOnForward(const Odometry & state, const Odometry & init) { - float64_t forward_thr = 1.0; - float64_t dx = state.pose.pose.position.x - init.pose.pose.position.x; + double forward_thr = 1.0; + double dx = state.pose.pose.position.x - init.pose.pose.position.x; EXPECT_GT(dx, forward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); } void isOnBackward(const Odometry & state, const Odometry & init) { - float64_t backward_thr = -1.0; - float64_t dx = state.pose.pose.position.x - init.pose.pose.position.x; + double backward_thr = -1.0; + double dx = state.pose.pose.position.x - init.pose.pose.position.x; EXPECT_LT(dx, backward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); } void isOnForwardLeft(const Odometry & state, const Odometry & init) { - float64_t forward_thr = 1.0; - float64_t left_thr = 0.1f; - float64_t dx = state.pose.pose.position.x - init.pose.pose.position.x; - float64_t dy = state.pose.pose.position.y - init.pose.pose.position.y; + double forward_thr = 1.0; + double left_thr = 0.1f; + double dx = state.pose.pose.position.x - init.pose.pose.position.x; + double dy = state.pose.pose.position.y - init.pose.pose.position.y; EXPECT_GT(dx, forward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); EXPECT_GT(dy, left_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); } void isOnBackwardRight(const Odometry & state, const Odometry & init) { - float64_t backward_thr = -1.0; - float64_t right_thr = -0.1; - float64_t dx = state.pose.pose.position.x - init.pose.pose.position.x; - float64_t dy = state.pose.pose.position.y - init.pose.pose.position.y; + double backward_thr = -1.0; + double right_thr = -0.1; + double dx = state.pose.pose.position.x - init.pose.pose.position.x; + double dy = state.pose.pose.position.y - init.pose.pose.position.y; EXPECT_LT(dx, backward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); EXPECT_LT(dy, right_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); } @@ -217,9 +217,9 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) const auto pub_sub_node = std::make_shared(); - const float32_t target_vel = 5.0f; - const float32_t target_acc = 5.0f; - const float32_t target_steer = 0.2f; + const double target_vel = 5.0f; + const double target_acc = 5.0f; + const double target_steer = 0.2f; auto _resetInitialpose = [&]() { resetInitialpose(sim_node, pub_sub_node); }; auto _sendFwdGear = [&]() { sendGear(GearCommand::DRIVE, sim_node, pub_sub_node); };