perf(trajectory_follower_nodes): change longitudinal control to use p…

…eriod parameter (tier4#763) * perf(trajectory_follower_nodes): change longitudinal control to use period parameter Signed-off-by: Shark Liu <shark.liu@autocore.ai> * perf(trajectory_follower_nodes): remove duplicate ros parameters in 'control.launch.py' Signed-off-by: Shark Liu <shark.liu@autocore.ai> * doc(trajectory_follower_nodes): update design doc according to code update Signed-off-by: Shark Liu <shark.liu@autocore.ai> * ci(pre-commit): autofix Co-authored-by: Shark Liu <shark.liu@autocore.ai> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
boyali · Oct 3, 2022 · 3475976 · 3475976
1 parent 870acdd
commit 3475976
Show file tree

Hide file tree

Showing 6 changed files with 13 additions and 17 deletions.
diff --git 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
 
 - <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1058>
+- <https://github.com/autowarefoundation/autoware.universe/issues/701>
diff --git a/...jectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp b/...jectory_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
@@ -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
@@ -39,7 +39,7 @@ LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_
   using std::placeholders::_1;
 
   // parameters timer
-  m_control_rate = declare_parameter<float64_t>("control_rate");
+  m_longitudinal_ctrl_period = declare_parameter<float64_t>("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::nanoseconds>(
+      std::chrono::duration<float64_t>(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<size_t>(m_control_rate * 0.5)) {
+  while (m_vel_hist.size() > static_cast<size_t>(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<rclcpp::Time>(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
@@ -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
@@ -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