Skip to content

Commit 6632d79

Browse files
YoshiRiYoshiRi
authored
feat(radar_object_tracker): tune radar object tracker node for untrustable radar input (#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>
1 parent 65f0a71 commit 6632d79

15 files changed

+931
-71
lines changed

‎perception/radar_object_tracker/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@ ament_auto_add_library(radar_object_tracker_node SHARED
2727
src/radar_object_tracker_node/radar_object_tracker_node.cpp
2828
src/tracker/model/tracker_base.cpp
2929
src/tracker/model/linear_motion_tracker.cpp
30+
src/tracker/model/constant_turn_rate_motion_tracker.cpp
3031
src/utils/utils.cpp
3132
src/data_association/data_association.cpp
3233
)

‎perception/radar_object_tracker/README.md

+10-1
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ See more details in the [models.md](models.md).
4646
| `publish_rate` | double | 10.0 | The rate at which to publish the output messages |
4747
| `world_frame_id` | string | "map" | The frame ID of the world coordinate system |
4848
| `enable_delay_compensation` | bool | false | Whether to enable delay compensation. If set to `true`, output topic is published by timer with `publish_rate`. |
49-
| `tracking_config_directory` | string | "" | The directory containing the tracking configuration files |
49+
| `tracking_config_directory` | string | "./config/tracking/" | The directory containing the tracking configuration files |
5050
| `enable_logging` | bool | false | Whether to enable logging |
5151
| `logging_file_path` | string | "/tmp/association_log.json" | The path to the file where logs should be written |
5252
| `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).
6565

6666
See more details in the [models.md](models.md).
6767

68+
### Tracker parameters
69+
70+
Currently, this package supports the following trackers:
71+
72+
- `linear_motion_tracker`
73+
- `constant_turn_rate_motion_tracker`
74+
75+
Default settings for each tracker are defined in the [./config/tracking/](./config/tracking/), and described in [models.md](models.md).
76+
6877
## Assumptions / Known limits
6978

7079
<!-- In the future, you can add assumptions and known limitations of this package. -->

‎perception/radar_object_tracker/config/data_association_matrix.param.yaml

+4-4
Original file line numberDiff line numberDiff line change
@@ -14,10 +14,10 @@
1414
max_dist_matrix:
1515
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
1616
[4.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN
17-
4.0, 4.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR
18-
4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK
19-
4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS
20-
4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER
17+
4.0, 8.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #CAR
18+
4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #TRUCK
19+
4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #BUS
20+
4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #TRAILER
2121
3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #MOTORBIKE
2222
3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE
2323
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

+7-7
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,9 @@
11
/**:
22
ros__parameters:
3-
car_tracker: "linear_motion_tracker"
4-
truck_tracker: "linear_motion_tracker"
5-
bus_tracker: "linear_motion_tracker"
6-
trailer_tracker: "linear_motion_tracker"
7-
pedestrian_tracker: "linear_motion_tracker"
8-
bicycle_tracker: "linear_motion_tracker"
9-
motorcycle_tracker: "linear_motion_tracker"
3+
car_tracker: "constant_turn_rate_motion_tracker"
4+
truck_tracker: "constant_turn_rate_motion_tracker"
5+
bus_tracker: "constant_turn_rate_motion_tracker"
6+
trailer_tracker: "constant_turn_rate_motion_tracker"
7+
pedestrian_tracker: "constant_turn_rate_motion_tracker"
8+
bicycle_tracker: "constant_turn_rate_motion_tracker"
9+
motorcycle_tracker: "constant_turn_rate_motion_tracker"

‎perception/radar_object_tracker/config/radar_object_tracker.param.yaml

+4-5
Original file line numberDiff line numberDiff line change
@@ -4,12 +4,11 @@
44
# basic settings
55
world_frame_id: "map"
66
tracker_lifetime: 1.0 # [sec]
7-
# if empty, use default config declared in this package
8-
tracking_config_directory: ""
7+
measurement_count_threshold: 3 # object will be published if it is tracked more than this threshold
98

109
# delay compensate parameters
1110
publish_rate: 10.0
12-
enable_delay_compensation: false
11+
enable_delay_compensation: true
1312

1413
# logging
1514
enable_logging: false
@@ -18,10 +17,10 @@
1817
# filtering
1918
## 1. distance based filtering: remove closer objects than this threshold
2019
use_distance_based_noise_filtering: true
21-
minimum_range_threshold: 70.0 # [m]
20+
minimum_range_threshold: 60.0 # [m]
2221

2322
## 2. lanelet map based filtering
2423
use_map_based_noise_filtering: true
2524
max_distance_from_lane: 5.0 # [m]
2625
max_angle_diff_from_lane: 0.785398 # [rad] (45 deg)
27-
max_lateral_velocity: 5.0 # [m/s]
26+
max_lateral_velocity: 7.0 # [m/s]

‎perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
default:
2+
# This file defines the parameters for the linear motion tracker.
3+
# All this parameter coordinate is assumed to be in the vehicle coordinate system.
4+
# So, the x axis is pointing to the front of the vehicle, y axis is pointing to the left of the vehicle.
5+
ekf_params:
6+
# random walk noise is used to model the acceleration noise
7+
process_noise_std: # [m/s^2]
8+
x: 0.5
9+
y: 0.5
10+
yaw: 0.1
11+
vx: 1.0 # assume 1m/s velocity noise
12+
wz: 0.4
13+
measurement_noise_std:
14+
x: 4.0 # [m]
15+
y: 4.0 # [m]
16+
# y: 0.02 # rad/m if use_polar_coordinate_in_measurement_noise is true
17+
yaw: 0.2 # [rad]
18+
vx: 10 # [m/s]
19+
initial_covariance_std:
20+
x: 3.0 # [m]
21+
y: 6.0 # [m]
22+
yaw: 10.0 # [rad]
23+
vx: 100.0 # [m/s]
24+
wz: 10.0 # [rad/s]
25+
# input flag
26+
trust_yaw_input: false # set true if yaw input of sensor is reliable
27+
trust_twist_input: false # set true if twist input of sensor is reliable
28+
use_polar_coordinate_in_measurement_noise: false # set true if you want to define the measurement noise in polar coordinate
29+
assume_zero_yaw_rate: false # set true if you want to assume zero yaw rate
30+
# output limitation
31+
limit:
32+
max_speed: 80.0 # [m/s]
33+
# low pass filter is used to smooth the yaw and shape estimation
34+
low_pass_filter:
35+
time_constant: 1.0 # [s]
36+
sampling_time: 0.1 # [s]

‎perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml

+15-9
Original file line numberDiff line numberDiff line change
@@ -7,22 +7,28 @@ default:
77
process_noise_std: # [m/s^2]
88
ax: 0.98 # assume 0.1G acceleration noise
99
ay: 0.98
10-
vx: 0.1 # assume 0.1m/s velocity noise
11-
vy: 0.1
10+
vx: 1.0 # assume 1m/s velocity noise
11+
vy: 1.0
1212
x: 1.0 # assume 1m position noise
1313
y: 1.0
1414
measurement_noise_std:
1515
x: 0.6 # [m]
16-
y: 0.9 # [m]
17-
vx: 0.4 # [m/s]
18-
vy: 1 # [m/s]
16+
# y: 4.0 # [m]
17+
y: 0.01 # rad/m if use_polar_coordinate_in_measurement_noise is true
18+
vx: 10 # [m/s]
19+
vy: 100 # [m/s]
1920
initial_covariance_std:
2021
x: 3.0 # [m]
2122
y: 6.0 # [m]
22-
vx: 1.0 # [m/s]
23-
vy: 5.0 # [m/s]
24-
ax: 0.5 # [m/s^2]
25-
ay: 1.0 # [m/s^2]
23+
vx: 1000.0 # [m/s]
24+
vy: 1000.0 # [m/s]
25+
ax: 1000.0 # [m/s^2]
26+
ay: 1000.0 # [m/s^2]
27+
estimate_acc: false # set true if you want to estimate the acceleration
28+
# input flag
29+
trust_yaw_input: false # set true if yaw input of sensor is reliable
30+
trust_twist_input: false # set true if twist input of sensor is reliable
31+
use_polar_coordinate_in_measurement_noise: true # set true if you want to define the measurement noise in polar coordinate
2632
# output limitation
2733
limit:
2834
max_speed: 80.0 # [m/s]

‎perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,8 @@ class RadarObjectTrackerNode : public rclcpp::Node
7676
float tracker_lifetime_;
7777
std::map<std::uint8_t, std::string> tracker_map_;
7878

79+
int measurement_count_threshold_;
80+
7981
void onMeasurement(
8082
const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg);
8183
void onTimer();

‎perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp

+112
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
// Copyright 2020 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_
16+
#define RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_
17+
18+
#include "radar_object_tracker/tracker/model/tracker_base.hpp"
19+
20+
#include <kalman_filter/kalman_filter.hpp>
21+
22+
#include <string>
23+
24+
using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
25+
class ConstantTurnRateMotionTracker : public Tracker // means constant turn rate motion tracker
26+
{
27+
private:
28+
autoware_auto_perception_msgs::msg::DetectedObject object_;
29+
rclcpp::Logger logger_;
30+
31+
private:
32+
KalmanFilter ekf_;
33+
rclcpp::Time last_update_time_;
34+
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, WZ = 4 };
35+
36+
struct EkfParams
37+
{
38+
// dimension
39+
char dim_x = 5;
40+
// system noise
41+
double q_cov_x;
42+
double q_cov_y;
43+
double q_cov_yaw;
44+
double q_cov_vx;
45+
double q_cov_wz;
46+
// measurement noise
47+
double r_cov_x;
48+
double r_cov_y;
49+
double r_cov_yaw;
50+
double r_cov_vx;
51+
// initial state covariance
52+
double p0_cov_x;
53+
double p0_cov_y;
54+
double p0_cov_yaw;
55+
double p0_cov_vx;
56+
double p0_cov_wz;
57+
};
58+
static EkfParams ekf_params_;
59+
60+
// limitation
61+
static double max_vx_;
62+
// rough tracking parameters
63+
float z_;
64+
65+
// lpf parameter
66+
static double filter_tau_; // time constant of 1st order low pass filter
67+
static double filter_dt_; // sampling time of 1st order low pass filter
68+
69+
// static flags
70+
static bool is_initialized_;
71+
static bool trust_yaw_input_;
72+
static bool trust_twist_input_;
73+
static bool use_polar_coordinate_in_measurement_noise_;
74+
static bool assume_zero_yaw_rate_;
75+
76+
private:
77+
struct BoundingBox
78+
{
79+
double length;
80+
double width;
81+
double height;
82+
};
83+
struct Cylinder
84+
{
85+
double width;
86+
double height;
87+
};
88+
BoundingBox bounding_box_;
89+
Cylinder cylinder_;
90+
91+
public:
92+
ConstantTurnRateMotionTracker(
93+
const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
94+
const std::string & tracker_param_file, const std::uint8_t & label);
95+
96+
static void loadDefaultModelParameters(const std::string & path);
97+
bool predict(const rclcpp::Time & time) override;
98+
bool predict(const double dt, KalmanFilter & ekf) const;
99+
bool measure(
100+
const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
101+
const geometry_msgs::msg::Transform & self_transform) override;
102+
bool measureWithPose(
103+
const autoware_auto_perception_msgs::msg::DetectedObject & object,
104+
const geometry_msgs::msg::Transform & self_transform);
105+
bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
106+
bool getTrackedObject(
107+
const rclcpp::Time & time,
108+
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
109+
virtual ~ConstantTurnRateMotionTracker() {}
110+
};
111+
112+
#endif // RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_

‎perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp

+16-7
Original file line numberDiff line numberDiff line change
@@ -56,18 +56,25 @@ class LinearMotionTracker : public Tracker
5656
double p0_cov_vy;
5757
double p0_cov_ax;
5858
double p0_cov_ay;
59-
} ekf_params_;
59+
};
60+
static EkfParams ekf_params_;
6061

6162
// limitation
62-
double max_vx_;
63-
double max_vy_;
63+
static double max_vx_;
64+
static double max_vy_;
6465
// rough tracking parameters
6566
float z_;
6667
float yaw_;
6768

6869
// lpf parameter
69-
double filter_tau_; // time constant of 1st order low pass filter
70-
double filter_dt_; // sampling time of 1st order low pass filter
70+
static double filter_tau_; // time constant of 1st order low pass filter
71+
static double filter_dt_; // sampling time of 1st order low pass filter
72+
73+
static bool is_initialized_;
74+
static bool estimate_acc_;
75+
static bool trust_yaw_input_;
76+
static bool trust_twist_input_;
77+
static bool use_polar_coordinate_in_measurement_noise_;
7178

7279
private:
7380
struct BoundingBox
@@ -89,13 +96,15 @@ class LinearMotionTracker : public Tracker
8996
const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
9097
const std::string & tracker_param_file, const std::uint8_t & label);
9198

92-
void loadDefaultModelParameters(const std::string & path);
99+
static void loadDefaultModelParameters(const std::string & path);
93100
bool predict(const rclcpp::Time & time) override;
94101
bool predict(const double dt, KalmanFilter & ekf) const;
95102
bool measure(
96103
const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
97104
const geometry_msgs::msg::Transform & self_transform) override;
98-
bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
105+
bool measureWithPose(
106+
const autoware_auto_perception_msgs::msg::DetectedObject & object,
107+
const geometry_msgs::msg::Transform & self_transform);
99108
bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
100109
bool getTrackedObject(
101110
const rclcpp::Time & time,

‎perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#ifndef RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_
1616
#define RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_
1717

18+
#include "model/constant_turn_rate_motion_tracker.hpp"
1819
#include "model/linear_motion_tracker.hpp"
1920
#include "model/tracker_base.hpp"
2021

‎perception/radar_object_tracker/launch/radar_object_tracker.launch.xml

+3-1
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,9 @@
44
<arg name="output" default="objects"/>
55
<arg name="world_frame_id" default="map"/>
66
<arg name="publish_rate" default="10.0"/>
7-
<arg name="enable_delay_compensation" default="false"/>
7+
<arg name="enable_delay_compensation" default="true"/>
88
<arg name="tracker_setting_path" default="$(find-pkg-share radar_object_tracker)/config/default_tracker.param.yaml"/>
9+
<arg name="tracking_config_directory" default="$(find-pkg-share radar_object_tracker)/config/tracking/"/>
910
<arg name="data_association_matrix_path" default="$(find-pkg-share radar_object_tracker)/config/data_association_matrix.param.yaml"/>
1011
<arg name="radar_object_tracker_param_path" default="$(find-pkg-share radar_object_tracker)/config/radar_object_tracker.param.yaml"/>
1112
<arg name="vector_map_topic" default="/map/vector_map"/>
@@ -17,6 +18,7 @@
1718
<param from="$(var tracker_setting_path)"/>
1819
<param from="$(var data_association_matrix_path)"/>
1920
<param from="$(var radar_object_tracker_param_path)"/>
21+
<param name="tracking_config_directory" value="$(var tracking_config_directory)"/>
2022
<param name="world_frame_id" value="$(var world_frame_id)"/>
2123
<param name="publish_rate" value="$(var publish_rate)"/>
2224
<param name="enable_delay_compensation" value="$(var enable_delay_compensation)"/>

‎perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp

+6-2
Original file line numberDiff line numberDiff line change
@@ -207,6 +207,7 @@ RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_
207207
// Parameters
208208
tracker_lifetime_ = declare_parameter<double>("tracker_lifetime");
209209
double publish_rate = declare_parameter<double>("publish_rate");
210+
measurement_count_threshold_ = declare_parameter<int>("measurement_count_threshold");
210211
world_frame_id_ = declare_parameter<std::string>("world_frame_id");
211212
bool enable_delay_compensation{declare_parameter<bool>("enable_delay_compensation")};
212213
tracker_config_directory_ = declare_parameter<std::string>("tracking_config_directory");
@@ -363,6 +364,10 @@ std::shared_ptr<Tracker> RadarObjectTrackerNode::createNewTracker(
363364
if (tracker == "linear_motion_tracker") {
364365
std::string config_file = tracker_config_directory_ + "linear_motion_tracker.yaml";
365366
return std::make_shared<LinearMotionTracker>(time, object, config_file, label);
367+
} else if (tracker == "constant_turn_rate_motion_tracker") {
368+
std::string config_file =
369+
tracker_config_directory_ + "constant_turn_rate_motion_tracker.yaml";
370+
return std::make_shared<ConstantTurnRateMotionTracker>(time, object, config_file, label);
366371
} else {
367372
// not implemented yet so put warning
368373
RCLCPP_WARN(get_logger(), "Tracker %s is not implemented yet", tracker.c_str());
@@ -530,8 +535,7 @@ void RadarObjectTrackerNode::sanitizeTracker(
530535
inline bool RadarObjectTrackerNode::shouldTrackerPublish(
531536
const std::shared_ptr<const Tracker> tracker) const
532537
{
533-
constexpr int measurement_count_threshold = 3;
534-
if (tracker->getTotalMeasurementCount() < measurement_count_threshold) {
538+
if (tracker->getTotalMeasurementCount() < measurement_count_threshold_) {
535539
return false;
536540
}
537541
return true;

0 commit comments

Comments
 (0)