Skip to content

Commit

Permalink
feat(radar object tracking): add new object tracking package for rada…
Browse files Browse the repository at this point in the history 
…r object (autowarefoundation#4232)

* create new package to track radar object

Signed-off-by: yoshiri <[email protected]>

* rename directory

Signed-off-by: yoshiri <[email protected]>

* add README description

Signed-off-by: yoshiri <[email protected]>

* migrate association from multi object tracker package

Signed-off-by: yoshiri <[email protected]>

* add linear motion tracker node

Signed-off-by: yoshiri <[email protected]>

* move function to util cpp

Signed-off-by: yoshiri <[email protected]>

* fix build error problem

Signed-off-by: yoshiri <[email protected]>

* add yaml parameter loader

Signed-off-by: yoshiri <[email protected]>

* load parameter from config yaml files

Signed-off-by: yoshiri <[email protected]>

* enable to logging association result

Signed-off-by: yoshiri <[email protected]>

* quit using random walk model and give system noise directly

Signed-off-by: yoshiri <[email protected]>

* update README

Signed-off-by: yoshiri <[email protected]>

* disable json logging as a default

Signed-off-by: yoshiri <[email protected]>

---------

Signed-off-by: yoshiri <[email protected]>
  • Loading branch information
@YoshiRi
YoshiRi authored Jul 20, 2023
1 parent 91a8432 commit d0685fc
Show file tree
Hide file tree
Showing 26 changed files with 2,808 additions and 0 deletions.
54 changes: 54 additions & 0 deletions perception/radar_object_tracker/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,54 @@
cmake_minimum_required(VERSION 3.8)
project(radar_object_tracker)

find_package(autoware_cmake REQUIRED)
autoware_package()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

### Find Eigen Dependencies
find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(nlohmann_json REQUIRED) # for debug

include_directories(
SYSTEM
${EIGEN3_INCLUDE_DIR}
)

# Targets
ament_auto_add_library(mu_successive_shortest_path SHARED
src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
)

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/utils/utils.cpp
src/data_association/data_association.cpp
)

target_link_libraries(radar_object_tracker_node
mu_successive_shortest_path
Eigen3::Eigen
yaml-cpp
nlohmann_json::nlohmann_json # for debug
)

rclcpp_components_register_node(radar_object_tracker_node
PLUGIN "RadarObjectTrackerNode"
EXECUTABLE radar_object_tracker
)

# testing
# todo

# Package
ament_auto_package(
INSTALL_TO_SHARE
launch
config
)
76 changes: 76 additions & 0 deletions perception/radar_object_tracker/README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,76 @@
# Radar Object Tracker

## Purpose

This package provides a radar object tracking node that processes sequences of detected objects to assign consistent identities to them and estimate their velocities.

## Inner-workings / Algorithms

This radar object tracker is a combination of data association and tracking algorithms.

<!-- In the future, you can add an overview image here -->
<!-- ![radar_object_tracker_overview](image/radar_object_tracker_overview.svg) -->

### Data Association

The data association algorithm matches detected objects to existing tracks.

### Tracker Models

The tracker models used in this package vary based on the class of the detected object.
See more details in the [models.md](models.md).

<!-- In the future, you can add flowcharts, state transitions, and other details about how this package works. -->

## Inputs / Outputs

### Input

| Name | Type | Description |
| --------- | ----------------------------------------------------- | ---------------- |
| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | Detected objects |

### Output

| Name | Type | Description |
| ---------- | ---------------------------------------------------- | --------------- |
| `~/output` | `autoware_auto_perception_msgs::msg::TrackedObjects` | Tracked objects |

## Parameters

### 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 |

## Assumptions / Known limits

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

## (Optional) Error detection and handling

<!-- In the future, you can add details about how this package detects and handles errors. -->

## (Optional) Performance characterization

<!-- In the future, you can add details about the performance of this package. -->

## (Optional) References/External links

<!-- In the future, you can add references and links to external code used in this package. -->

## (Optional) Future extensions / Unimplemented parts

<!-- In the future, you can add details about planned extensions or unimplemented parts of this package. -->
66 changes: 66 additions & 0 deletions perception/radar_object_tracker/config/data_association_matrix.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
/**:
ros__parameters:
can_assign_matrix:
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN <-Measurement
[1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN <-Tracker
0, 1, 1, 1, 1, 0, 0, 0, #CAR
0, 1, 1, 1, 1, 0, 0, 0, #TRUCK
0, 1, 1, 1, 1, 0, 0, 0, #BUS
0, 1, 1, 1, 1, 0, 0, 0, #TRAILER
0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE
0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE
0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN

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
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
max_area_matrix:
# NOTE: The size of truck is 12 m length x 3 m width.
# NOTE: The size of trailer is 20 m length x 3 m width.
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
[100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, #UNKNOWN
12.10, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR
36.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK
60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS
60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER
2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE
2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE
2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN
min_area_matrix:
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
[ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN
3.600, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #CAR
6.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRUCK
10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #BUS
10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRAILER
0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #MOTORBIKE
0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #BICYCLE
0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100] #PEDESTRIAN
max_rad_matrix: # If value is greater than pi, it will be ignored.
#UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN
[3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN
3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR
3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK
3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS
3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER
3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE
3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE
3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN

min_iou_matrix: # If value is negative, it will be ignored.
#UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
[0.0001, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN
0.1, -0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR
0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK
0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS
0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.0001] #PEDESTRIAN
9 changes: 9 additions & 0 deletions perception/radar_object_tracker/config/default_tracker.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +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"
8 changes: 8 additions & 0 deletions perception/radar_object_tracker/config/simulation_tracker.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
/**:
ros__parameters:
car_tracker: "pass_through_tracker"
truck_tracker: "pass_through_tracker"
bus_tracker: "pass_through_tracker"
pedestrian_tracker: "pass_through_tracker"
bicycle_tracker: "pass_through_tracker"
motorcycle_tracker: "pass_through_tracker"
32 changes: 32 additions & 0 deletions perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
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]
ax: 0.98 # assume 0.1G acceleration noise
ay: 0.98
vx: 0.1 # assume 0.1m/s velocity noise
vy: 0.1
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]
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]
# 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]
66 changes: 66 additions & 0 deletions ...n/radar_object_tracker/include/radar_object_tracker/data_association/data_association.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
// 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.
//
//
// Author: v1.0 Yukihiro Saito
//

#ifndef RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_

#include <list>
#include <memory>
#include <unordered_map>
#include <vector>

#define EIGEN_MPL2_ONLY
#include "object_recognition_utils/object_recognition_utils.hpp"
#include "radar_object_tracker/data_association/solver/gnn_solver.hpp"
#include "radar_object_tracker/tracker/tracker.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>

#include <string>
class DataAssociation
{
private:
Eigen::MatrixXi can_assign_matrix_;
Eigen::MatrixXd max_dist_matrix_;
Eigen::MatrixXd max_area_matrix_;
Eigen::MatrixXd min_area_matrix_;
Eigen::MatrixXd max_rad_matrix_;
Eigen::MatrixXd min_iou_matrix_;
const double score_threshold_;
std::unique_ptr<gnn_solver::GnnSolverInterface> gnn_solver_ptr_;

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
DataAssociation(
std::vector<int> can_assign_vector, std::vector<double> max_dist_vector,
std::vector<double> max_area_vector, std::vector<double> min_area_vector,
std::vector<double> max_rad_vector, std::vector<double> min_iou_vector);
void assign(
const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
std::unordered_map<int, int> & reverse_assignment);
Eigen::MatrixXd calcScoreMatrix(
const autoware_auto_perception_msgs::msg::DetectedObjects & measurements,
const std::list<std::shared_ptr<Tracker>> & trackers, const bool debug_log,
const std::string & file_name);
virtual ~DataAssociation() {}
};

#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
22 changes: 22 additions & 0 deletions .../radar_object_tracker/include/radar_object_tracker/data_association/solver/gnn_solver.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
// Copyright 2021 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__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_

#include "radar_object_tracker/data_association/solver/gnn_solver_interface.hpp"
#include "radar_object_tracker/data_association/solver/mu_successive_shortest_path.hpp"
#include "radar_object_tracker/data_association/solver/successive_shortest_path.hpp"

#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
35 changes: 35 additions & 0 deletions ...ect_tracker/include/radar_object_tracker/data_association/solver/gnn_solver_interface.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
// Copyright 2021 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__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
#define RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_

#include <unordered_map>
#include <vector>

namespace gnn_solver
{
class GnnSolverInterface
{
public:
GnnSolverInterface() = default;
virtual ~GnnSolverInterface() = default;

virtual void maximizeLinearAssignment(
const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
std::unordered_map<int, int> * reverse_assignment) = 0;
};
} // namespace gnn_solver

#endif // RADAR_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
Loading

0 comments on commit d0685fc

Please sign in to comment.