feat(vehicle cmd gate): add transition filter (autowarefoundation#1244)

* feat(vehicle_cmd_gate): add transition filter

Signed-off-by: Takamasa Horibe <[email protected]>

* fix precommit

Signed-off-by: Takamasa Horibe <[email protected]>

* remove debug code

Signed-off-by: Takamasa Horibe <[email protected]>

* update param yaml

Signed-off-by: Takamasa Horibe <[email protected]>

* update readme

Signed-off-by: Takamasa Horibe <[email protected]>

* fix default topic name

Signed-off-by: Takamasa Horibe <[email protected]>

control/vehicle_cmd_gate/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.14)
+cmake_minimum_required(VERSION 3.5)
 project(vehicle_cmd_gate)
 
 find_package(autoware_cmake REQUIRED)

control/vehicle_cmd_gate/README.md

@@ -26,6 +26,7 @@
 | `~/input/emergency/hazard_lights_cmd`       | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand`     | hazard lights command from emergency handler                         |
 | `~/input/emergency/gear_cmd`                | `autoware_auto_vehicle_msgs::msg::GearCommand`             | gear command from emergency handler                                  |
 | `~/input/engage`                            | `autoware_auto_vehicle_msgs::msg::Engage`                  | engage signal                                                        |
+| `~/input/operation_mode`                    | `tier4_system_msgs::msg::OperationMode`                    | operation mode of Autoware                                           |
 
 ### Output
 
@@ -39,23 +40,29 @@
 | `~/output/gate_mode`                   | `tier4_control_msgs::msg::GateMode`                        | gate mode (AUTO or EXTERNAL)                             |
 | `~/output/engage`                      | `autoware_auto_vehicle_msgs::msg::Engage`                  | engage signal                                            |
 | `~/output/external_emergency`          | `tier4_external_api_msgs::msg::Emergency`                  | external emergency signal                                |
+| `~/output/operation_mode`              | `tier4_system_msgs::msg::OperationMode`                    | current operation mode of the vehicle_cmd_gate           |
 
 ## Parameters
 
-| Parameter                                   | Type   | Description                                                    |
-| ------------------------------------------- | ------ | -------------------------------------------------------------- |
-| `update_period`                             | double | update period                                                  |
-| `use_emergency_handling`                    | bool   | true when emergency handler is used                            |
-| `use_external_emergency_stop`               | bool   | true when external emergency stop information is used          |
-| `system_emergency_heartbeat_timeout`        | double | timeout for system emergency                                   |
-| `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency                                 |
-| `stop_hold_acceleration`                    | double | longitudinal acceleration cmd when vehicle should stop         |
-| `emergency_acceleration`                    | double | longitudinal acceleration cmd when vehicle stop with emergency |
-| `vel_lim`                                   | double | limit of longitudinal velocity                                 |
-| `lon_acc_lim`                               | double | limit of longitudinal acceleration                             |
-| `lon_jerk_lim`                              | double | limit of longitudinal jerk                                     |
-| `lat_acc_lim`                               | double | limit of lateral acceleration                                  |
-| `lat_jerk_lim`                              | double | limit of lateral jerk                                          |
+| Parameter                                   | Type   | Description                                                                 |
+| ------------------------------------------- | ------ | --------------------------------------------------------------------------- |
+| `update_period`                             | double | update period                                                               |
+| `use_emergency_handling`                    | bool   | true when emergency handler is used                                         |
+| `use_external_emergency_stop`               | bool   | true when external emergency stop information is used                       |
+| `system_emergency_heartbeat_timeout`        | double | timeout for system emergency                                                |
+| `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency                                              |
+| `stop_hold_acceleration`                    | double | longitudinal acceleration cmd when vehicle should stop                      |
+| `emergency_acceleration`                    | double | longitudinal acceleration cmd when vehicle stop with emergency              |
+| `nominal.vel_lim`                           | double | limit of longitudinal velocity (activated in AUTONOMOUS operation mode)     |
+| `nominal.lon_acc_lim`                       | double | limit of longitudinal acceleration (activated in AUTONOMOUS operation mode) |
+| `nominal.lon_jerk_lim`                      | double | limit of longitudinal jerk (activated in AUTONOMOUS operation mode)         |
+| `nominal.lat_acc_lim`                       | double | limit of lateral acceleration (activated in AUTONOMOUS operation mode)      |
+| `nominal.lat_jerk_lim`                      | double | limit of lateral jerk (activated in AUTONOMOUS operation mode)              |
+| `on_transition.vel_lim`                     | double | limit of longitudinal velocity (activated in TRANSITION operation mode)     |
+| `on_transition.lon_acc_lim`                 | double | limit of longitudinal acceleration (activated in TRANSITION operation mode) |
+| `on_transition.lon_jerk_lim`                | double | limit of longitudinal jerk (activated in TRANSITION operation mode)         |
+| `on_transition.lat_acc_lim`                 | double | limit of lateral acceleration (activated in TRANSITION operation mode)      |
+| `on_transition.lat_jerk_lim`                | double | limit of lateral jerk (activated in TRANSITION operation mode)              |
 
 ## Assumptions / Known limits
 

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -6,8 +6,15 @@
     stop_hold_acceleration: -1.5
     emergency_acceleration: -2.4
     stopped_state_entry_duration_time: 0.1
-    vel_lim: 25.0
-    lon_acc_lim: 5.0
-    lon_jerk_lim: 5.0
-    lat_acc_lim: 5.0
-    lat_jerk_lim: 5.0
+    nominal:
+      vel_lim: 25.0
+      lon_acc_lim: 5.0
+      lon_jerk_lim: 5.0
+      lat_acc_lim: 5.0
+      lat_jerk_lim: 5.0
+    on_transition:
+      vel_lim: 50.0
+      lon_acc_lim: 1.0
+      lon_jerk_lim: 0.5
+      lat_acc_lim: 1.2
+      lat_jerk_lim: 0.75

control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp

@@ -15,20 +15,34 @@
 #ifndef VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_
 #define VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_
 
+#include <rclcpp/rclcpp.hpp>
+
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
 
+struct VehicleCmdFilterParam
+{
+  double wheel_base;
+  double vel_lim;
+  double lon_acc_lim;
+  double lon_jerk_lim;
+  double lat_acc_lim;
+  double lat_jerk_lim;
+  double actual_steer_diff_lim;
+};
 class VehicleCmdFilter
 {
 public:
   VehicleCmdFilter();
   ~VehicleCmdFilter() = default;
 
-  void setWheelBase(double v) { wheel_base_ = v; }
-  void setVelLim(double v) { vel_lim_ = v; }
-  void setLonAccLim(double v) { lon_acc_lim_ = v; }
-  void setLonJerkLim(double v) { lon_jerk_lim_ = v; }
-  void setLatAccLim(double v) { lat_acc_lim_ = v; }
-  void setLatJerkLim(double v) { lat_jerk_lim_ = v; }
+  void setWheelBase(double v) { param_.wheel_base = v; }
+  void setVelLim(double v) { param_.vel_lim = v; }
+  void setLonAccLim(double v) { param_.lon_acc_lim = v; }
+  void setLonJerkLim(double v) { param_.lon_jerk_lim = v; }
+  void setLatAccLim(double v) { param_.lat_acc_lim = v; }
+  void setLatJerkLim(double v) { param_.lat_jerk_lim = v; }
+  void setActualSteerDiffLim(double v) { param_.actual_steer_diff_lim = v; }
+  void setParam(const VehicleCmdFilterParam & p) { param_ = p; }
   void setPrevCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand & v)
   {
     prev_cmd_ = v;
@@ -44,14 +58,15 @@ class VehicleCmdFilter
     const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const;
   void limitLateralWithLatJerk(
     const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const;
+  void limitActualSteerDiff(
+    const double current_steer_angle,
+    autoware_auto_control_msgs::msg::AckermannControlCommand & input) const;
+  void filterAll(
+    const double dt, const double current_steer_angle,
+    autoware_auto_control_msgs::msg::AckermannControlCommand & input) const;
 
 private:
-  double wheel_base_;
-  double vel_lim_;
-  double lon_acc_lim_;
-  double lon_jerk_lim_;
-  double lat_acc_lim_;
-  double lat_jerk_lim_;
+  VehicleCmdFilterParam param_;
   autoware_auto_control_msgs::msg::AckermannControlCommand prev_cmd_;
 
   double calcLatAcc(const autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const;

control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp

@@ -36,6 +36,7 @@
 #include <tier4_external_api_msgs/msg/heartbeat.hpp>
 #include <tier4_external_api_msgs/srv/engage.hpp>
 #include <tier4_external_api_msgs/srv/set_emergency.hpp>
+#include <tier4_system_msgs/msg/operation_mode.hpp>
 #include <tier4_vehicle_msgs/msg/vehicle_emergency_stamped.hpp>
 
 #include <memory>
@@ -53,6 +54,7 @@ using tier4_control_msgs::msg::GateMode;
 using tier4_external_api_msgs::msg::Emergency;
 using tier4_external_api_msgs::msg::Heartbeat;
 using tier4_external_api_msgs::srv::SetEmergency;
+using tier4_system_msgs::msg::OperationMode;
 using tier4_vehicle_msgs::msg::VehicleEmergencyStamped;
 
 using diagnostic_msgs::msg::DiagnosticStatus;
@@ -87,12 +89,14 @@ class VehicleCmdGate : public rclcpp::Node
   rclcpp::Publisher<HazardLightsCommand>::SharedPtr hazard_light_cmd_pub_;
   rclcpp::Publisher<GateMode>::SharedPtr gate_mode_pub_;
   rclcpp::Publisher<EngageMsg>::SharedPtr engage_pub_;
+  rclcpp::Publisher<OperationMode>::SharedPtr operation_mode_pub_;
 
   // Subscription
   rclcpp::Subscription<EmergencyState>::SharedPtr emergency_state_sub_;
   rclcpp::Subscription<Heartbeat>::SharedPtr external_emergency_stop_heartbeat_sub_;
   rclcpp::Subscription<GateMode>::SharedPtr gate_mode_sub_;
   rclcpp::Subscription<SteeringReport>::SharedPtr steer_sub_;
+  rclcpp::Subscription<OperationMode>::SharedPtr operation_mode_sub_;
 
   void onGateMode(GateMode::ConstSharedPtr msg);
   void onEmergencyState(EmergencyState::ConstSharedPtr msg);
@@ -208,6 +212,10 @@ class VehicleCmdGate : public rclcpp::Node
   VehicleCmdFilter filter_;
   AckermannControlCommand filterControlCommand(const AckermannControlCommand & msg);
 
+  // filtering on transition
+  OperationMode current_operation_mode_;
+  VehicleCmdFilter filter_on_transition_;
+
   // Start request service
   struct StartRequest
   {

control/vehicle_cmd_gate/package.xml

@@ -23,6 +23,7 @@
   <depend>tier4_control_msgs</depend>
   <depend>tier4_debug_msgs</depend>
   <depend>tier4_external_api_msgs</depend>
+  <depend>tier4_system_msgs</depend>
   <depend>tier4_vehicle_msgs</depend>
   <depend>vehicle_info_util</depend>
 

control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp

@@ -21,35 +21,36 @@ VehicleCmdFilter::VehicleCmdFilter() {}
 void VehicleCmdFilter::limitLongitudinalWithVel(
   autoware_auto_control_msgs::msg::AckermannControlCommand & input) const
 {
-  input.longitudinal.speed =
-    std::max(std::min(static_cast<double>(input.longitudinal.speed), vel_lim_), -vel_lim_);
+  input.longitudinal.speed = std::max(
+    std::min(static_cast<double>(input.longitudinal.speed), param_.vel_lim), -param_.vel_lim);
 }
 
 void VehicleCmdFilter::limitLongitudinalWithAcc(
   const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const
 {
   input.longitudinal.acceleration = std::max(
-    std::min(static_cast<double>(input.longitudinal.acceleration), lon_acc_lim_), -lon_acc_lim_);
+    std::min(static_cast<double>(input.longitudinal.acceleration), param_.lon_acc_lim),
+    -param_.lon_acc_lim);
   input.longitudinal.speed =
-    limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, lon_acc_lim_ * dt);
+    limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, param_.lon_acc_lim * dt);
 }
 
 void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk(
   const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const
 {
   input.longitudinal.acceleration = limitDiff(
-    input.longitudinal.acceleration, prev_cmd_.longitudinal.acceleration, lon_jerk_lim_ * dt);
+    input.longitudinal.acceleration, prev_cmd_.longitudinal.acceleration, param_.lon_jerk_lim * dt);
 }
 
 void VehicleCmdFilter::limitLateralWithLatAcc(
   [[maybe_unused]] const double dt,
   autoware_auto_control_msgs::msg::AckermannControlCommand & input) const
 {
   double latacc = calcLatAcc(input);
-  if (std::fabs(latacc) > lat_acc_lim_) {
+  if (std::fabs(latacc) > param_.lat_acc_lim) {
     double v_sq =
       std::max(static_cast<double>(input.longitudinal.speed * input.longitudinal.speed), 0.001);
-    double steer_lim = std::atan(lat_acc_lim_ * wheel_base_ / v_sq);
+    double steer_lim = std::atan(param_.lat_acc_lim * param_.wheel_base / v_sq);
     input.lateral.steering_tire_angle = latacc > 0.0 ? steer_lim : -steer_lim;
   }
 }
@@ -60,8 +61,8 @@ void VehicleCmdFilter::limitLateralWithLatJerk(
   double curr_latacc = calcLatAcc(input);
   double prev_latacc = calcLatAcc(prev_cmd_);
 
-  const double latacc_max = prev_latacc + lat_jerk_lim_ * dt;
-  const double latacc_min = prev_latacc - lat_jerk_lim_ * dt;
+  const double latacc_max = prev_latacc + param_.lat_jerk_lim * dt;
+  const double latacc_min = prev_latacc - param_.lat_jerk_lim * dt;
 
   if (curr_latacc > latacc_max) {
     input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_max);
@@ -70,17 +71,39 @@ void VehicleCmdFilter::limitLateralWithLatJerk(
   }
 }
 
+void VehicleCmdFilter::limitActualSteerDiff(
+  const double current_steer_angle,
+  autoware_auto_control_msgs::msg::AckermannControlCommand & input) const
+{
+  auto ds = input.lateral.steering_tire_angle - current_steer_angle;
+  ds = std::clamp(ds, -param_.actual_steer_diff_lim, param_.actual_steer_diff_lim);
+  input.lateral.steering_tire_angle = current_steer_angle + ds;
+}
+
+void VehicleCmdFilter::filterAll(
+  const double dt, const double current_steer_angle,
+  autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const
+{
+  limitLongitudinalWithJerk(dt, cmd);
+  limitLongitudinalWithAcc(dt, cmd);
+  limitLongitudinalWithVel(cmd);
+  limitLateralWithLatJerk(dt, cmd);
+  limitLateralWithLatAcc(dt, cmd);
+  limitActualSteerDiff(current_steer_angle, cmd);
+  return;
+}
+
 double VehicleCmdFilter::calcSteerFromLatacc(const double v, const double latacc) const
 {
   const double v_sq = std::max(v * v, 0.001);
-  return std::atan(latacc * wheel_base_ / v_sq);
+  return std::atan(latacc * param_.wheel_base / v_sq);
 }
 
 double VehicleCmdFilter::calcLatAcc(
   const autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const
 {
   double v = cmd.longitudinal.speed;
-  return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheel_base_;
+  return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base;
 }
 
 double VehicleCmdFilter::limitDiff(

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -66,6 +66,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
   engage_pub_ = this->create_publisher<EngageMsg>("output/engage", durable_qos);
   pub_external_emergency_ =
     this->create_publisher<Emergency>("output/external_emergency", durable_qos);
+  operation_mode_pub_ = this->create_publisher<OperationMode>("output/operation_mode", durable_qos);
 
   // Subscriber
   emergency_state_sub_ = this->create_subscription<EmergencyState>(
@@ -79,6 +80,10 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     "input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1));
   steer_sub_ = this->create_subscription<SteeringReport>(
     "input/steering", 1, std::bind(&VehicleCmdGate::onSteering, this, _1));
+  operation_mode_sub_ = this->create_subscription<tier4_system_msgs::msg::OperationMode>(
+    "input/operation_mode", 1, [this](const tier4_system_msgs::msg::OperationMode::SharedPtr msg) {
+      current_operation_mode_ = *msg;
+    });
 
   // Subscriber for auto
   auto_control_cmd_sub_ = this->create_subscription<AckermannControlCommand>(
@@ -133,18 +138,29 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
 
   // Vehicle Parameter
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
-  double wheel_base = vehicle_info.wheel_base_m;
-  double vel_lim = declare_parameter("vel_lim", 25.0);
-  double lon_acc_lim = declare_parameter("lon_acc_lim", 5.0);
-  double lon_jerk_lim = declare_parameter("lon_jerk_lim", 5.0);
-  double lat_acc_lim = declare_parameter("lat_acc_lim", 5.0);
-  double lat_jerk_lim = declare_parameter("lat_jerk_lim", 5.0);
-  filter_.setWheelBase(wheel_base);
-  filter_.setVelLim(vel_lim);
-  filter_.setLonAccLim(lon_acc_lim);
-  filter_.setLonJerkLim(lon_jerk_lim);
-  filter_.setLatAccLim(lat_acc_lim);
-  filter_.setLatJerkLim(lat_jerk_lim);
+  {
+    VehicleCmdFilterParam p;
+    p.wheel_base = vehicle_info.wheel_base_m;
+    p.vel_lim = declare_parameter("nominal.vel_lim", 25.0);
+    p.lon_acc_lim = declare_parameter("nominal.lon_acc_lim", 5.0);
+    p.lon_jerk_lim = declare_parameter("nominal.lon_jerk_lim", 5.0);
+    p.lat_acc_lim = declare_parameter("nominal.lat_acc_lim", 5.0);
+    p.lat_jerk_lim = declare_parameter("nominal.lat_jerk_lim", 5.0);
+    p.actual_steer_diff_lim = declare_parameter("nominal.actual_steer_diff_lim", 1.0);
+    filter_.setParam(p);
+  }
+
+  {
+    VehicleCmdFilterParam p;
+    p.wheel_base = vehicle_info.wheel_base_m;
+    p.vel_lim = declare_parameter("on_transition.vel_lim", 25.0);
+    p.lon_acc_lim = declare_parameter("on_transition.lon_acc_lim", 0.5);
+    p.lon_jerk_lim = declare_parameter("on_transition.lon_jerk_lim", 0.25);
+    p.lat_acc_lim = declare_parameter("on_transition.lat_acc_lim", 0.5);
+    p.lat_jerk_lim = declare_parameter("on_transition.lat_jerk_lim", 0.25);
+    p.actual_steer_diff_lim = declare_parameter("on_transition.actual_steer_diff_lim", 0.05);
+    filter_on_transition_.setParam(p);
+  }
 
   // Set default value
   current_gate_mode_.data = GateMode::AUTO;
@@ -485,19 +501,28 @@ void VehicleCmdGate::publishStatus()
   gate_mode_pub_->publish(current_gate_mode_);
   engage_pub_->publish(autoware_engage);
   pub_external_emergency_->publish(external_emergency);
+  operation_mode_pub_->publish(current_operation_mode_);
 }
 
 AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannControlCommand & in)
 {
   AckermannControlCommand out = in;
   const double dt = getDt();
 
-  filter_.limitLongitudinalWithVel(out);
-  filter_.limitLongitudinalWithAcc(dt, out);
-  filter_.limitLongitudinalWithJerk(dt, out);
-  filter_.limitLateralWithLatAcc(dt, out);
-  filter_.limitLateralWithLatJerk(dt, out);
+  const auto mode = current_operation_mode_.mode;
+
+  // Apply transition_filter when transiting from MANUAL to AUTO.
+  if (mode == OperationMode::TRANSITION_TO_AUTO) {
+    filter_on_transition_.filterAll(dt, current_steer_, out);
+  } else {
+    filter_.filterAll(dt, current_steer_, out);
+  }
+
+  // set prev value for both to keep consistency over switching
+  // TODO(Horibe): prev value should be actual steer, vel, acc when Manual mode to keep
+  // consistency when switching from Manual to Auto.
   filter_.setPrevCmd(out);
+  filter_on_transition_.setPrevCmd(out);
 
   return out;
 }