Skip to content

Commit d0685fc

Browse files
YoshiRiYoshiRi
authored
feat(radar object tracking): add new object tracking package for radar object (autowarefoundation#4232)
* create new package to track radar object Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * rename directory Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * add README description Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * migrate association from multi object tracker package Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * add linear motion tracker node Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * move function to util cpp Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix build error problem Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * add yaml parameter loader Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * load parameter from config yaml files Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * enable to logging association result Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * quit using random walk model and give system noise directly Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * update README Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * disable json logging as a default Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> --------- Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
1 parent 91a8432 commit d0685fc

26 files changed

+2808
-0
lines changed

‎perception/radar_object_tracker/CMakeLists.txt

+54
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(radar_object_tracker)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
8+
add_compile_options(-Wall -Wextra -Wpedantic)
9+
endif()
10+
11+
### Find Eigen Dependencies
12+
find_package(eigen3_cmake_module REQUIRED)
13+
find_package(Eigen3 REQUIRED)
14+
find_package(nlohmann_json REQUIRED) # for debug
15+
16+
include_directories(
17+
SYSTEM
18+
${EIGEN3_INCLUDE_DIR}
19+
)
20+
21+
# Targets
22+
ament_auto_add_library(mu_successive_shortest_path SHARED
23+
src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
24+
)
25+
26+
ament_auto_add_library(radar_object_tracker_node SHARED
27+
src/radar_object_tracker_node/radar_object_tracker_node.cpp
28+
src/tracker/model/tracker_base.cpp
29+
src/tracker/model/linear_motion_tracker.cpp
30+
src/utils/utils.cpp
31+
src/data_association/data_association.cpp
32+
)
33+
34+
target_link_libraries(radar_object_tracker_node
35+
mu_successive_shortest_path
36+
Eigen3::Eigen
37+
yaml-cpp
38+
nlohmann_json::nlohmann_json # for debug
39+
)
40+
41+
rclcpp_components_register_node(radar_object_tracker_node
42+
PLUGIN "RadarObjectTrackerNode"
43+
EXECUTABLE radar_object_tracker
44+
)
45+
46+
# testing
47+
# todo
48+
49+
# Package
50+
ament_auto_package(
51+
INSTALL_TO_SHARE
52+
launch
53+
config
54+
)

‎perception/radar_object_tracker/README.md

+76
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
# Radar Object Tracker
2+
3+
## Purpose
4+
5+
This package provides a radar object tracking node that processes sequences of detected objects to assign consistent identities to them and estimate their velocities.
6+
7+
## Inner-workings / Algorithms
8+
9+
This radar object tracker is a combination of data association and tracking algorithms.
10+
11+
<!-- In the future, you can add an overview image here -->
12+
<!-- ![radar_object_tracker_overview](image/radar_object_tracker_overview.svg) -->
13+
14+
### Data Association
15+
16+
The data association algorithm matches detected objects to existing tracks.
17+
18+
### Tracker Models
19+
20+
The tracker models used in this package vary based on the class of the detected object.
21+
See more details in the [models.md](models.md).
22+
23+
<!-- In the future, you can add flowcharts, state transitions, and other details about how this package works. -->
24+
25+
## Inputs / Outputs
26+
27+
### Input
28+
29+
| Name | Type | Description |
30+
| --------- | ----------------------------------------------------- | ---------------- |
31+
| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | Detected objects |
32+
33+
### Output
34+
35+
| Name | Type | Description |
36+
| ---------- | ---------------------------------------------------- | --------------- |
37+
| `~/output` | `autoware_auto_perception_msgs::msg::TrackedObjects` | Tracked objects |
38+
39+
## Parameters
40+
41+
### Node Parameters
42+
43+
| Name | Type | Default Value | Description |
44+
| --------------------------- | ------ | ----------------------------- | ----------------------------------------------------------- |
45+
| `publish_rate` | double | 30.0 | The rate at which to publish the output messages |
46+
| `world_frame_id` | string | "world" | The frame ID of the world coordinate system |
47+
| `enable_delay_compensation` | bool | false | Whether to enable delay compensation |
48+
| `tracking_config_directory` | string | "" | The directory containing the tracking configuration files |
49+
| `enable_logging` | bool | false | Whether to enable logging |
50+
| `logging_file_path` | string | "~/.ros/association_log.json" | The path to the file where logs should be written |
51+
| `can_assign_matrix` | array | | An array of integers used in the data association algorithm |
52+
| `max_dist_matrix` | array | | An array of doubles used in the data association algorithm |
53+
| `max_area_matrix` | array | | An array of doubles used in the data association algorithm |
54+
| `min_area_matrix` | array | | An array of doubles used in the data association algorithm |
55+
| `max_rad_matrix` | array | | An array of doubles used in the data association algorithm |
56+
| `min_iou_matrix` | array | | An array of doubles used in the data association algorithm |
57+
58+
## Assumptions / Known limits
59+
60+
<!-- In the future, you can add assumptions and known limitations of this package. -->
61+
62+
## (Optional) Error detection and handling
63+
64+
<!-- In the future, you can add details about how this package detects and handles errors. -->
65+
66+
## (Optional) Performance characterization
67+
68+
<!-- In the future, you can add details about the performance of this package. -->
69+
70+
## (Optional) References/External links
71+
72+
<!-- In the future, you can add references and links to external code used in this package. -->
73+
74+
## (Optional) Future extensions / Unimplemented parts
75+
76+
<!-- In the future, you can add details about planned extensions or unimplemented parts of this package. -->

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

+66
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
/**:
2+
ros__parameters:
3+
can_assign_matrix:
4+
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN <-Measurement
5+
[1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN <-Tracker
6+
0, 1, 1, 1, 1, 0, 0, 0, #CAR
7+
0, 1, 1, 1, 1, 0, 0, 0, #TRUCK
8+
0, 1, 1, 1, 1, 0, 0, 0, #BUS
9+
0, 1, 1, 1, 1, 0, 0, 0, #TRAILER
10+
0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE
11+
0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE
12+
0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN
13+
14+
max_dist_matrix:
15+
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
16+
[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
21+
3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #MOTORBIKE
22+
3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE
23+
2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN
24+
max_area_matrix:
25+
# NOTE: The size of truck is 12 m length x 3 m width.
26+
# NOTE: The size of trailer is 20 m length x 3 m width.
27+
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
28+
[100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, #UNKNOWN
29+
12.10, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR
30+
36.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK
31+
60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS
32+
60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER
33+
2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE
34+
2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE
35+
2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN
36+
min_area_matrix:
37+
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
38+
[ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN
39+
3.600, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #CAR
40+
6.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRUCK
41+
10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #BUS
42+
10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRAILER
43+
0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #MOTORBIKE
44+
0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #BICYCLE
45+
0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100] #PEDESTRIAN
46+
max_rad_matrix: # If value is greater than pi, it will be ignored.
47+
#UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN
48+
[3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN
49+
3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR
50+
3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK
51+
3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS
52+
3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER
53+
3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE
54+
3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE
55+
3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN
56+
57+
min_iou_matrix: # If value is negative, it will be ignored.
58+
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
59+
[0.0001, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN
60+
0.1, -0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR
61+
0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK
62+
0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS
63+
0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER
64+
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE
65+
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE
66+
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.0001] #PEDESTRIAN

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

+9
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
/**:
2+
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"

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

+8
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
/**:
2+
ros__parameters:
3+
car_tracker: "pass_through_tracker"
4+
truck_tracker: "pass_through_tracker"
5+
bus_tracker: "pass_through_tracker"
6+
pedestrian_tracker: "pass_through_tracker"
7+
bicycle_tracker: "pass_through_tracker"
8+
motorcycle_tracker: "pass_through_tracker"

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

+32
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
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+
ax: 0.98 # assume 0.1G acceleration noise
9+
ay: 0.98
10+
vx: 0.1 # assume 0.1m/s velocity noise
11+
vy: 0.1
12+
x: 1.0 # assume 1m position noise
13+
y: 1.0
14+
measurement_noise_std:
15+
x: 0.6 # [m]
16+
y: 0.9 # [m]
17+
vx: 0.4 # [m/s]
18+
vy: 1 # [m/s]
19+
initial_covariance_std:
20+
x: 3.0 # [m]
21+
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]
26+
# output limitation
27+
limit:
28+
max_speed: 80.0 # [m/s]
29+
# low pass filter is used to smooth the yaw and shape estimation
30+
low_pass_filter:
31+
time_constant: 1.0 # [s]
32+
sampling_time: 0.1 # [s]

‎perception/radar_object_tracker/include/radar_object_tracker/data_association/data_association.hpp

+66
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
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+
//
16+
// Author: v1.0 Yukihiro Saito
17+
//
18+
19+
#ifndef RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
20+
#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
21+
22+
#include <list>
23+
#include <memory>
24+
#include <unordered_map>
25+
#include <vector>
26+
27+
#define EIGEN_MPL2_ONLY
28+
#include "object_recognition_utils/object_recognition_utils.hpp"
29+
#include "radar_object_tracker/data_association/solver/gnn_solver.hpp"
30+
#include "radar_object_tracker/tracker/tracker.hpp"
31+
32+
#include <Eigen/Core>
33+
#include <Eigen/Geometry>
34+
35+
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
36+
37+
#include <string>
38+
class DataAssociation
39+
{
40+
private:
41+
Eigen::MatrixXi can_assign_matrix_;
42+
Eigen::MatrixXd max_dist_matrix_;
43+
Eigen::MatrixXd max_area_matrix_;
44+
Eigen::MatrixXd min_area_matrix_;
45+
Eigen::MatrixXd max_rad_matrix_;
46+
Eigen::MatrixXd min_iou_matrix_;
47+
const double score_threshold_;
48+
std::unique_ptr<gnn_solver::GnnSolverInterface> gnn_solver_ptr_;
49+
50+
public:
51+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52+
DataAssociation(
53+
std::vector<int> can_assign_vector, std::vector<double> max_dist_vector,
54+
std::vector<double> max_area_vector, std::vector<double> min_area_vector,
55+
std::vector<double> max_rad_vector, std::vector<double> min_iou_vector);
56+
void assign(
57+
const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
58+
std::unordered_map<int, int> & reverse_assignment);
59+
Eigen::MatrixXd calcScoreMatrix(
60+
const autoware_auto_perception_msgs::msg::DetectedObjects & measurements,
61+
const std::list<std::shared_ptr<Tracker>> & trackers, const bool debug_log,
62+
const std::string & file_name);
63+
virtual ~DataAssociation() {}
64+
};
65+
66+
#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_

‎perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver.hpp

+22
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
// Copyright 2021 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__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
16+
#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
17+
18+
#include "radar_object_tracker/data_association/solver/gnn_solver_interface.hpp"
19+
#include "radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp"
20+
#include "radar_object_tracker/data_association/solver/successive_shortest_path.hpp"
21+
22+
#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_

‎perception/radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver_interface.hpp

+35
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
// Copyright 2021 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__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
16+
#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
17+
18+
#include <unordered_map>
19+
#include <vector>
20+
21+
namespace gnn_solver
22+
{
23+
class GnnSolverInterface
24+
{
25+
public:
26+
GnnSolverInterface() = default;
27+
virtual ~GnnSolverInterface() = default;
28+
29+
virtual void maximizeLinearAssignment(
30+
const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
31+
std::unordered_map<int, int> * reverse_assignment) = 0;
32+
};
33+
} // namespace gnn_solver
34+
35+
#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_

0 commit comments

Comments
 (0)