feat(radar_object_tracker): tune radar object tracker node for untrus…

…table radar input (autowarefoundation#5409) * fix include file Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * switchable trust/untrust input yaw and twist information via parameters Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * Set measurement count threshold tunable Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * enable to switch off acceleration estimation Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * add constant turn rate tracker model Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * tune default parameters Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * set orientation unreliable when object is yaw input is not trusted Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * set ekf parameter to static so that it reduces yaml loading function calls Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix launch file to load tracking_config_directory as arguments Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * update readme Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> --------- Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
lexavtanke · Oct 30, 2023 · 6632d79 · 6632d79
1 parent 65f0a71
commit 6632d79
Show file tree

Hide file tree

Showing 15 changed files with 931 additions and 71 deletions.
diff --git a/perception/radar_object_tracker/CMakeLists.txt b/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
 )

diff --git a/perception/radar_object_tracker/README.md b/perception/radar_object_tracker/README.md
@@ -46,7 +46,7 @@ See more details in the [models.md](models.md).
 | `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 | ""                          | The directory containing the tracking configuration files                                                       |
+| `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                                                                          |
@@ -65,6 +65,15 @@ See more details in the [models.md](models.md).
 
 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
 
 <!-- In the future, you can add assumptions and known limitations of this package. -->

diff --git a/perception/radar_object_tracker/config/data_association_matrix.param.yaml b/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

diff --git a/perception/radar_object_tracker/config/default_tracker.param.yaml b/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"
diff --git a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml
@@ -4,12 +4,11 @@
     # basic settings
     world_frame_id: "map"
     tracker_lifetime: 1.0  # [sec]
-    # if empty, use default config declared in this package
-    tracking_config_directory: ""
+    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: false
+    enable_delay_compensation: true
 
     # logging
     enable_logging: false
@@ -18,10 +17,10 @@
     # filtering
     ## 1. distance based filtering: remove closer objects than this threshold
     use_distance_based_noise_filtering: true
-    minimum_range_threshold: 70.0 # [m]
+    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: 5.0 # [m/s]
+    max_lateral_velocity: 7.0 # [m/s]
diff --git a/perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml b/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]
diff --git a/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml b/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]

diff --git a/...cker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp b/...cker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp
@@ -76,6 +76,8 @@ class RadarObjectTrackerNode : public rclcpp::Node
   float tracker_lifetime_;
   std::map<std::uint8_t, std::string> tracker_map_;
 
+  int measurement_count_threshold_;
+
   void onMeasurement(
     const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg);
   void onTimer();

diff --git a/..._tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp b/..._tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp
@@ -0,0 +1,112 @@
+// 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 RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_
+#define RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_
+
+#include "radar_object_tracker/tracker/model/tracker_base.hpp"
+
+#include <kalman_filter/kalman_filter.hpp>
+
+#include <string>
+
+using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
+class ConstantTurnRateMotionTracker : public Tracker  // means constant turn rate motion tracker
+{
+private:
+  autoware_auto_perception_msgs::msg::DetectedObject object_;
+  rclcpp::Logger logger_;
+
+private:
+  KalmanFilter ekf_;
+  rclcpp::Time last_update_time_;
+  enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, WZ = 4 };
+
+  struct EkfParams
+  {
+    // dimension
+    char dim_x = 5;
+    // system noise
+    double q_cov_x;
+    double q_cov_y;
+    double q_cov_yaw;
+    double q_cov_vx;
+    double q_cov_wz;
+    // measurement noise
+    double r_cov_x;
+    double r_cov_y;
+    double r_cov_yaw;
+    double r_cov_vx;
+    // initial state covariance
+    double p0_cov_x;
+    double p0_cov_y;
+    double p0_cov_yaw;
+    double p0_cov_vx;
+    double p0_cov_wz;
+  };
+  static EkfParams ekf_params_;
+
+  // limitation
+  static double max_vx_;
+  // rough tracking parameters
+  float z_;
+
+  // lpf parameter
+  static double filter_tau_;  // time constant of 1st order low pass filter
+  static double filter_dt_;   // sampling time of 1st order low pass filter
+
+  // static flags
+  static bool is_initialized_;
+  static bool trust_yaw_input_;
+  static bool trust_twist_input_;
+  static bool use_polar_coordinate_in_measurement_noise_;
+  static bool assume_zero_yaw_rate_;
+
+private:
+  struct BoundingBox
+  {
+    double length;
+    double width;
+    double height;
+  };
+  struct Cylinder
+  {
+    double width;
+    double height;
+  };
+  BoundingBox bounding_box_;
+  Cylinder cylinder_;
+
+public:
+  ConstantTurnRateMotionTracker(
+    const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    const std::string & tracker_param_file, const std::uint8_t & label);
+
+  static void loadDefaultModelParameters(const std::string & path);
+  bool predict(const rclcpp::Time & time) override;
+  bool predict(const double dt, KalmanFilter & ekf) const;
+  bool measure(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
+    const geometry_msgs::msg::Transform & self_transform) override;
+  bool measureWithPose(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    const geometry_msgs::msg::Transform & self_transform);
+  bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
+  bool getTrackedObject(
+    const rclcpp::Time & time,
+    autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
+  virtual ~ConstantTurnRateMotionTracker() {}
+};
+
+#endif  // RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_
diff --git a/...radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp b/...radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp
@@ -56,18 +56,25 @@ class LinearMotionTracker : public Tracker
     double p0_cov_vy;
     double p0_cov_ax;
     double p0_cov_ay;
-  } ekf_params_;
+  };
+  static EkfParams ekf_params_;
 
   // limitation
-  double max_vx_;
-  double max_vy_;
+  static double max_vx_;
+  static double max_vy_;
   // rough tracking parameters
   float z_;
   float yaw_;
 
   // lpf parameter
-  double filter_tau_;  // time constant of 1st order low pass filter
-  double filter_dt_;   // sampling time of 1st order low pass filter
+  static double filter_tau_;  // time constant of 1st order low pass filter
+  static double filter_dt_;   // sampling time of 1st order low pass filter
+
+  static bool is_initialized_;
+  static bool estimate_acc_;
+  static bool trust_yaw_input_;
+  static bool trust_twist_input_;
+  static bool use_polar_coordinate_in_measurement_noise_;
 
 private:
   struct BoundingBox
@@ -89,13 +96,15 @@ class LinearMotionTracker : public Tracker
     const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
     const std::string & tracker_param_file, const std::uint8_t & label);
 
-  void loadDefaultModelParameters(const std::string & path);
+  static void loadDefaultModelParameters(const std::string & path);
   bool predict(const rclcpp::Time & time) override;
   bool predict(const double dt, KalmanFilter & ekf) const;
   bool measure(
     const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
-  bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
+  bool measureWithPose(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    const geometry_msgs::msg::Transform & self_transform);
   bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
   bool getTrackedObject(
     const rclcpp::Time & time,

diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp
@@ -15,6 +15,7 @@
 #ifndef RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_
 #define RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_
 
+#include "model/constant_turn_rate_motion_tracker.hpp"
 #include "model/linear_motion_tracker.hpp"
 #include "model/tracker_base.hpp"
 

diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml
@@ -4,8 +4,9 @@
   <arg name="output" default="objects"/>
   <arg name="world_frame_id" default="map"/>
   <arg name="publish_rate" default="10.0"/>
-  <arg name="enable_delay_compensation" default="false"/>
+  <arg name="enable_delay_compensation" default="true"/>
   <arg name="tracker_setting_path" default="$(find-pkg-share radar_object_tracker)/config/default_tracker.param.yaml"/>
+  <arg name="tracking_config_directory" default="$(find-pkg-share radar_object_tracker)/config/tracking/"/>
   <arg name="data_association_matrix_path" default="$(find-pkg-share radar_object_tracker)/config/data_association_matrix.param.yaml"/>
   <arg name="radar_object_tracker_param_path" default="$(find-pkg-share radar_object_tracker)/config/radar_object_tracker.param.yaml"/>
   <arg name="vector_map_topic" default="/map/vector_map"/>
@@ -17,6 +18,7 @@
     <param from="$(var tracker_setting_path)"/>
     <param from="$(var data_association_matrix_path)"/>
     <param from="$(var radar_object_tracker_param_path)"/>
+    <param name="tracking_config_directory" value="$(var tracking_config_directory)"/>
     <param name="world_frame_id" value="$(var world_frame_id)"/>
     <param name="publish_rate" value="$(var publish_rate)"/>
     <param name="enable_delay_compensation" value="$(var enable_delay_compensation)"/>

diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp
@@ -207,6 +207,7 @@ RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_
   // Parameters
   tracker_lifetime_ = declare_parameter<double>("tracker_lifetime");
   double publish_rate = declare_parameter<double>("publish_rate");
+  measurement_count_threshold_ = declare_parameter<int>("measurement_count_threshold");
   world_frame_id_ = declare_parameter<std::string>("world_frame_id");
   bool enable_delay_compensation{declare_parameter<bool>("enable_delay_compensation")};
   tracker_config_directory_ = declare_parameter<std::string>("tracking_config_directory");
@@ -363,6 +364,10 @@ std::shared_ptr<Tracker> RadarObjectTrackerNode::createNewTracker(
     if (tracker == "linear_motion_tracker") {
       std::string config_file = tracker_config_directory_ + "linear_motion_tracker.yaml";
       return std::make_shared<LinearMotionTracker>(time, object, config_file, label);
+    } else if (tracker == "constant_turn_rate_motion_tracker") {
+      std::string config_file =
+        tracker_config_directory_ + "constant_turn_rate_motion_tracker.yaml";
+      return std::make_shared<ConstantTurnRateMotionTracker>(time, object, config_file, label);
     } else {
       // not implemented yet so put warning
       RCLCPP_WARN(get_logger(), "Tracker %s is not implemented yet", tracker.c_str());
@@ -530,8 +535,7 @@ void RadarObjectTrackerNode::sanitizeTracker(
 inline bool RadarObjectTrackerNode::shouldTrackerPublish(
   const std::shared_ptr<const Tracker> tracker) const
 {
-  constexpr int measurement_count_threshold = 3;
-  if (tracker->getTotalMeasurementCount() < measurement_count_threshold) {
+  if (tracker->getTotalMeasurementCount() < measurement_count_threshold_) {
     return false;
   }
   return true;