refactor(multi_object_tracker): internal message driven process (#10203)

* refactor(multi_object_tracker): streamline input channel configuration handling

feat(multi_object_tracker): introduce InputChannel struct for input channel configuration

refactor(multi_object_tracker): improve marker handling and initialization in TrackerObjectDebugger

feat(multi_object_tracker): enhance InputChannel with trust flags for object properties

refactor(multi_object_tracker): remove unused channel_size parameter from tracker constructors

feat(multi_object_tracker): update InputChannel flags to trust object extension and classification

fix(multi_object_tracker): replace channel.index with channel_index for consistency

feat(multi_object_tracker): update TrackerObjectDebugger and TrackerProcessor to accept channels_config parameter

refactor(multi_object_tracker): remove redundant existence probability initialization from tracker constructors

feat(multi_object_tracker): integrate data association into TrackerProcessor and add associate method

feat(multi_object_tracker): enhance updateWithMeasurement to include channel_info for improved classification handling

refactor(multi_object_tracker): replace object_id with uuid in DynamicObject and related classes

fix(multi_object_tracker): update UUID handling in Tracker to use uuid_msg for consistency

refactor(multi_object_tracker): simplify pose and covariance handling in tracker classes

refactor(multi_object_tracker): replace pose_with_covariance with separate pose and covariance attributes in DynamicObject

refactor: remove z state from tracker. it will uses object state

refactor(multi_object_tracker): streamline object handling in trackers and remove unnecessary shape processing

refactor(multi_object_tracker): remove z position handling from trackers and update object kinematics structure

refactor(multi_object_tracker): remove BoundingBox structure from trackers and implement object extension limits

refactor(multi_object_tracker): remove unnecessary blank lines in tracker getTrackedObject methods

refactor(multi_object_tracker): simplify input channel configuration by removing trust flags and consolidating parameters

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* refactor(multi_object_tracker): use const reference in loop and simplify tracker update logic

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* refactor(multi_object_tracker): update shape handling and streamline object tracking logic

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* refactor(multi_object_tracker): update shape handling to use geometry_msgs::msg::Point for anchor vectors

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* style(pre-commit): autofix

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* refactor(multi_object_tracker): modify getNearestCornerOrSurface function signature and update related logic

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

refactor(multi_object_tracker): remove self_transform parameter from measure and update methods

refactor(multi_object_tracker): update calcAnchorPointOffset function signature and streamline object handling

refactor(multi_object_tracker): set shape type to BOUNDING_BOX for object trackers

---------

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: technolojin

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 @@ class DataAssociation
 
 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,12 @@ 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);
+void getNearestCornerOrSurface(
+  const geometry_msgs::msg::Transform & self_transform, types::DynamicObject & object);
 
 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, 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,40 +32,21 @@ 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(
-    const types::DynamicObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) override;
+  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
   bool measureWithPose(const types::DynamicObject & object);
   bool measureWithShape(const types::DynamicObject & object);
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
-
-private:
-  types::DynamicObject getUpdatingObject(
-    const types::DynamicObject & object,
-    const geometry_msgs::msg::Transform & self_transform) const;
 };
 
 }  // namespace autoware::multi_object_tracker

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

@@ -36,13 +36,10 @@ 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(
-    const types::DynamicObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) override;
+  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
   virtual ~MultipleVehicleTracker() {}
 };

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

@@ -30,18 +30,14 @@ 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,
-    const geometry_msgs::msg::Transform & self_transform) override;
+  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
 };
 

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

@@ -36,13 +36,10 @@ 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(
-    const types::DynamicObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) override;
+  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
   virtual ~PedestrianAndBicycleTracker() {}
 };

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

@@ -32,46 +32,22 @@ 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(
-    const types::DynamicObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) override;
+  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
   bool measureWithPose(const types::DynamicObject & object);
   bool measureWithShape(const types::DynamicObject & object);
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
-
-private:
-  types::DynamicObject getUpdatingObject(
-    const types::DynamicObject & object,
-    const geometry_msgs::msg::Transform & self_transform) const;
 };
 
 }  // namespace autoware::multi_object_tracker

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 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,24 +79,15 @@ 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 bool measure(
-    const types::DynamicObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) = 0;
+  // virtual functions
+  virtual bool measure(const types::DynamicObject & object, const rclcpp::Time & time) = 0;
 
 public:
   virtual bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const = 0;