Increase hardcoded acceleration limit for simulation (#265)

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 0.9.10 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

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,31 +26,31 @@ class LongitudinalAccelLimiter
 
         /**
          * \brief Constructor for LongitudinalAccelLimiter
-         * 
+         *
          * \param accel_limit The acceleration limit that should be enforced, in
          * units of m/s/s
          */
         LongitudinalAccelLimiter(double accel_limit):
             _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
             longitudinalAccelLimitCtrl(const autoware_msgs::msg::ControlCommandStamped& msg);
 
                 /**
          * 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;
 };
 
 }

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<std_msgs::msg::Float32> ctrl_lacc_limit_debug_pub_, ctrl_ljerk_limit_debug_pub_;
     carma_ros2_utils::PubPtr<std_msgs::msg::Float32> twist_lacc_result_pub_, twist_ljerk_result_pub_;
     carma_ros2_utils::PubPtr<std_msgs::msg::Float32> ctrl_lacc_result_pub_, ctrl_ljerk_result_pub_;
-    
+
     //subscribers
     carma_ros2_utils::SubPtr<geometry_msgs::msg::TwistStamped> twist_sub_;
     carma_ros2_utils::SubPtr<autoware_msgs::msg::ControlCommandStamped> ctrl_sub_;
     carma_ros2_utils::SubPtr<autoware_config_msgs::msg::ConfigTwistFilter> config_sub_;
-    
+
     //ros params
     double wheel_base_;
     double longitudinal_velocity_limit_;

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<double>("config_speed_limit", longitudinal_velocity_limit_);
   longitudinal_velocity_limit_ = longitudinal_velocity_limit_ * 0.44704;
-  
+
   get_parameter<double>("vehicle_acceleration_limit", longitudinal_accel_limit_);
+
+  bool use_sim_time = false;
+  get_parameter<bool>("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(get_logger(), "Simulation mode detected. Accounting for hardcoded limit, longitudinal acceleration limit used: " << hardcoded_acceleration_limit);
+  }
+  else
+  {
+    RCLCPP_INFO_STREAM(get_logger(), "Accounting for 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<double>("vehicle_lateral_accel_limit", lateral_accel_limit_);
   get_parameter<double>("vehicle_lateral_jerk_limit", lateral_jerk_limit_);
   get_parameter<double>("lowpass_gain_linear_x", lowpass_gain_linear_x_);
   get_parameter<double>("lowpass_gain_angular_x", lowpass_gain_angular_z_);
   get_parameter<double>("lowpass_gain_steering_angle", lowpass_gain_steering_angle_);
-  
+
 
   //Setup subscribers
   twist_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>("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<geometry_msgs::msg::TwistStamped>("twist_cmd", 5);
   ctrl_pub_ = create_publisher<autoware_msgs::msg::ControlCommandStamped>("ctrl_cmd", 5);
-  
+
   // Publishers on private handles
   twist_lacc_limit_debug_pub_ = create_publisher<std_msgs::msg::Float32>("~/limitation_debug/twist/lateral_accel", 5);
   twist_ljerk_limit_debug_pub_ = create_publisher<std_msgs::msg::Float32>("~/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<std_msgs::msg::Float32>("~/result/twist/lateral_jerk", 5);
   ctrl_lacc_result_pub_ = create_publisher<std_msgs::msg::Float32>("~/result/ctrl/lateral_accel", 5);
   ctrl_ljerk_result_pub_ = create_publisher<std_msgs::msg::Float32>("~/result/ctrl/lateral_jerk", 5);
-  
+
 
   return CallbackReturn::SUCCESS;
 
@@ -413,4 +430,4 @@ void TwistFilter::CtrlCmdCallback(
 #include "rclcpp_components/register_node_macro.hpp"
 
 // Register the component with class_loader
-RCLCPP_COMPONENTS_REGISTER_NODE(twist_filter::TwistFilter)
+RCLCPP_COMPONENTS_REGISTER_NODE(twist_filter::TwistFilter)