Merge pull request autowarefoundation#628 from tier4/sync-upstream

chore: sync upstream

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -10,6 +10,7 @@
     emergency_acceleration: -2.4
     moderate_stop_service_acceleration: -1.5
     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/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);
@@ -145,6 +148,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
   emergency_acceleration_ = declare_parameter<double>("emergency_acceleration");
   moderate_stop_service_acceleration_ =
     declare_parameter<double>("moderate_stop_service_acceleration");
+  stop_check_duration_ = declare_parameter<double>("stop_check_duration");
 
   // Vehicle Parameter
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -492,7 +496,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont
   const double dt = getDt();
   const auto mode = current_operation_mode_;
   const auto current_status_cmd = getActualStatusAsCommand();
-  const auto ego_is_stopped = std::abs(current_status_cmd.longitudinal.speed) < 1e-3;
+  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.

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -20,6 +20,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>
 
@@ -70,6 +71,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;
@@ -219,6 +221,10 @@ class VehicleCmdGate : public rclcpp::Node
   // Pause interface for API
   std::unique_ptr<AdapiPauseInterface> adapi_pause_;
   std::unique_ptr<ModerateStopInterface> moderate_stop_interface_;
+
+  // stop checker
+  std::unique_ptr<VehicleStopChecker> vehicle_stop_checker_;
+  double stop_check_duration_;
 };
 
 }  // namespace vehicle_cmd_gate

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -102,7 +102,7 @@ def launch_setup(context, *args, **kwargs):
             behavior_path_planner_param,
             vehicle_param,
             {
-                "lane_change.enable_abort_lane_change": LaunchConfiguration(
+                "lane_change.cancel.enable_on_lane_changing_phase": LaunchConfiguration(
                     "use_experimental_lane_change_function"
                 ),
                 "lane_change.enable_collision_check_at_prepare_phase": LaunchConfiguration(

perception/tensorrt_yolox/CMakeLists.txt

@@ -63,6 +63,8 @@ function(download FILE_NAME FILE_HASH)
 endfunction()
 
 download(yolox-tiny.onnx 97028baf73ce55e115599c9c60651b08)
+download(yolox-sPlus-opt.onnx bf3b0155351f90fcdca2626acbfd3bcf)
+download(yolox-sPlus-opt.EntropyV2-calibration.table c6e6f1999d5724a017516a956096701f)
 download(label.txt 9ceadca8b72b6169ee6aabb861fe3e1e)
 
 ##########

perception/tensorrt_yolox/README.md

@@ -79,11 +79,17 @@ hence these parameters are ignored when users specify ONNX models including this
 
 This package accepts both `EfficientNMS_TRT` attached ONNXs and [models published from the official YOLOX repository](https://github.com/Megvii-BaseDetection/YOLOX/tree/main/demo/ONNXRuntime#download-onnx-models) (we referred to them as "plain" models).
 
+In addition to `yolox-tiny.onnx`, a custom model named `yolox-sPlus-opt.onnx` is either available.
+This model is based on YOLOX-s and tuned to perform more accurate detection with almost comparable execution speed with `yolox-tiny`.
+To get better results with this model, users are recommended to use some specific running arguments
+such as `precision:=int8`, `calibration_algorithm:=Entropy`, `clip_value:=6.0`.
+Users can refer `launch/yolox_sPlus_opt.launch.xml` to see how this model can be used.
+
 All models are automatically converted to TensorRT format.
 These converted files will be saved in the same directory as specified ONNX files
 with `.engine` filename extension and reused from the next run.
-The conversion process may take a while (typically a few minutes) and the inference process is blocked
-until complete the conversion, so it will take some time until detection results are published on the first run.
+The conversion process may take a while (**typically 10 to 20 minutes**) and the inference process is blocked
+until complete the conversion, so it will take some time until detection results are published (**even until appearing in the topic list**) on the first run
 
 ### Package acceptable model generation
 

perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp

@@ -81,9 +81,8 @@ class TrtYoloX
     const std::string & model_path, const std::string & precision, const int num_class = 8,
     const float score_threshold = 0.3, const float nms_threshold = 0.7,
     const tensorrt_common::BuildConfig build_config = tensorrt_common::BuildConfig(),
-    const bool use_gpu_preprocess = false,
-    const std::string & calibration_image_list_file = std::string(), const double norm_factor = 1.0,
-    const std::string & cache_dir = "",
+    const bool use_gpu_preprocess = false, std::string calibration_image_list_file = std::string(),
+    const double norm_factor = 1.0, const std::string & cache_dir = "",
     const tensorrt_common::BatchConfig & batch_config = {1, 1, 1},
     const size_t max_workspace_size = (1 << 30));
 
@@ -101,7 +100,7 @@ class TrtYoloX
    * @warning if we don't allocate buffers using it, "preprocess_gpu" allocates buffers at the
    * beginning
    */
-  void initPreprocesBuffer(int width, int height);
+  void initPreprocessBuffer(int width, int height);
 
   /**
    * @brief output TensorRT profiles for each layer
@@ -169,11 +168,11 @@ class TrtYoloX
 
   CudaUniquePtrHost<float[]> out_prob_h_;
 
-  // flag whether prepreceses are performed on GPU
+  // flag whether preprocess are performed on GPU
   bool use_gpu_preprocess_;
-  // host buffer for preprecessing on GPU
+  // host buffer for preprocessing on GPU
   CudaUniquePtrHost<unsigned char[]> image_buf_h_;
-  // device buffer for preprecessing on GPU
+  // device buffer for preprocessing on GPU
   CudaUniquePtr<unsigned char[]> image_buf_d_;
   // normalization factor used for preprocessing
   double norm_factor_;

perception/tensorrt_yolox/launch/yolox.launch.xml

@@ -0,0 +1 @@
+yolox_s_plus_opt.launch.xml

perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="input/image" default="/sensing/camera/camera0/image_rect_color"/>
+  <arg name="output/objects" default="/perception/object_recognition/detection/rois0"/>
+  <arg name="model_name" default="yolox-sPlus-opt"/>
+  <arg name="model_path" default="$(find-pkg-share tensorrt_yolox)/data"/>
+  <arg name="score_threshold" default="0.35"/>
+  <arg name="nms_threshold" default="0.7"/>
+  <arg name="precision" default="int8" description="operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8]"/>
+  <arg
+    name="calibration_algorithm"
+    default="Entropy"
+    description="Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]"
+  />
+  <arg name="dla_core_id" default="-1" description="If positive ID value is specified, the node assign inference task to the DLA core"/>
+  <arg name="quantize_first_layer" default="false" description="If true, set the operating precision for the first (input) layer to be fp16. This option is valid only when precision==int8"/>
+  <arg name="quantize_last_layer" default="false" description="If true, set the operating precision for the last (output) layer to be fp16. This option is valid only when precision==int8"/>
+  <arg
+    name="profile_per_layer"
+    default="false"
+    description="If true, profiler function will be enabled. Since the profile function may affect execution speed, it is recommended to set this flag true only for development purpose."
+  />
+  <arg
+    name="clip_value"
+    default="6.0"
+    description="If positive value is specified, the value of each layer output will be clipped between [0.0, clip_value]. This option is valid only when precision==int8 and used to manually specify the dynamic range instead of using any calibration."
+  />
+  <arg name="preprocess_on_gpu" default="true" description="If true, pre-processing is performed on GPU"/>
+  <arg name="calibration_image_list_path" default="" description="Path to a file which contains path to images. Those images will be used for int8 quantization."/>
+  <arg name="use_decompress" default="true" description="use image decompress"/>
+  <arg name="build_only" default="false" description="exit after trt engine is built"/>
+
+  <node pkg="image_transport_decompressor" exec="image_transport_decompressor_node" name="image_transport_decompressor_node" if="$(var use_decompress)">
+    <remap from="~/input/compressed_image" to="$(var input/image)/compressed"/>
+    <remap from="~/output/raw_image" to="$(var input/image)"/>
+  </node>
+
+  <node pkg="tensorrt_yolox" exec="tensorrt_yolox_node_exe" name="tensorrt_yolox" output="screen">
+    <remap from="~/in/image" to="$(var input/image)"/>
+    <remap from="~/out/objects" to="$(var output/objects)"/>
+    <param name="score_threshold" value="$(var score_threshold)"/>
+    <param name="nms_threshold" value="$(var nms_threshold)"/>
+    <param name="model_path" value="$(var model_path)/$(var model_name).onnx"/>
+    <param name="label_path" value="$(var model_path)/label.txt"/>
+    <param name="precision" value="$(var precision)"/>
+    <param name="calibration_algorithm" value="$(var calibration_algorithm)"/>
+    <param name="dla_core_id" value="$(var dla_core_id)"/>
+    <param name="quantize_first_layer" value="$(var quantize_first_layer)"/>
+    <param name="quantize_last_layer" value="$(var quantize_last_layer)"/>
+    <param name="profile_per_layer" value="$(var profile_per_layer)"/>
+    <param name="clip_value" value="$(var clip_value)"/>
+    <param name="preprocess_on_gpu" value="$(var preprocess_on_gpu)"/>
+    <param name="calibration_image_list_path" value="$(var calibration_image_list_path)"/>
+    <param name="build_only" value="$(var build_only)"/>
+  </node>
+</launch>

perception/tensorrt_yolox/launch/yolox_tiny.launch.xml

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="input/image" default="/sensing/camera/camera0/image_rect_color"/>
+  <arg name="output/objects" default="/perception/object_recognition/detection/rois0"/>
+  <arg name="model_name" default="yolox-tiny"/>
+  <arg name="model_path" default="$(find-pkg-share tensorrt_yolox)/data"/>
+  <arg name="score_threshold" default="0.35"/>
+  <arg name="nms_threshold" default="0.7"/>
+  <arg name="precision" default="fp16" description="operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8]"/>
+  <arg
+    name="calibration_algorithm"
+    default="MinMax"
+    description="Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]"
+  />
+  <arg name="dla_core_id" default="-1" description="If positive ID value is specified, the node assign inference task to the DLA core"/>
+  <arg name="quantize_first_layer" default="false" description="If true, set the operating precision for the first (input) layer to be fp16. This option is valid only when precision==int8"/>
+  <arg name="quantize_last_layer" default="false" description="If true, set the operating precision for the last (output) layer to be fp16. This option is valid only when precision==int8"/>
+  <arg
+    name="profile_per_layer"
+    default="false"
+    description="If true, profiler function will be enabled. Since the profile function may affect execution speed, it is recommended to set this flag true only for development purpose."
+  />
+  <arg
+    name="clip_value"
+    default="0.0"
+    description="If positive value is specified, the value of each layer output will be clipped between [0.0, clip_value]. This option is valid only when precision==int8 and used to manually specify the dynamic range instead of using any calibration."
+  />
+  <arg name="preprocess_on_gpu" default="true" description="If true, pre-processing is performed on GPU"/>
+  <arg name="calibration_image_list_path" default="" description="Path to a file which contains path to images. Those images will be used for int8 quantization."/>
+  <arg name="use_decompress" default="true" description="use image decompress"/>
+  <arg name="build_only" default="false" description="exit after trt engine is built"/>
+
+  <node pkg="image_transport_decompressor" exec="image_transport_decompressor_node" name="image_transport_decompressor_node" if="$(var use_decompress)">
+    <remap from="~/input/compressed_image" to="$(var input/image)/compressed"/>
+    <remap from="~/output/raw_image" to="$(var input/image)"/>
+  </node>
+
+  <node pkg="tensorrt_yolox" exec="tensorrt_yolox_node_exe" name="tensorrt_yolox" output="screen">
+    <remap from="~/in/image" to="$(var input/image)"/>
+    <remap from="~/out/objects" to="$(var output/objects)"/>
+    <param name="score_threshold" value="$(var score_threshold)"/>
+    <param name="nms_threshold" value="$(var nms_threshold)"/>
+    <param name="model_path" value="$(var model_path)/$(var model_name).onnx"/>
+    <param name="label_path" value="$(var model_path)/label.txt"/>
+    <param name="precision" value="$(var precision)"/>
+    <param name="calibration_algorithm" value="$(var calibration_algorithm)"/>
+    <param name="dla_core_id" value="$(var dla_core_id)"/>
+    <param name="quantize_first_layer" value="$(var quantize_first_layer)"/>
+    <param name="quantize_last_layer" value="$(var quantize_last_layer)"/>
+    <param name="profile_per_layer" value="$(var profile_per_layer)"/>
+    <param name="clip_value" value="$(var clip_value)"/>
+    <param name="preprocess_on_gpu" value="$(var preprocess_on_gpu)"/>
+    <param name="calibration_image_list_path" value="$(var calibration_image_list_path)"/>
+    <param name="build_only" value="$(var build_only)"/>
+  </node>
+</launch>