refactor(multi_object_tracker): internal message driven process

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp

@@ -36,6 +36,16 @@
 
 namespace autoware::multi_object_tracker
 {
+struct AssociatorConfig
+{
+  std::vector<int> can_assign_matrix;
+  std::vector<double> max_dist_matrix;
+  std::vector<double> max_area_matrix;
+  std::vector<double> min_area_matrix;
+  std::vector<double> max_rad_matrix;
+  std::vector<double> min_iou_matrix;
+};
+
 class DataAssociation
 {
 private:
@@ -50,17 +60,20 @@
 
 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);
+  explicit DataAssociation(const AssociatorConfig & config);
+  virtual ~DataAssociation() {}
+
   void assign(
     const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
     std::unordered_map<int, int> & reverse_assignment);
+
+  double calculateScore(
+    const types::DynamicObject & tracked_object, const std::uint8_t tracker_label,
+    const types::DynamicObject & measurement_object, const std::uint8_t measurement_label) const;
+
   Eigen::MatrixXd calcScoreMatrix(
     const types::DynamicObjectList & measurements,
     const std::list<std::shared_ptr<Tracker>> & trackers);
-  virtual ~DataAssociation() {}
 };
 
 }  // namespace autoware::multi_object_tracker

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp

@@ -11,9 +11,6 @@
 // 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 Taekjin Lee
 
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_
@@ -40,13 +37,13 @@ bool convertConvexHullToBoundingBox(
 bool getMeasurementYaw(
   const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw);
 
-int getNearestCornerOrSurface(
-  const double x, const double y, const double yaw, const double width, const double length,
-  const geometry_msgs::msg::Transform & self_transform);
+geometry_msgs::msg::Point getNearestCornerOrSurface(
+  const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform);
 
 void calcAnchorPointOffset(
-  const double w, const double l, const int indx, const types::DynamicObject & input_object,
-  const double & yaw, types::DynamicObject & offset_object, Eigen::Vector2d & tracking_offset);
+  const types::DynamicObject & this_object, const types::DynamicObject & input_object,
+  const geometry_msgs::msg::Point anchor_vector, Eigen::Vector2d & tracking_offset,
+  types::DynamicObject & offset_object);
 }  // namespace shapes
 }  // namespace autoware::multi_object_tracker
 

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp

@@ -11,9 +11,6 @@
 // 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 Taekjin Lee
 
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_
@@ -25,6 +22,7 @@
 #include <autoware_perception_msgs/msg/shape.hpp>
 #include <autoware_perception_msgs/msg/tracked_object.hpp>
 #include <autoware_perception_msgs/msg/tracked_object_kinematics.hpp>
+#include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/polygon.hpp>
 #include <geometry_msgs/msg/pose_with_covariance.hpp>
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
@@ -34,12 +32,27 @@
 
 #include <boost/optional.hpp>
 
+#include <string>
 #include <vector>
 
 namespace autoware::multi_object_tracker
 {
 namespace types
 {
+// constants
+constexpr size_t max_channel_size = 16;
+
+// channel configuration
+struct InputChannel
+{
+  uint index;               // index of the channel
+  std::string input_topic;  // topic name of the detection, e.g. "/detection/lidar"
+  std::string long_name = "Detected Object";  // full name of the detection
+  std::string short_name = "DET";             // abbreviation of the name
+  bool is_spawn_enabled = true;               // enable spawn of the object
+};
+
+// object model
 enum OrientationAvailability : uint8_t {
   UNAVAILABLE = 0,
   SIGN_UNKNOWN = 1,
@@ -48,8 +61,6 @@ enum OrientationAvailability : uint8_t {
 
 struct ObjectKinematics
 {
-  geometry_msgs::msg::PoseWithCovariance pose_with_covariance;
-  geometry_msgs::msg::TwistWithCovariance twist_with_covariance;
   bool has_position_covariance = false;
   OrientationAvailability orientation_availability;
   bool has_twist = false;
@@ -58,12 +69,27 @@ struct ObjectKinematics
 
 struct DynamicObject
 {
-  unique_identifier_msgs::msg::UUID object_id = unique_identifier_msgs::msg::UUID();
+  // identification
+  unique_identifier_msgs::msg::UUID uuid = unique_identifier_msgs::msg::UUID();
+
+  // existence information
   uint channel_index;
   float existence_probability;
+  std::vector<float> existence_probabilities;
+
+  // object classification
   std::vector<autoware_perception_msgs::msg::ObjectClassification> classification;
+
+  // object kinematics (pose and twist)
   ObjectKinematics kinematics;
+  geometry_msgs::msg::Pose pose;
+  std::array<double, 36> pose_covariance;
+  geometry_msgs::msg::Twist twist;
+  std::array<double, 36> twist_covariance;
+
+  // object extension (size and shape)
   autoware_perception_msgs::msg::Shape shape;
+  geometry_msgs::msg::Point anchor_point;
 };
 
 struct DynamicObjectList
@@ -82,7 +108,6 @@ DynamicObjectList toDynamicObjectList(
 autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object);
 
 }  // namespace types
-
 }  // namespace autoware::multi_object_tracker
 
 #endif  // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp

@@ -32,27 +32,15 @@ namespace autoware::multi_object_tracker
 class BicycleTracker : public Tracker
 {
 private:
-  types::DynamicObject object_;
   rclcpp::Logger logger_;
 
   object_model::ObjectModel object_model_ = object_model::bicycle;
 
-  double z_;
-
-  struct BoundingBox
-  {
-    double length;
-    double width;
-    double height;
-  };
-  BoundingBox bounding_box_;
-
   BicycleMotionModel motion_model_;
   using IDX = BicycleMotionModel::IDX;
 
 public:
-  BicycleTracker(
-    const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size);
+  BicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp

@@ -36,8 +36,7 @@ class MultipleVehicleTracker : public Tracker
   VehicleTracker big_vehicle_tracker_;
 
 public:
-  MultipleVehicleTracker(
-    const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size);
+  MultipleVehicleTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp

@@ -30,14 +30,12 @@ namespace autoware::multi_object_tracker
 class PassThroughTracker : public Tracker
 {
 private:
-  types::DynamicObject object_;
   types::DynamicObject prev_observed_object_;
   rclcpp::Logger logger_;
   rclcpp::Time last_update_time_;
 
 public:
-  PassThroughTracker(
-    const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size);
+  PassThroughTracker(const rclcpp::Time & time, const types::DynamicObject & object);
   bool predict(const rclcpp::Time & time) override;
   bool measure(
     const types::DynamicObject & object, const rclcpp::Time & time,

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp

@@ -36,8 +36,7 @@ class PedestrianAndBicycleTracker : public Tracker
   BicycleTracker bicycle_tracker_;
 
 public:
-  PedestrianAndBicycleTracker(
-    const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size);
+  PedestrianAndBicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp

@@ -32,33 +32,16 @@ namespace autoware::multi_object_tracker
 class PedestrianTracker : public Tracker
 {
 private:
-  types::DynamicObject object_;
   rclcpp::Logger logger_;
 
   object_model::ObjectModel object_model_ = object_model::pedestrian;
 
-  double z_;
-
-  struct BoundingBox
-  {
-    double length;
-    double width;
-    double height;
-  };
-  struct Cylinder
-  {
-    double width;
-    double height;
-  };
-  BoundingBox bounding_box_;
-  Cylinder cylinder_;
   // cspell: ignore CTRV
   CTRVMotionModel motion_model_;
   using IDX = CTRVMotionModel::IDX;
 
 public:
-  PedestrianTracker(
-    const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size);
+  PedestrianTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp

@@ -20,6 +20,7 @@
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_
 
 #define EIGEN_MPL2_ONLY
+#include "autoware/multi_object_tracker/object_model/object_model.hpp"
 #include "autoware/multi_object_tracker/object_model/types.hpp"
 
 #include <Eigen/Core>
@@ -39,11 +40,6 @@ namespace autoware::multi_object_tracker
 class Tracker
 {
 private:
-  unique_identifier_msgs::msg::UUID uuid_;
-
-  // classification
-  std::vector<autoware_perception_msgs::msg::ObjectClassification> classification_;
-
   // existence states
   int no_measurement_count_;
   int total_no_measurement_count_;
@@ -53,10 +49,7 @@ class Tracker
   float total_existence_probability_;
 
 public:
-  Tracker(
-    const rclcpp::Time & time,
-    const std::vector<autoware_perception_msgs::msg::ObjectClassification> & classification,
-    const size_t & channel_size);
+  Tracker(const rclcpp::Time & time, const types::DynamicObject & object);
   virtual ~Tracker() = default;
 
   void initializeExistenceProbabilities(
@@ -68,17 +61,12 @@ class Tracker
   }
   bool updateWithMeasurement(
     const types::DynamicObject & object, const rclcpp::Time & measurement_time,
-    const geometry_msgs::msg::Transform & self_transform);
+    const geometry_msgs::msg::Transform & self_transform, const types::InputChannel & channel_info);
   bool updateWithoutMeasurement(const rclcpp::Time & now);
 
-  // classification
-  std::vector<autoware_perception_msgs::msg::ObjectClassification> getClassification() const
-  {
-    return classification_;
-  }
   std::uint8_t getHighestProbLabel() const
   {
-    return autoware::object_recognition_utils::getHighestProbLabel(classification_);
+    return autoware::object_recognition_utils::getHighestProbLabel(object_.classification);
   }
 
   // existence states
@@ -91,21 +79,14 @@ class Tracker
   }
 
 protected:
-  unique_identifier_msgs::msg::UUID getUUID() const { return uuid_; }
-  void setClassification(
-    const std::vector<autoware_perception_msgs::msg::ObjectClassification> & classification)
-  {
-    classification_ = classification;
-  }
+  types::DynamicObject object_;
+
   void updateClassification(
     const std::vector<autoware_perception_msgs::msg::ObjectClassification> & classification);
 
-  // virtual functions
-public:
-  virtual geometry_msgs::msg::PoseWithCovariance getPoseWithCovariance(
-    const rclcpp::Time & time) const;
+  void limitObjectExtension(const object_model::ObjectModel object_model);
 
-protected:
+  // virtual functions
   virtual bool measure(
     const types::DynamicObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) = 0;

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp

@@ -19,6 +19,7 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_
 
+#include "autoware/multi_object_tracker/object_model/object_model.hpp"
 #include "autoware/multi_object_tracker/object_model/types.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
 #include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp"
@@ -31,25 +32,15 @@ namespace autoware::multi_object_tracker
 class UnknownTracker : public Tracker
 {
 private:
-  types::DynamicObject object_;
   rclcpp::Logger logger_;
 
-  struct EkfParams
-  {
-    double r_cov_x;
-    double r_cov_y;
-    double r_cov_vx;
-    double r_cov_vy;
-  } ekf_params_;
-
-  double z_;
+  object_model::ObjectModel object_model_ = object_model::unknown;
 
   CVMotionModel motion_model_;
   using IDX = CVMotionModel::IDX;
 
 public:
-  UnknownTracker(
-    const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size);
+  UnknownTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(