Skip to content

Commit

Permalink
Refractored the parameters, build the schema file, updated the readme…
Browse files Browse the repository at this point in the history
… file.
  • Loading branch information
tby-udel committed Jun 6, 2024
1 parent df99718 commit edd0a93
Show file tree
Hide file tree
Showing 37 changed files with 583 additions and 264 deletions.
46 changes: 11 additions & 35 deletions perception/multi_object_tracker/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ This multi object tracker consists of data association and EKF.

![multi_object_tracker_overview](image/multi_object_tracker_overview.svg)


### Data association

The data association performs maximum score matching, called min cost max flow problem.
Expand Down Expand Up @@ -57,49 +58,24 @@ Multiple inputs are pre-defined in the input channel parameters (described below

### Output

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

## Parameters

### Input Channel parameters

Available input channels are defined in [input_channels.param.yaml](config/input_channels.param.yaml).

| Name | Type | Description |
| --------------------------------- | ------------------------------------------------ | ------------------------------------- |
| `<channel>` | | the name of channel |
| `<channel>.topic` | `autoware_perception_msgs::msg::DetectedObjects` | detected objects |
| `<channel>.can_spawn_new_tracker` | `bool` | a switch allow to spawn a new tracker |
| `<channel>.optional.name` | `std::string` | channel name for analysis |
| `<channel>.optional.short_name` | `std::string` | short name for visualization |
{{ json_to_markdown("perception/multi_object_tracker/schema/input_channels.schema.json") }}

### Core Parameters

Node parameters are defined in [multi_object_tracker_node.param.yaml](config/multi_object_tracker_node.param.yaml) and association parameters are defined in [data_association_matrix.param.yaml](config/data_association_matrix.param.yaml).

#### Node parameters

| Name | Type | Description |
| --------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------- |
| `***_tracker` | string | EKF tracker name for each class |
| `world_frame_id` | double | object kinematics definition frame |
| `enable_delay_compensation` | bool | if True, tracker use timers to schedule publishers and use prediction step to extrapolate object state at desired timestamp |
| `publish_rate` | double | Timer frequency to output with delay compensation |
| `publish_processing_time` | bool | enable to publish debug message of process time information |
| `publish_tentative_objects` | bool | enable to publish tentative tracked objects, which have lower confidence |
| `publish_debug_markers` | bool | enable to publish debug markers, which indicates association of multi-inputs, existence probability of each detection |

#### Association parameters

| Name | Type | Description |
| ------------------- | ------ | ------------------------------------------- |
| `can_assign_matrix` | double | Assignment table for data association |
| `max_dist_matrix` | double | Maximum distance table for data association |
| `max_area_matrix` | double | Maximum area table for data association |
| `min_area_matrix` | double | Minimum area table for data association |
| `max_rad_matrix` | double | Maximum angle table for data association |
{{ json_to_markdown("perception/multi_object_tracker/schema/multi_object_tracker_node.schema.json") }}
{{ json_to_markdown("perception/multi_object_tracker/schema/data_association_matrix.schema.json") }}

#### Simulation parameters

{{ json_to_markdown("perception/multi_object_tracker/schema/simulation_tracker.schema.json") }}

## Assumptions / Known limits

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <autoware_perception_msgs/msg/detected_objects.hpp>
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>

class DataAssociation
{
Expand All @@ -55,7 +55,7 @@ class DataAssociation
const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
std::unordered_map<int, int> & reverse_assignment);
Eigen::MatrixXd calcScoreMatrix(
const autoware_perception_msgs::msg::DetectedObjects & measurements,
const autoware_auto_perception_msgs::msg::DetectedObjects & measurements,
const std::list<std::shared_ptr<Tracker>> & trackers);
virtual ~DataAssociation() {}
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,8 @@
#include <tier4_autoware_utils/ros/uuid_helper.hpp>

#include "unique_identifier_msgs/msg/uuid.hpp"
#include <autoware_perception_msgs/msg/detected_objects.hpp>
#include <autoware_perception_msgs/msg/tracked_objects.hpp>
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

Expand Down Expand Up @@ -87,7 +87,7 @@ class TrackerObjectDebugger
void collect(
const rclcpp::Time & message_time, const std::list<std::shared_ptr<Tracker>> & list_tracker,
const uint & channel_index,
const autoware_perception_msgs::msg::DetectedObjects & detected_objects,
const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects,
const std::unordered_map<int, int> & direct_assignment,
const std::unordered_map<int, int> & reverse_assignment);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,8 @@
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>

#include <autoware_perception_msgs/msg/detected_objects.hpp>
#include <autoware_perception_msgs/msg/tracked_objects.hpp>
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>

#include <list>
Expand Down Expand Up @@ -54,7 +54,7 @@ class TrackerDebugger

// ROS node, publishers
rclcpp::Node & node_;
rclcpp::Publisher<autoware_perception_msgs::msg::TrackedObjects>::SharedPtr
rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
debug_tentative_objects_pub_;
std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_objects_markers_pub_;
Expand All @@ -80,7 +80,7 @@ class TrackerDebugger
// Object publishing
bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; }
void publishTentativeObjects(
const autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const;
const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const;

// Time measurement
void startMeasurementTime(
Expand All @@ -98,7 +98,7 @@ class TrackerDebugger
void collectObjectInfo(
const rclcpp::Time & message_time, const std::list<std::shared_ptr<Tracker>> & list_tracker,
const uint & channel_index,
const autoware_perception_msgs::msg::DetectedObjects & detected_objects,
const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects,
const std::unordered_map<int, int> & direct_assignment,
const std::unordered_map<int, int> & reverse_assignment);
void publishObjectsMarkers();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@

#include <rclcpp/rclcpp.hpp>

#include <autoware_perception_msgs/msg/detected_objects.hpp>
#include <autoware_perception_msgs/msg/tracked_objects.hpp>
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>

#include <tf2/LinearMath/Transform.h>
Expand All @@ -55,9 +55,9 @@
namespace multi_object_tracker
{

using DetectedObject = autoware_perception_msgs::msg::DetectedObject;
using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects;
using TrackedObjects = autoware_perception_msgs::msg::TrackedObjects;
using DetectedObject = autoware_auto_perception_msgs::msg::DetectedObject;
using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects;
using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects;

class MultiObjectTracker : public rclcpp::Node
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@

#include "rclcpp/rclcpp.hpp"

#include "autoware_perception_msgs/msg/detected_objects.hpp"
#include "autoware_auto_perception_msgs/msg/detected_objects.hpp"

#include <deque>
#include <functional>
Expand All @@ -28,7 +28,7 @@

namespace multi_object_tracker
{
using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects;
using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects;
using ObjectsList = std::vector<std::pair<uint, DetectedObjects>>;

struct InputChannel
Expand All @@ -52,7 +52,7 @@ class InputStream
func_trigger_ = func_trigger;
}

void onMessage(const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
void updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time);

bool isTimeInitialized() const { return initial_count_ > 0; }
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@

#include <rclcpp/rclcpp.hpp>

#include <autoware_perception_msgs/msg/detected_objects.hpp>
#include <autoware_perception_msgs/msg/tracked_objects.hpp>
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>

#include <list>
#include <map>
Expand All @@ -39,11 +39,11 @@ class TrackerProcessor
// tracker processes
void predict(const rclcpp::Time & time);
void update(
const autoware_perception_msgs::msg::DetectedObjects & transformed_objects,
const autoware_auto_perception_msgs::msg::DetectedObjects & transformed_objects,
const geometry_msgs::msg::Transform & self_transform,
const std::unordered_map<int, int> & direct_assignment, const uint & channel_index);
void spawn(
const autoware_perception_msgs::msg::DetectedObjects & detected_objects,
const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects,
const geometry_msgs::msg::Transform & self_transform,
const std::unordered_map<int, int> & reverse_assignment, const uint & channel_index);
void prune(const rclcpp::Time & time);
Expand All @@ -52,10 +52,10 @@ class TrackerProcessor
bool isConfidentTracker(const std::shared_ptr<Tracker> & tracker) const;
void getTrackedObjects(
const rclcpp::Time & time,
autoware_perception_msgs::msg::TrackedObjects & tracked_objects) const;
autoware_auto_perception_msgs::msg::TrackedObjects & tracked_objects) const;
void getTentativeObjects(
const rclcpp::Time & time,
autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const;
autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const;

void getExistenceProbabilities(std::vector<std::vector<float>> & existence_vectors) const;

Expand All @@ -74,7 +74,7 @@ class TrackerProcessor
void removeOldTracker(const rclcpp::Time & time);
void removeOverlappedTracker(const rclcpp::Time & time);
std::shared_ptr<Tracker> createNewTracker(
const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const;
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
class BicycleTracker : public Tracker
{
private:
autoware_perception_msgs::msg::DetectedObject object_;
autoware_auto_perception_msgs::msg::DetectedObject object_;
rclcpp::Logger logger_;

private:
Expand Down Expand Up @@ -55,22 +55,22 @@ class BicycleTracker : public Tracker

public:
BicycleTracker(
const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object,
const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
const uint & channel_index);

bool predict(const rclcpp::Time & time) override;
bool measure(
const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const geometry_msgs::msg::Transform & self_transform) override;
autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_perception_msgs::msg::DetectedObject & object,
autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform);
bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool getTrackedObject(
const rclcpp::Time & time,
autoware_perception_msgs::msg::TrackedObject & object) const override;
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
virtual ~BicycleTracker() {}
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
class BigVehicleTracker : public Tracker
{
private:
autoware_perception_msgs::msg::DetectedObject object_;
autoware_auto_perception_msgs::msg::DetectedObject object_;
rclcpp::Logger logger_;

private:
Expand Down Expand Up @@ -60,22 +60,22 @@ class BigVehicleTracker : public Tracker

public:
BigVehicleTracker(
const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object,
const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
const uint & channel_index);

bool predict(const rclcpp::Time & time) override;
bool measure(
const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const geometry_msgs::msg::Transform & self_transform) override;
autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_perception_msgs::msg::DetectedObject & object,
autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform);
bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool getTrackedObject(
const rclcpp::Time & time,
autoware_perception_msgs::msg::TrackedObject & object) const override;
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
virtual ~BigVehicleTracker() {}

private:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,17 +34,17 @@ class MultipleVehicleTracker : public Tracker

public:
MultipleVehicleTracker(
const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object,
const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
const uint & channel_index);

bool predict(const rclcpp::Time & time) override;
bool measure(
const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const geometry_msgs::msg::Transform & self_transform) override;
bool getTrackedObject(
const rclcpp::Time & time,
autoware_perception_msgs::msg::TrackedObject & object) const override;
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
virtual ~MultipleVehicleTracker() {}
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
class NormalVehicleTracker : public Tracker
{
private:
autoware_perception_msgs::msg::DetectedObject object_;
autoware_auto_perception_msgs::msg::DetectedObject object_;
rclcpp::Logger logger_;

private:
Expand Down Expand Up @@ -60,22 +60,22 @@ class NormalVehicleTracker : public Tracker

public:
NormalVehicleTracker(
const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object,
const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
const uint & channel_index);

bool predict(const rclcpp::Time & time) override;
bool measure(
const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
const geometry_msgs::msg::Transform & self_transform) override;
autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_perception_msgs::msg::DetectedObject & object,
autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const geometry_msgs::msg::Transform & self_transform);
bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object);
bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object);
bool getTrackedObject(
const rclcpp::Time & time,
autoware_perception_msgs::msg::TrackedObject & object) const override;
autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
virtual ~NormalVehicleTracker() {}

private:
Expand Down
Loading

0 comments on commit edd0a93

Please sign in to comment.