Merge pull request #996 from YoshiRi/feat/refine_radar_tracking_algor…

…ithm

chore(radar_object_tracker): refine radar object tracker

perception/radar_object_tracker/CMakeLists.txt

@@ -27,6 +27,7 @@ ament_auto_add_library(radar_object_tracker_node SHARED
   src/radar_object_tracker_node/radar_object_tracker_node.cpp
   src/tracker/model/tracker_base.cpp
   src/tracker/model/linear_motion_tracker.cpp
+  src/tracker/model/constant_turn_rate_motion_tracker.cpp
   src/utils/utils.cpp
   src/data_association/data_association.cpp
 )

perception/radar_object_tracker/README.md

@@ -26,9 +26,10 @@ See more details in the [models.md](models.md).
 
 ### Input
 
-| Name      | Type                                                  | Description      |
-| --------- | ----------------------------------------------------- | ---------------- |
-| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | Detected objects |
+| Name          | Type                                                  | Description      |
+| ------------- | ----------------------------------------------------- | ---------------- |
+| `~/input`     | `autoware_auto_perception_msgs::msg::DetectedObjects` | Detected objects |
+| `/vector/map` | `autoware_auto_msgs::msg::HADMapBin`                  | Map data         |
 
 ### Output
 
@@ -40,20 +41,38 @@ See more details in the [models.md](models.md).
 
 ### Node Parameters
 
-| Name                        | Type   | Default Value                 | Description                                                 |
-| --------------------------- | ------ | ----------------------------- | ----------------------------------------------------------- |
-| `publish_rate`              | double | 30.0                          | The rate at which to publish the output messages            |
-| `world_frame_id`            | string | "world"                       | The frame ID of the world coordinate system                 |
-| `enable_delay_compensation` | bool   | false                         | Whether to enable delay compensation                        |
-| `tracking_config_directory` | string | ""                            | The directory containing the tracking configuration files   |
-| `enable_logging`            | bool   | false                         | Whether to enable logging                                   |
-| `logging_file_path`         | string | "~/.ros/association_log.json" | The path to the file where logs should be written           |
-| `can_assign_matrix`         | array  |                               | An array of integers used in the data association algorithm |
-| `max_dist_matrix`           | array  |                               | An array of doubles used in the data association algorithm  |
-| `max_area_matrix`           | array  |                               | An array of doubles used in the data association algorithm  |
-| `min_area_matrix`           | array  |                               | An array of doubles used in the data association algorithm  |
-| `max_rad_matrix`            | array  |                               | An array of doubles used in the data association algorithm  |
-| `min_iou_matrix`            | array  |                               | An array of doubles used in the data association algorithm  |
+| Name                                 | Type   | Default Value               | Description                                                                                                     |
+| ------------------------------------ | ------ | --------------------------- | --------------------------------------------------------------------------------------------------------------- |
+| `publish_rate`                       | double | 10.0                        | The rate at which to publish the output messages                                                                |
+| `world_frame_id`                     | string | "map"                       | The frame ID of the world coordinate system                                                                     |
+| `enable_delay_compensation`          | bool   | false                       | Whether to enable delay compensation. If set to `true`, output topic is published by timer with `publish_rate`. |
+| `tracking_config_directory`          | string | "./config/tracking/"        | The directory containing the tracking configuration files                                                       |
+| `enable_logging`                     | bool   | false                       | Whether to enable logging                                                                                       |
+| `logging_file_path`                  | string | "/tmp/association_log.json" | The path to the file where logs should be written                                                               |
+| `tracker_lifetime`                   | double | 1.0                         | The lifetime of the tracker in seconds                                                                          |
+| `use_distance_based_noise_filtering` | bool   | true                        | Whether to use distance based filtering                                                                         |
+| `minimum_range_threshold`            | double | 70.0                        | Minimum distance threshold for filtering in meters                                                              |
+| `use_map_based_noise_filtering`      | bool   | true                        | Whether to use map based filtering                                                                              |
+| `max_distance_from_lane`             | double | 5.0                         | Maximum distance from lane for filtering in meters                                                              |
+| `max_angle_diff_from_lane`           | double | 0.785398                    | Maximum angle difference from lane for filtering in radians                                                     |
+| `max_lateral_velocity`               | double | 5.0                         | Maximum lateral velocity for filtering in m/s                                                                   |
+| `can_assign_matrix`                  | array  |                             | An array of integers used in the data association algorithm                                                     |
+| `max_dist_matrix`                    | array  |                             | An array of doubles used in the data association algorithm                                                      |
+| `max_area_matrix`                    | array  |                             | An array of doubles used in the data association algorithm                                                      |
+| `min_area_matrix`                    | array  |                             | An array of doubles used in the data association algorithm                                                      |
+| `max_rad_matrix`                     | array  |                             | An array of doubles used in the data association algorithm                                                      |
+| `min_iou_matrix`                     | array  |                             | An array of doubles used in the data association algorithm                                                      |
+
+See more details in the [models.md](models.md).
+
+### Tracker parameters
+
+Currently, this package supports the following trackers:
+
+- `linear_motion_tracker`
+- `constant_turn_rate_motion_tracker`
+
+Default settings for each tracker are defined in the [./config/tracking/](./config/tracking/), and described in [models.md](models.md).
 
 ## Assumptions / Known limits
 

perception/radar_object_tracker/config/data_association_matrix.param.yaml

@@ -14,10 +14,10 @@
     max_dist_matrix:
       #UNKNOWN, CAR, TRUCK, BUS,  TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
       [4.0,     1.0, 1.0,   1.0,  1.0,     1.0,       1.0,     1.0,       #UNKNOWN
-       4.0,     4.0, 5.0,   5.0,  5.0,     1.0,       1.0,     1.0,       #CAR
-       4.0,     2.0, 5.0,   5.0,  5.0,     1.0,       1.0,     1.0,       #TRUCK
-       4.0,     2.0, 5.0,   5.0,  5.0,     1.0,       1.0,     1.0,       #BUS
-       4.0,     2.0, 5.0,   5.0,  5.0,     1.0,       1.0,     1.0,       #TRAILER
+       4.0,     8.0, 8.0,   8.0,  8.0,     1.0,       1.0,     1.0,       #CAR
+       4.0,     6.0, 8.0,   8.0,  8.0,     1.0,       1.0,     1.0,       #TRUCK
+       4.0,     6.0, 8.0,   8.0,  8.0,     1.0,       1.0,     1.0,       #BUS
+       4.0,     6.0, 8.0,   8.0,  8.0,     1.0,       1.0,     1.0,       #TRAILER
        3.0,     1.0, 1.0,   1.0,  1.0,     3.0,       3.0,     2.0,       #MOTORBIKE
        3.0,     1.0, 1.0,   1.0,  1.0,     3.0,       3.0,     2.0,       #BICYCLE
        2.0,     1.0, 1.0,   1.0,  1.0,     3.0,       3.0,     2.0]       #PEDESTRIAN

perception/radar_object_tracker/config/default_tracker.param.yaml

@@ -1,9 +1,9 @@
 /**:
   ros__parameters:
-    car_tracker: "linear_motion_tracker"
-    truck_tracker: "linear_motion_tracker"
-    bus_tracker: "linear_motion_tracker"
-    trailer_tracker: "linear_motion_tracker"
-    pedestrian_tracker: "linear_motion_tracker"
-    bicycle_tracker: "linear_motion_tracker"
-    motorcycle_tracker: "linear_motion_tracker"
+    car_tracker: "constant_turn_rate_motion_tracker"
+    truck_tracker: "constant_turn_rate_motion_tracker"
+    bus_tracker: "constant_turn_rate_motion_tracker"
+    trailer_tracker: "constant_turn_rate_motion_tracker"
+    pedestrian_tracker: "constant_turn_rate_motion_tracker"
+    bicycle_tracker: "constant_turn_rate_motion_tracker"
+    motorcycle_tracker: "constant_turn_rate_motion_tracker"

perception/radar_object_tracker/config/radar_object_tracker.param.yaml

@@ -0,0 +1,26 @@
+# general parameters for radar_object_tracker node
+/**:
+  ros__parameters:
+    # basic settings
+    world_frame_id: "map"
+    tracker_lifetime: 1.0  # [sec]
+    measurement_count_threshold: 3 # object will be published if it is tracked more than this threshold
+
+    # delay compensate parameters
+    publish_rate: 10.0
+    enable_delay_compensation: true
+
+    # logging
+    enable_logging: false
+    logging_file_path: "/tmp/association_log.json"
+
+    # filtering
+    ## 1. distance based filtering: remove closer objects than this threshold
+    use_distance_based_noise_filtering: true
+    minimum_range_threshold: 60.0 # [m]
+
+    ## 2. lanelet map based filtering
+    use_map_based_noise_filtering: true
+    max_distance_from_lane: 5.0 # [m]
+    max_angle_diff_from_lane: 0.785398 # [rad] (45 deg)
+    max_lateral_velocity: 7.0 # [m/s]

perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml

@@ -0,0 +1,36 @@
+default:
+  # This file defines the parameters for the linear motion tracker.
+  # All this parameter coordinate is assumed to be in the vehicle coordinate system.
+  # So, the x axis is pointing to the front of the vehicle, y axis is pointing to the left of the vehicle.
+  ekf_params:
+    # random walk noise is used to model the acceleration noise
+    process_noise_std: # [m/s^2]
+      x: 0.5
+      y: 0.5
+      yaw: 0.1
+      vx: 1.0 # assume 1m/s velocity noise
+      wz: 0.4
+    measurement_noise_std:
+      x: 4.0 # [m]
+      y: 4.0 # [m]
+      # y: 0.02 # rad/m if use_polar_coordinate_in_measurement_noise is true
+      yaw: 0.2 # [rad]
+      vx: 10 # [m/s]
+    initial_covariance_std:
+      x: 3.0 # [m]
+      y: 6.0 # [m]
+      yaw: 10.0 # [rad]
+      vx: 100.0 # [m/s]
+      wz: 10.0 # [rad/s]
+  # input flag
+  trust_yaw_input: false # set true if yaw input of sensor is reliable
+  trust_twist_input: false # set true if twist input of sensor is reliable
+  use_polar_coordinate_in_measurement_noise: false # set true if you want to define the measurement noise in polar coordinate
+  assume_zero_yaw_rate: false # set true if you want to assume zero yaw rate
+  # output limitation
+  limit:
+    max_speed: 80.0 # [m/s]
+  # low pass filter is used to smooth the yaw and shape estimation
+  low_pass_filter:
+    time_constant: 1.0 # [s]
+    sampling_time: 0.1 # [s]

perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml

@@ -7,22 +7,28 @@ default:
     process_noise_std: # [m/s^2]
       ax: 0.98 # assume 0.1G acceleration noise
       ay: 0.98
-      vx: 0.1 # assume 0.1m/s velocity noise
-      vy: 0.1
+      vx: 1.0 # assume 1m/s velocity noise
+      vy: 1.0
       x: 1.0 # assume 1m position noise
       y: 1.0
     measurement_noise_std:
       x: 0.6 # [m]
-      y: 0.9 # [m]
-      vx: 0.4 # [m/s]
-      vy: 1 # [m/s]
+      # y: 4.0 # [m]
+      y: 0.01 # rad/m if use_polar_coordinate_in_measurement_noise is true
+      vx: 10 # [m/s]
+      vy: 100 # [m/s]
     initial_covariance_std:
       x: 3.0 # [m]
       y: 6.0 # [m]
-      vx: 1.0 # [m/s]
-      vy: 5.0 # [m/s]
-      ax: 0.5 # [m/s^2]
-      ay: 1.0 # [m/s^2]
+      vx: 1000.0 # [m/s]
+      vy: 1000.0 # [m/s]
+      ax: 1000.0 # [m/s^2]
+      ay: 1000.0 # [m/s^2]
+    estimate_acc: false # set true if you want to estimate the acceleration
+  # input flag
+  trust_yaw_input: false # set true if yaw input of sensor is reliable
+  trust_twist_input: false # set true if twist input of sensor is reliable
+  use_polar_coordinate_in_measurement_noise: true # set true if you want to define the measurement noise in polar coordinate
   # output limitation
   limit:
     max_speed: 80.0 # [m/s]