feat: add vehicle cmd gate package

control/vehicle_cmd_gate/CMakeLists.txt

@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.5)
+project(vehicle_cmd_gate)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+### Dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# Generate exe file
+ament_auto_add_library(vehicle_cmd_gate_node SHARED
+  src/vehicle_cmd_gate.cpp
+  src/vehicle_cmd_filter.cpp
+)
+
+rclcpp_components_register_node(vehicle_cmd_gate_node
+  PLUGIN "VehicleCmdGate"
+  EXECUTABLE vehicle_cmd_gate
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(test_vehicle_cmd_gate
+    test/src/test_main.cpp
+    test/src/test_vehicle_cmd_filter.cpp
+  )
+  ament_target_dependencies(test_vehicle_cmd_gate
+    rclcpp
+    autoware_control_msgs
+  )
+  target_link_libraries(test_vehicle_cmd_gate
+    vehicle_cmd_gate_node
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+)

control/vehicle_cmd_gate/README.md

@@ -0,0 +1,62 @@
+# vehicle_cmd_gate
+
+## Purpose
+
+`vehicle_cmd_gate` is the package to get information from emergency handler, planning module, external controller, and send a msg to vehicle.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                                        | Type                                                       | Description                                                          |
+| ------------------------------------------- | ---------------------------------------------------------- | -------------------------------------------------------------------- |
+| `~/input/steering`                          | `autoware_auto_vehicle_msgs::msg::SteeringReport`          | steering status                                                      |
+| `~/input/auto/control_cmd`                  | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from planning module   |
+| `~/input/auto/turn_indicators_cmd`          | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand`   | turn indicators command from planning module                         |
+| `~/input/auto/hazard_lights_cmd`            | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand`     | hazard lights command from planning module                           |
+| `~/input/auto/gear_cmd`                     | `autoware_auto_vehicle_msgs::msg::GearCommand`             | gear command from planning module                                    |
+| `~/input/external/control_cmd`              | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from external          |
+| `~/input/external/turn_indicators_cmd`      | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand`   | turn indicators command from external                                |
+| `~/input/external/hazard_lights_cmd`        | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand`     | hazard lights command from external                                  |
+| `~/input/external/gear_cmd`                 | `autoware_auto_vehicle_msgs::msg::GearCommand`             | gear command from external                                           |
+| `~/input/external_emergency_stop_heartbeat` | `autoware_external_api_msgs::msg::Heartbeat`               | heartbeat                                                            |
+| `~/input/gate_mode`                         | `autoware_control_msgs::msg::GateMode`                     | gate mode (AUTO or EXTERNAL)                                         |
+| `~/input/emergency_state`                   | `autoware_auto_system_msgs::msg::EmergencyState`           | used to detect the emergency situation of the vehicle                |
+| `~/input/emergency/control_cmd`             | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from emergency handler |
+| `~/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                                                        |
+
+### Output
+
+| Name                                   | Type                                                       | Description                                              |
+| -------------------------------------- | ---------------------------------------------------------- | -------------------------------------------------------- |
+| `~/output/vehicle_cmd_emergency`       | `autoware_auto_system_msgs::msg::EmergencyState`           | emergency state which was originally in vehicle command  |
+| `~/output/command/control_cmd`         | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity to vehicle |
+| `~/output/command/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand`   | turn indicators command to vehicle                       |
+| `~/output/command/hazard_lights_cmd`   | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand`     | hazard lights command to vehicle                         |
+| `~/output/command/gear_cmd`            | `autoware_auto_vehicle_msgs::msg::GearCommand`             | gear command to vehicle                                  |
+| `~/output/gate_mode`                   | `autoware_control_msgs::msg::GateMode`                     | gate mode (AUTO or EXTERNAL)                             |
+| `~/output/engage`                      | `autoware_auto_vehicle_msgs::msg::Engage`                  | engage signal                                            |
+| `~/output/external_emergency`          | `autoware_external_api_msgs::msg::Emergency`               | external emergency signal                                |
+
+## 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                                          |
+
+## Assumptions / Known limits
+
+TBD.

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -0,0 +1,12 @@
+/**:
+  ros__parameters:
+    update_rate: 10.0
+    system_emergency_heartbeat_timeout: 0.5
+    external_emergency_stop_heartbeat_timeout: 0.0
+    stop_hold_acceleration: -1.5
+    emergency_acceleration: -2.4
+    vel_lim: 25.0
+    lon_acc_lim: 5.0
+    lon_jerk_lim: 5.0
+    lat_acc_lim: 5.0
+    lat_jerk_lim: 5.0

control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_filter.hpp

@@ -0,0 +1,62 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// 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.
+
+#ifndef VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_
+#define VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_
+
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+
+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 setPrevCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand & v)
+  {
+    prev_cmd_ = v;
+  }
+
+  void limitLongitudinalWithVel(
+    autoware_auto_control_msgs::msg::AckermannControlCommand & input) const;
+  void limitLongitudinalWithAcc(
+    const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const;
+  void limitLongitudinalWithJerk(
+    const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const;
+  void limitLateralWithLatAcc(
+    const double dt, autoware_auto_control_msgs::msg::AckermannControlCommand & input) const;
+  void limitLateralWithLatJerk(
+    const double dt, 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_;
+  autoware_auto_control_msgs::msg::AckermannControlCommand prev_cmd_;
+
+  double calcLatAcc(const autoware_auto_control_msgs::msg::AckermannControlCommand & cmd) const;
+  double calcSteerFromLatacc(const double v, const double latacc) const;
+  double limitDiff(const double curr, const double prev, const double diff_lim) const;
+};
+
+#endif  // VEHICLE_CMD_GATE__VEHICLE_CMD_FILTER_HPP_