diff --git a/control/trajectory_follower_nodes/design/longitudinal_controller-design.md b/control/trajectory_follower_nodes/design/longitudinal_controller-design.md index 3348257181ca9..6fbbe1d9c03ff 100644 --- a/control/trajectory_follower_nodes/design/longitudinal_controller-design.md +++ b/control/trajectory_follower_nodes/design/longitudinal_controller-design.md @@ -148,7 +148,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | Name | Type | Description | Default value | | :----------------------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| control_rate | double | control rate [Hz] | 30 | +| longitudinal_ctrl_period | double | longitudinal control period [s] | 0.03 | | delay_compensation_time | double | delay for longitudinal control [s] | 0.17 | | enable_smooth_stop | bool | flag to enable transition to STOPPING | true | | enable_overshoot_emergency | bool | flag to enable transition to EMERGENCY when the ego is over the stop line with a certain distance. See `emergency_state_overshoot_stop_dist`. | true | @@ -240,3 +240,4 @@ If the ego is still running, strong acceleration (`strong_stop_acc`) to stop rig ## Related issues - +- diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp index 8d6d8d7eaac55..9832f0dfd6e09 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp @@ -115,8 +115,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY }; ControlState m_control_state{ControlState::STOPPED}; - // timer callback - float64_t m_control_rate; + // control period + float64_t m_longitudinal_ctrl_period; // delay compensation float64_t m_delay_compensation_time; diff --git a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml index d1cdabadef158..129abd2680509 100644 --- a/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml +++ b/control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - control_rate: 30.0 + longitudinal_ctrl_period: 0.03 # control period [s] delay_compensation_time: 0.17 enable_smooth_stop: true diff --git a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp index 8cefe228eefaa..70930416f8412 100644 --- a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp +++ b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp @@ -39,7 +39,7 @@ LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_ using std::placeholders::_1; // parameters timer - m_control_rate = declare_parameter("control_rate"); + m_longitudinal_ctrl_period = declare_parameter("longitudinal_ctrl_period"); m_wheel_base = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo().wheel_base_m; @@ -184,7 +184,8 @@ LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_ // Timer { - const auto period_ns = rclcpp::Rate(m_control_rate).period(); + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(m_longitudinal_ctrl_period)); m_timer_control = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&LongitudinalController::callbackTimerControl, this)); } @@ -618,7 +619,7 @@ void LongitudinalController::publishCtrlCmd(const Motion & ctrl_cmd, float64_t c // store current velocity history m_vel_hist.push_back({this->now(), current_vel}); - while (m_vel_hist.size() > static_cast(m_control_rate * 0.5)) { + while (m_vel_hist.size() > static_cast(0.5 / m_longitudinal_ctrl_period)) { m_vel_hist.erase(m_vel_hist.begin()); } @@ -659,14 +660,14 @@ float64_t LongitudinalController::getDt() { float64_t dt; if (!m_prev_control_time) { - dt = 1.0 / m_control_rate; + dt = m_longitudinal_ctrl_period; m_prev_control_time = std::make_shared(this->now()); } else { dt = (this->now() - *m_prev_control_time).seconds(); *m_prev_control_time = this->now(); } - const float64_t max_dt = 1.0 / m_control_rate * 2.0; - const float64_t min_dt = 1.0 / m_control_rate * 0.5; + const float64_t max_dt = m_longitudinal_ctrl_period * 2.0; + const float64_t min_dt = m_longitudinal_ctrl_period * 0.5; return std::max(std::min(dt, max_dt), min_dt); } diff --git a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml index 2dde3cea50d6b..b3af131b2e778 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - control_rate: 30.0 + longitudinal_ctrl_period: 0.03 # control period [s] delay_compensation_time: 0.17 enable_smooth_stop: true diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 7359e2866b7eb..37c43a2ab392c 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -112,9 +112,7 @@ def launch_setup(context, *args, **kwargs): lon_controller_param, vehicle_info_param, { - "control_rate": LaunchConfiguration("control_rate"), "show_debug_info": LaunchConfiguration("show_debug_info"), - "enable_smooth_stop": LaunchConfiguration("enable_smooth_stop"), "enable_pub_debug": LaunchConfiguration("enable_pub_debug"), }, ], @@ -354,11 +352,7 @@ def add_launch_arg(name: str, default_value=None, description=None): ) # velocity controller - add_launch_arg("control_rate", "30.0", "control rate") add_launch_arg("show_debug_info", "false", "show debug information") - add_launch_arg( - "enable_smooth_stop", "true", "enable smooth stop (in velocity controller state)" - ) add_launch_arg("enable_pub_debug", "true", "enable to publish debug information") # vehicle cmd gate