diff --git a/common/hardcoded_params/include/hardcoded_params/control_limits/control_limits.h b/common/hardcoded_params/include/hardcoded_params/control_limits/control_limits.h index 96f89c5432d..3a2b6270fd3 100644 --- a/common/hardcoded_params/include/hardcoded_params/control_limits/control_limits.h +++ b/common/hardcoded_params/include/hardcoded_params/control_limits/control_limits.h @@ -21,9 +21,19 @@ namespace hardcoded_params namespace control_limits { /** - * The maximum allowable longitudinal velocity of the vehicle in m/s + * The maximum allowable longitudinal velocity of the vehicle in m/s */ constexpr double MAX_LONGITUDINAL_VELOCITY_MPS = 35.7632; constexpr double MAX_LONGITUDINAL_ACCEL_MPS2 = 3.5; + +/** +* The maximum allowable simulation parameters +* At the time of writing this, platform uses CARLA 0.9.10 as the vehicle's physics simulator. +* CARLA's this version uses below hardcoded value for its vehicles (so even if increasing this value, +* actual simulation won't support larger values unless forked and compiled) +* https://github.com/carla-simulator/ros-bridge/blob/0.9.10.1/carla_ackermann_control/src/carla_ackermann_control/carla_control_physics.py#L254 +*/ +constexpr double MAX_SIMULATION_LONGITUDINAL_ACCEL_MPS2 = 8.0; + } } // namespace hardcoded_params diff --git a/core_planning/twist_filter/include/accel_limiter.hpp b/core_planning/twist_filter/include/accel_limiter.hpp index a8ab326b119..69a1e2ced70 100644 --- a/core_planning/twist_filter/include/accel_limiter.hpp +++ b/core_planning/twist_filter/include/accel_limiter.hpp @@ -8,10 +8,12 @@ namespace twist_filter{ constexpr double MAX_LONGITUDINAL_ACCEL_HARDCODED_LIMIT_M_S_2 = hardcoded_params::control_limits::MAX_LONGITUDINAL_ACCEL_MPS2; +constexpr double MAX_SIMULATION_LONGITUDINAL_ACCEL_HARDCODED_LIMIT_M_S_2 = hardcoded_params::control_limits::MAX_SIMULATION_LONGITUDINAL_ACCEL_MPS2; + class LongitudinalAccelLimiter { - public: + public: /** * Default constructor, not recommended for use. */ @@ -24,7 +26,7 @@ class LongitudinalAccelLimiter /** * \brief Constructor for LongitudinalAccelLimiter - * + * * \param accel_limit The acceleration limit that should be enforced, in * units of m/s/s */ @@ -32,13 +34,13 @@ class LongitudinalAccelLimiter _accel_limit(accel_limit), _initialized(false), _prev_v(0.0), - _prev_t(0.0) {}; + _prev_t(0.0) {}; /** * Limit the longitudinal acceleration found in the input ControlCommandStamped - * + * * \param msg The message to be evaluated - * \return A copy of the message with the longitudinal accel limited + * \return A copy of the message with the longitudinal accel limited * based on params or hardcoded limit */ autoware_msgs::msg::ControlCommandStamped @@ -46,9 +48,9 @@ class LongitudinalAccelLimiter /** * Limit the longitudinal acceleration found in the input TwistStamped - * + * * \param msg The message to be evaluated - * \return A copy of the message with the longitudinal accel limited + * \return A copy of the message with the longitudinal accel limited * based on params or hardcoded limit */ geometry_msgs::msg::TwistStamped @@ -58,7 +60,7 @@ class LongitudinalAccelLimiter double _accel_limit; bool _initialized; double _prev_v; - rclcpp::Time _prev_t; + rclcpp::Time _prev_t; }; } \ No newline at end of file diff --git a/core_planning/twist_filter/include/twist_filter.hpp b/core_planning/twist_filter/include/twist_filter.hpp index 1a609265163..b23d86415e5 100644 --- a/core_planning/twist_filter/include/twist_filter.hpp +++ b/core_planning/twist_filter/include/twist_filter.hpp @@ -19,7 +19,7 @@ * Modifications: * - Added limiting for longitudinal velocity per CARMA specifications. Refactored * namespacing and header as needed to support unit testing. - * - Kyle Rush + * - Kyle Rush * - 9/11/2020 * - Refactored for ros2 * - 11/16/2022 @@ -75,12 +75,12 @@ class TwistFilter : public carma_ros2_utils::CarmaLifecycleNode carma_ros2_utils::PubPtr ctrl_lacc_limit_debug_pub_, ctrl_ljerk_limit_debug_pub_; carma_ros2_utils::PubPtr twist_lacc_result_pub_, twist_ljerk_result_pub_; carma_ros2_utils::PubPtr ctrl_lacc_result_pub_, ctrl_ljerk_result_pub_; - + //subscribers carma_ros2_utils::SubPtr twist_sub_; carma_ros2_utils::SubPtr ctrl_sub_; carma_ros2_utils::SubPtr config_sub_; - + //ros params double wheel_base_; double longitudinal_velocity_limit_; diff --git a/core_planning/twist_filter/src/twist_filter.cpp b/core_planning/twist_filter/src/twist_filter.cpp index 8e9ca1e1f5b..62a1cdffb2c 100644 --- a/core_planning/twist_filter/src/twist_filter.cpp +++ b/core_planning/twist_filter/src/twist_filter.cpp @@ -18,7 +18,7 @@ * Modifications: * - Added limiting for longitudinal velocity per CARMA specifications. Refactored * namespacing and header as needed to support unit testing. - * - Kyle Rush + * - Kyle Rush * - 9/11/2020 */ @@ -53,17 +53,34 @@ carma_ros2_utils::CallbackReturn TwistFilter::handle_on_configure(const rclcpp_l // parameters on private handles get_parameter("config_speed_limit", longitudinal_velocity_limit_); longitudinal_velocity_limit_ = longitudinal_velocity_limit_ * 0.44704; - + get_parameter("vehicle_acceleration_limit", longitudinal_accel_limit_); + + bool use_sim_time = false; + get_parameter("use_sim_time", use_sim_time); + + auto hardcoded_acceleration_limit = MAX_LONGITUDINAL_ACCEL_HARDCODED_LIMIT_M_S_2; + + // if simulation + if (use_sim_time) + { + hardcoded_acceleration_limit = MAX_SIMULATION_LONGITUDINAL_ACCEL_HARDCODED_LIMIT_M_S_2; + RCLCPP_INFO_STREAM(rclcpp::get_logger("logger"), "Simulation mode detected. Accounting hardcoded limit longitudinal acceleration limit used: " << hardcoded_acceleration_limit); + } + else + { + RCLCPP_INFO_STREAM(rclcpp::get_logger("logger"), "Accounting hardcoded limit, longitudinal acceleration limit used: " << hardcoded_acceleration_limit); + } + _lon_accel_limiter = LongitudinalAccelLimiter{ - std::min(longitudinal_accel_limit_, MAX_LONGITUDINAL_ACCEL_HARDCODED_LIMIT_M_S_2)}; - + std::min(longitudinal_accel_limit_, hardcoded_acceleration_limit)}; + get_parameter("vehicle_lateral_accel_limit", lateral_accel_limit_); get_parameter("vehicle_lateral_jerk_limit", lateral_jerk_limit_); get_parameter("lowpass_gain_linear_x", lowpass_gain_linear_x_); get_parameter("lowpass_gain_angular_x", lowpass_gain_angular_z_); get_parameter("lowpass_gain_steering_angle", lowpass_gain_steering_angle_); - + //Setup subscribers twist_sub_ = create_subscription("twist_raw", 1, std::bind(&TwistFilter::TwistCmdCallback, this, std_ph::_1)); @@ -73,7 +90,7 @@ carma_ros2_utils::CallbackReturn TwistFilter::handle_on_configure(const rclcpp_l //Setup publishers twist_pub_ = create_publisher("twist_cmd", 5); ctrl_pub_ = create_publisher("ctrl_cmd", 5); - + // Publishers on private handles twist_lacc_limit_debug_pub_ = create_publisher("~/limitation_debug/twist/lateral_accel", 5); twist_ljerk_limit_debug_pub_ = create_publisher("~/limitation_debug/twist/lateral_jerk", 5); @@ -83,7 +100,7 @@ carma_ros2_utils::CallbackReturn TwistFilter::handle_on_configure(const rclcpp_l twist_ljerk_result_pub_ = create_publisher("~/result/twist/lateral_jerk", 5); ctrl_lacc_result_pub_ = create_publisher("~/result/ctrl/lateral_accel", 5); ctrl_ljerk_result_pub_ = create_publisher("~/result/ctrl/lateral_jerk", 5); - + return CallbackReturn::SUCCESS;