Merge pull request autowarefoundation#637 from maxime-clem/hotfix/red…

…uce_delays

feat(vehicle_cmd_gate): reduce delay

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -9,6 +9,7 @@
     stop_hold_acceleration: -1.5
     emergency_acceleration: -2.4
     stopped_state_entry_duration_time: 0.1
+    stop_check_duration: 1.0
     nominal:
       vel_lim: 25.0
       lon_acc_lim: 5.0

control/vehicle_cmd_gate/package.xml

@@ -21,6 +21,7 @@
   <depend>component_interface_utils</depend>
   <depend>diagnostic_updater</depend>
   <depend>geometry_msgs</depend>
+  <depend>motion_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>std_srvs</depend>

control/vehicle_cmd_gate/script/delays_checker.py

@@ -0,0 +1,142 @@
+#!/usr/bin/env python3
+
+# Copyright 2023 TIER IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from autoware_auto_control_msgs.msg import AckermannControlCommand
+from autoware_auto_vehicle_msgs.msg import Engage
+from geometry_msgs.msg import AccelWithCovarianceStamped
+from nav_msgs.msg import Odometry
+import rclpy
+from rclpy.node import Node
+
+accel_topic = "/localization/acceleration"
+odom_topic = "/localization/kinematic_state"
+in_gate_cmd_topic = "/control/trajectory_follower/control_cmd"
+out_gate_cmd_topic = "/control/command/control_cmd"
+engage_topic = "/autoware/engage"
+
+
+# This node measure the delays between when we want ego to move, and when it actually starts moving.
+# There are 2 cases measured:
+# 1) After engaging
+#   Before autoware is engaged, assuming a valid and non stopping trajectory is generated
+#   by the planning module,
+#   then the controller should output a control command with positive speed and acceleration.
+#   In that case, the delay measured is the duration between autoware being engaged,
+#   and the vehicle starting to move (based on the measured velocity).
+# 2) Restart
+#   When already engaged, the ego vehicle may stop and restart multiple times.
+#   (e.g., when stopping at a red light). In that case,
+#   the trajectory is stopping and the control command has a non-positive velocity and acceleration.
+#   In that case, the delay measured is the duration between the controller's command switching to
+#   a positive velocity and acceleration, and the ego vehicle actually starting to move.
+class DelaysChecker(Node):
+    def __init__(self):
+        super().__init__("delays_checker")
+
+        self.autoware_engage = None
+        self.ego_is_stopped = True
+        self.prev_in_cmd = None
+        self.last_engage_time = None
+        self.last_start_time = None
+        self.current_accel = 0.0
+
+        # planning path and trajectories
+        self.sub_accel = self.create_subscription(
+            AccelWithCovarianceStamped,
+            accel_topic,
+            self.CallBackAccel,
+            1,
+        )
+        self.sub_odom = self.create_subscription(
+            Odometry,
+            odom_topic,
+            self.CallBackOdom,
+            1,
+        )
+        self.sub_engage = self.create_subscription(Engage, engage_topic, self.CallBackEngage, 1)
+        self.sub_in_gate_cmd = self.create_subscription(
+            AckermannControlCommand,
+            in_gate_cmd_topic,
+            self.CallBackInCmd,
+            1,
+        )
+        self.sub_out_gate_cmd = self.create_subscription(
+            AckermannControlCommand,
+            out_gate_cmd_topic,
+            self.CallBackOutCmd,
+            1,
+        )
+
+    def CallBackEngage(self, msg):
+        if not self.autoware_engage or msg.engage != self.autoware_engage:
+            self.autoware_engage = msg.engage
+            if self.autoware_engage == 1:
+                self.last_engage_time = self.get_clock().now()
+
+    def CallBackInCmd(self, msg):
+        is_start = (
+            self.prev_in_cmd
+            and self.ego_is_stopped
+            and self.prev_in_cmd.longitudinal.acceleration < 0.0
+            and msg.longitudinal.acceleration > 0.0
+        )
+        if is_start:
+            self.last_start_time = self.get_clock().now()
+        self.prev_in_cmd = msg
+
+    def CallBackOutCmd(self, msg):
+        None
+
+    def CallBackAccel(self, msg):
+        self.current_accel = msg.accel.accel.linear.x
+
+    def CallBackOdom(self, msg):
+        if msg.twist.twist.linear.x < 1e-6:
+            if not self.ego_is_stopped:
+                self.last_engage_time = None
+            self.ego_is_stopped = True
+        elif self.ego_is_stopped:
+            # Ego starts to move
+            now = self.get_clock().now()
+            delay_ms = (
+                (now - self.last_engage_time).nanoseconds * 1e-6
+                if self.last_engage_time
+                else (
+                    (now - self.last_start_time).nanoseconds * 1e-6 if self.last_start_time else 0.0
+                )
+            )
+            self.get_logger().info(
+                "Move delay {}ms ({})".format(
+                    delay_ms, ("after engage" if self.last_engage_time else "restart")
+                )
+            )
+            self.ego_is_stopped = False
+
+
+def main(args=None):
+    try:
+        rclpy.init(args=args)
+        node = DelaysChecker()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -51,6 +51,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
   rclcpp::QoS durable_qos{1};
   durable_qos.transient_local();
 
+  // Stop Checker
+  vehicle_stop_checker_ = std::make_unique<VehicleStopChecker>(this);
+
   // Publisher
   vehicle_cmd_emergency_pub_ =
     create_publisher<VehicleEmergencyStamped>("output/vehicle_cmd_emergency", durable_qos);
@@ -143,6 +146,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     declare_parameter<double>("external_emergency_stop_heartbeat_timeout");
   stop_hold_acceleration_ = declare_parameter<double>("stop_hold_acceleration");
   emergency_acceleration_ = declare_parameter<double>("emergency_acceleration");
+  stop_check_duration_ = declare_parameter<double>("stop_check_duration");
 
   // Vehicle Parameter
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -409,6 +413,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
   // Publish commands
   vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
   control_cmd_pub_->publish(filtered_commands.control);
+  pause_->publish();
 
   // Save ControlCmd to steering angle when disengaged
   prev_control_cmd_ = filtered_commands.control;
@@ -480,6 +485,9 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont
   AckermannControlCommand out = in;
   const double dt = getDt();
   const auto mode = current_operation_mode_;
+  const auto current_status_cmd = getActualStatusAsCommand();
+  const auto ego_is_stopped = vehicle_stop_checker_->isVehicleStopped(stop_check_duration_);
+  const auto input_cmd_is_stopping = in.longitudinal.acceleration < 0.0;
 
   // Apply transition_filter when transiting from MANUAL to AUTO.
   if (mode.is_in_transition) {
@@ -491,8 +499,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont
   // set prev value for both to keep consistency over switching:
   // Actual steer, vel, acc should be considered in manual mode to prevent sudden motion when
   // switching from manual to autonomous
-  auto prev_values =
-    (mode.mode == OperationModeState::AUTONOMOUS) ? out : getActualStatusAsCommand();
+  auto prev_values = (mode.mode == OperationModeState::AUTONOMOUS) ? out : current_status_cmd;
 
   // TODO(Horibe): To prevent sudden acceleration/deceleration when switching from manual to
   // autonomous, the filter should be applied for actual speed and acceleration during manual
@@ -504,6 +511,14 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont
   // filter in manual mode.
   prev_values.longitudinal = out.longitudinal;  // TODO(Horibe): to be removed
 
+  // When ego is stopped and the input command is stopping,
+  // use the actual vehicle longitudinal state for the next filtering
+  // this is to prevent the jerk limits being applied on the "stop acceleration"
+  // which may be negative and cause delays when restarting the vehicle.
+  if (ego_is_stopped && input_cmd_is_stopping) {
+    prev_values.longitudinal = current_status_cmd.longitudinal;
+  }
+
   filter_.setPrevCmd(prev_values);
   filter_on_transition_.setPrevCmd(prev_values);
 

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -19,6 +19,7 @@
 #include "vehicle_cmd_filter.hpp"
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
+#include <motion_utils/motion_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
@@ -69,6 +70,7 @@ using nav_msgs::msg::Odometry;
 using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage;
 using EngageSrv = tier4_external_api_msgs::srv::Engage;
 
+using motion_utils::VehicleStopChecker;
 struct Commands
 {
   AckermannControlCommand control;
@@ -216,6 +218,10 @@ class VehicleCmdGate : public rclcpp::Node
 
   // Pause interface for API
   std::unique_ptr<PauseInterface> pause_;
+
+  // stop checker
+  std::unique_ptr<VehicleStopChecker> vehicle_stop_checker_;
+  double stop_check_duration_;
 };
 
 }  // namespace vehicle_cmd_gate

system/default_ad_api/src/motion.cpp

@@ -105,6 +105,17 @@ void MotionNode::change_state(const State state)
     pub_state_->publish(msg);
   }
   state_ = state;
+  update_pause(state);
+}
+
+void MotionNode::update_pause(const State state)
+{
+  if (state == State::Pausing) {
+    return change_pause(true);
+  }
+  if (state == State::Resuming) {
+    return change_pause(false);
+  }
 }
 
 void MotionNode::change_pause(bool pause)
@@ -120,24 +131,19 @@ void MotionNode::change_pause(bool pause)
 void MotionNode::on_timer()
 {
   update_state();
-
-  if (state_ == State::Pausing) {
-    return change_pause(true);
-  }
-  if (state_ == State::Resuming) {
-    return change_pause(false);
-  }
 }
 
 void MotionNode::on_is_paused(const control_interface::IsPaused::Message::ConstSharedPtr msg)
 {
   is_paused_ = msg->data;
+  update_state();
 }
 
 void MotionNode::on_is_start_requested(
   const control_interface::IsStartRequested::Message::ConstSharedPtr msg)
 {
   is_start_requested_ = msg->data;
+  update_state();
 }
 
 void MotionNode::on_accept(

system/default_ad_api/src/motion.hpp

@@ -54,6 +54,7 @@ class MotionNode : public rclcpp::Node
 
   void update_state();
   void change_state(const State state);
+  void update_pause(const State state);
   void change_pause(bool pause);
   void on_timer();
   void on_is_paused(const control_interface::IsPaused::Message::ConstSharedPtr msg);