feat(vehicle_cmd_gate): improve control command filter to prevent sudden acc and steer

control/vehicle_cmd_gate/CMakeLists.txt

@@ -13,13 +13,31 @@ ament_auto_add_library(vehicle_cmd_gate_node SHARED
 
 rclcpp_components_register_node(vehicle_cmd_gate_node
   PLUGIN "vehicle_cmd_gate::VehicleCmdGate"
-  EXECUTABLE vehicle_cmd_gate
+  EXECUTABLE vehicle_cmd_gate_exe
 )
 
+rosidl_generate_interfaces(
+  ${PROJECT_NAME}
+  "msg/IsFilterActivated.msg"
+  DEPENDENCIES builtin_interfaces
+)
+
+# to use same package defined message
+if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
+    rosidl_target_interfaces(vehicle_cmd_gate_node
+    ${PROJECT_NAME} "rosidl_typesupport_cpp")
+else()
+    rosidl_get_typesupport_target(
+            cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+    target_link_libraries(vehicle_cmd_gate_node "${cpp_typesupport_target}")
+endif()
+
+
 if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_vehicle_cmd_gate
     test/src/test_main.cpp
     test/src/test_vehicle_cmd_filter.cpp
+    test/src/test_filter_in_vehicle_cmd_gate_node.cpp
   )
   ament_target_dependencies(test_vehicle_cmd_gate
     rclcpp

control/vehicle_cmd_gate/README.md

@@ -43,26 +43,38 @@
 
 ## Parameters
 
-| Parameter                                   | Type   | Description                                                                 |
-| ------------------------------------------- | ------ | --------------------------------------------------------------------------- |
-| `update_period`                             | double | update period                                                               |
-| `use_emergency_handling`                    | bool   | true when emergency handler is used                                         |
-| `check_external_emergency_heartbeat`        | bool   | true when checking heartbeat for emergency stop                             |
-| `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              |
-| `moderate_stop_service_acceleration`        | double | longitudinal acceleration cmd when vehicle stop with moderate stop service  |
-| `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)              |
+| Parameter                                   | Type     | Description                                                                                                                                                                                 |
+| ------------------------------------------- | -------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `update_period`                             | double   | update period                                                                                                                                                                               |
+| `use_emergency_handling`                    | bool     | true when emergency handler is used                                                                                                                                                         |
+| `check_external_emergency_heartbeat`        | bool     | true when checking heartbeat for emergency stop                                                                                                                                             |
+| `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                                                                                                                              |
+| `moderate_stop_service_acceleration`        | double   | longitudinal acceleration cmd when vehicle stop with moderate stop service                                                                                                                  |
+| `nominal.vel_lim`                           | double   | limit of longitudinal velocity (activated in AUTONOMOUS operation mode)                                                                                                                     |
+| `nominal.reference_speed_point`             | <double> | velocity point used as a reference when calculate control command limit (activated in AUTONOMOUS operation mode). The size of this array must be equivalent to the size of the limit array. |
+| `nominal.lon_acc_lim`                       | <double> | array of limits of longitudinal acceleration (activated in AUTONOMOUS operation mode)                                                                                                       |
+| `nominal.lon_jerk_lim`                      | <double> | array of limits of longitudinal jerk (activated in AUTONOMOUS operation mode)                                                                                                               |
+| `nominal.lat_acc_lim`                       | <double> | array of limits of lateral acceleration (activated in AUTONOMOUS operation mode)                                                                                                            |
+| `nominal.lat_jerk_lim`                      | <double> | array of limits of lateral jerk (activated in AUTONOMOUS operation mode)                                                                                                                    |
+| `on_transition.vel_lim`                     | double   | limit of longitudinal velocity (activated in TRANSITION operation mode)                                                                                                                     |
+| `on_transition.reference_speed_point`       | <double> | velocity point used as a reference when calculate control command limit (activated in TRANSITION operation mode). The size of this array must be equivalent to the size of the limit array. |
+| `on_transition.lon_acc_lim`                 | <double> | array of limits of longitudinal acceleration (activated in TRANSITION operation mode)                                                                                                       |
+| `on_transition.lon_jerk_lim`                | <double> | array of limits of longitudinal jerk (activated in TRANSITION operation mode)                                                                                                               |
+| `on_transition.lat_acc_lim`                 | <double> | array of limits of lateral acceleration (activated in TRANSITION operation mode)                                                                                                            |
+| `on_transition.lat_jerk_lim`                | <double> | array of limits of lateral jerk (activated in TRANSITION operation mode)                                                                                                                    |
+
+## Filter function
+
+This module incorporates a limitation filter to the control command right before its published. Primarily for safety, this filter restricts the output range of all control commands published through Autoware.
+
+The limitation values are calculated based on the 1D interpolation of the limitation array parameters. Here is an example for the longitudinal jerk limit.
+
+![filter-example](./image/filter.png)
+
+Notation: this filter is not designed to enhance ride comfort. Its main purpose is to detect and remove abnormal values in the control outputs during the final stages of Autoware. If this filter is frequently active, it implies the control module may need tuning. If you're aiming to smoothen the signal via a low-pass filter or similar techniques, that should be handled in the control module. When the filter is activated, the topic `~/is_filter_activated` is published.
 
 ## Assumptions / Known limits
 

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -13,15 +13,21 @@
     stop_check_duration: 1.0
     nominal:
       vel_lim: 25.0
-      lon_acc_lim: 5.0
-      lon_jerk_lim: 5.0
-      lat_acc_lim: 5.0
-      lat_jerk_lim: 7.0
-      actual_steer_diff_lim: 1.0
+      reference_speed_points: [20.0, 30.0]
+      steer_lim: [1.0, 0.8]
+      steer_rate_lim: [1.0, 0.8]
+      lon_acc_lim: [5.0, 4.0]
+      lon_jerk_lim: [5.0, 4.0]
+      lat_acc_lim: [5.0, 4.0]
+      lat_jerk_lim: [7.0, 6.0]
+      actual_steer_diff_lim: [1.0, 0.8]
     on_transition:
       vel_lim: 50.0
-      lon_acc_lim: 1.0
-      lon_jerk_lim: 0.5
-      lat_acc_lim: 2.0
-      lat_jerk_lim: 7.0
-      actual_steer_diff_lim: 1.0
+      reference_speed_points: [20.0, 30.0]
+      steer_lim: [1.0, 0.8]
+      steer_rate_lim: [1.0, 0.8]
+      lon_acc_lim: [1.0, 0.9]
+      lon_jerk_lim: [0.5, 0.4]
+      lat_acc_lim: [2.0, 1.8]
+      lat_jerk_lim: [7.0, 6.0]
+      actual_steer_diff_lim: [1.0, 0.8]

control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml

@@ -7,7 +7,7 @@
   <!-- vehicle info -->
   <arg name="vehicle_info_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml"/>
 
-  <node pkg="vehicle_cmd_gate" exec="vehicle_cmd_gate" name="vehicle_cmd_gate" output="screen">
+  <node pkg="vehicle_cmd_gate" exec="vehicle_cmd_gate_exe" name="vehicle_cmd_gate" output="screen">
     <remap from="input/steering" to="/vehicle/status/steering_status"/>
 
     <remap from="input/auto/control_cmd" to="trajectory_follower/control_cmd"/>

control/vehicle_cmd_gate/msg/IsFilterActivated.msg

@@ -0,0 +1,10 @@
+builtin_interfaces/Time stamp
+
+bool is_activated
+
+# for additional information
+bool is_activated_on_steering
+bool is_activated_on_steering_rate
+bool is_activated_on_speed
+bool is_activated_on_acceleration
+bool is_activated_on_jerk

control/vehicle_cmd_gate/package.xml

@@ -13,6 +13,8 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <build_depend>rosidl_default_generators</build_depend>
+
   <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_control_msgs</depend>
   <depend>autoware_auto_system_msgs</depend>
@@ -32,11 +34,16 @@
   <depend>tier4_system_msgs</depend>
   <depend>tier4_vehicle_msgs</depend>
   <depend>vehicle_info_util</depend>
+  <depend>visualization_msgs</depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

control/vehicle_cmd_gate/src/marker_helper.hpp

@@ -0,0 +1,119 @@
+// Copyright 2020 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.
+
+#ifndef MARKER_HELPER_HPP_
+#define MARKER_HELPER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <string>
+
+namespace vehicle_cmd_gate
+{
+inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z)
+{
+  geometry_msgs::msg::Point point;
+
+  point.x = x;
+  point.y = y;
+  point.z = z;
+
+  return point;
+}
+
+inline geometry_msgs::msg::Quaternion createMarkerOrientation(
+  double x, double y, double z, double w)
+{
+  geometry_msgs::msg::Quaternion quaternion;
+
+  quaternion.x = x;
+  quaternion.y = y;
+  quaternion.z = z;
+  quaternion.w = w;
+
+  return quaternion;
+}
+
+inline geometry_msgs::msg::Vector3 createMarkerScale(double x, double y, double z)
+{
+  geometry_msgs::msg::Vector3 scale;
+
+  scale.x = x;
+  scale.y = y;
+  scale.z = z;
+
+  return scale;
+}
+
+inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, float a)
+{
+  std_msgs::msg::ColorRGBA color;
+
+  color.r = r;
+  color.g = g;
+  color.b = b;
+  color.a = a;
+
+  return color;
+}
+
+inline visualization_msgs::msg::Marker createMarker(
+  const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type,
+  geometry_msgs::msg::Point point, geometry_msgs::msg::Vector3 scale,
+  const std_msgs::msg::ColorRGBA & color)
+{
+  visualization_msgs::msg::Marker marker;
+
+  marker.header.frame_id = frame_id;
+  marker.header.stamp = rclcpp::Time(0);
+  marker.ns = ns;
+  marker.id = id;
+  marker.type = type;
+  marker.action = visualization_msgs::msg::Marker::ADD;
+  marker.lifetime = rclcpp::Duration::from_seconds(0.2);
+  marker.pose.position = point;
+  marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0);
+  marker.scale = scale;
+  marker.color = color;
+  marker.frame_locked = false;
+
+  return marker;
+}
+
+inline visualization_msgs::msg::Marker createStringMarker(
+  const std::string & frame_id, const std::string & ns, const int32_t id, const int32_t type,
+  geometry_msgs::msg::Point point, geometry_msgs::msg::Vector3 scale,
+  const std_msgs::msg::ColorRGBA & color, const std::string text)
+{
+  visualization_msgs::msg::Marker marker;
+
+  marker = createMarker(frame_id, ns, id, type, point, scale, color);
+  marker.text = text;
+
+  return marker;
+}
+
+inline void appendMarkerArray(
+  const visualization_msgs::msg::MarkerArray & additional_marker_array,
+  visualization_msgs::msg::MarkerArray * marker_array)
+{
+  for (const auto & marker : additional_marker_array.markers) {
+    marker_array->markers.push_back(marker);
+  }
+}
+}  // namespace vehicle_cmd_gate
+
+#endif  // MARKER_HELPER_HPP_