From 8ba56125812d701431ad7bf5720d93472e87fdb3 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 10 Mar 2025 12:00:07 +0900 Subject: [PATCH] refactor(multi_object_tracker): modify getNearestCornerOrSurface function signature and update related logic Signed-off-by: Taekjin LEE 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 --- .../object_model/shapes.hpp | 7 ++--- .../tracker/model/bicycle_tracker.hpp | 9 +----- .../model/multiple_vehicle_tracker.hpp | 4 +-- .../tracker/model/pass_through_tracker.hpp | 4 +-- .../model/pedestrian_and_bicycle_tracker.hpp | 4 +-- .../tracker/model/pedestrian_tracker.hpp | 9 +----- .../tracker/model/tracker_base.hpp | 6 ++-- .../tracker/model/unknown_tracker.hpp | 6 +--- .../tracker/model/vehicle_tracker.hpp | 8 +---- .../lib/object_model/shapes.cpp | 24 +++++++-------- .../lib/tracker/model/bicycle_tracker.cpp | 22 +++++--------- .../model/multiple_vehicle_tracker.cpp | 8 ++--- .../tracker/model/pass_through_tracker.cpp | 5 +--- .../model/pedestrian_and_bicycle_tracker.cpp | 7 ++--- .../lib/tracker/model/pedestrian_tracker.cpp | 27 +++++------------ .../lib/tracker/model/tracker_base.cpp | 4 +-- .../lib/tracker/model/unknown_tracker.cpp | 21 ++++---------- .../lib/tracker/model/vehicle_tracker.cpp | 29 +++++-------------- .../src/multi_object_tracker_node.cpp | 8 +---- .../src/processor/input_manager.cpp | 9 ++++-- .../src/processor/processor.cpp | 4 +-- .../src/processor/processor.hpp | 1 - 22 files changed, 69 insertions(+), 157 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp index 82fc9abf23ce9..5e1de408862e1 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp @@ -37,12 +37,11 @@ bool convertConvexHullToBoundingBox( bool getMeasurementYaw( const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw); -geometry_msgs::msg::Point getNearestCornerOrSurface( - const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); +void getNearestCornerOrSurface( + const geometry_msgs::msg::Transform & self_transform, types::DynamicObject & object); void calcAnchorPointOffset( - const types::DynamicObject & this_object, const types::DynamicObject & input_object, - const geometry_msgs::msg::Point anchor_vector, Eigen::Vector2d & tracking_offset, + const types::DynamicObject & this_object, Eigen::Vector2d & tracking_offset, types::DynamicObject & offset_object); } // namespace shapes } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 2e4449aee529d..5bf5ed23da9e9 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -43,17 +43,10 @@ class BicycleTracker : public Tracker 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 diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 80dbacc1d17d2..82122f34101cd 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -39,9 +39,7 @@ class MultipleVehicleTracker : public Tracker 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() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 938e704212b8d..d0dceabfaac25 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -37,9 +37,7 @@ class PassThroughTracker : public Tracker public: 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; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index abd59557507b2..f0e741ddc1993 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -39,9 +39,7 @@ class PedestrianAndBicycleTracker : public Tracker 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() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index c39809b4d0a17..65495f8b5e108 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -44,17 +44,10 @@ class PedestrianTracker : public Tracker 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 diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index 1bd8086615d40..637969c139cf1 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -61,7 +61,7 @@ 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); + const types::InputChannel & channel_info); bool updateWithoutMeasurement(const rclcpp::Time & now); std::uint8_t getHighestProbLabel() const @@ -87,9 +87,7 @@ class Tracker void limitObjectExtension(const object_model::ObjectModel object_model); // virtual functions - virtual bool measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) = 0; + 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; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index fdb24b8a3c1c8..5fb0a142ecf34 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -43,11 +43,7 @@ class UnknownTracker : public Tracker UnknownTracker(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; - types::DynamicObject getUpdatingObject( - const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); + 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; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index 1f7ba00a7c99d..4c682158a4aec 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -48,16 +48,10 @@ class VehicleTracker : public Tracker 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); }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index 83a6ef2ce9442..ee52e2a354d11 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -171,8 +171,8 @@ enum BBOX_IDX { * @param self_transform: Ego vehicle position in map frame * @return int index */ -geometry_msgs::msg::Point getNearestCornerOrSurface( - const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) +void getNearestCornerOrSurface( + const geometry_msgs::msg::Transform & self_transform, types::DynamicObject & object) { const double x = object.pose.position.x; const double y = object.pose.position.y; @@ -212,24 +212,22 @@ geometry_msgs::msg::Point getNearestCornerOrSurface( } else { anchor_y = -width / 2.0; } - geometry_msgs::msg::Point anchor_point; - anchor_point.x = anchor_x; - anchor_point.y = anchor_y; - return anchor_point; + + object.anchor_point.x = anchor_x; + object.anchor_point.y = anchor_y; } void calcAnchorPointOffset( - 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) + const types::DynamicObject & this_object, Eigen::Vector2d & tracking_offset, + types::DynamicObject & updating_object) { // copy value - offset_object = input_object; + const geometry_msgs::msg::Point anchor_vector = updating_object.anchor_point; // invalid anchor if (anchor_vector.x <= 1e-6 && anchor_vector.y <= 1e-6) { return; } - double input_yaw = tf2::getYaw(input_object.pose.orientation); + double input_yaw = tf2::getYaw(updating_object.pose.orientation); // current object width and height const double length = this_object.shape.dimensions.x; @@ -251,8 +249,8 @@ void calcAnchorPointOffset( // offset input object const Eigen::Matrix2d R = Eigen::Rotation2Dd(input_yaw).toRotationMatrix(); const Eigen::Vector2d rotated_offset = R * tracking_offset; - offset_object.pose.position.x += rotated_offset.x(); - offset_object.pose.position.y += rotated_offset.y(); + updating_object.pose.position.x += rotated_offset.x(); + updating_object.pose.position.y += rotated_offset.y(); } } // namespace shapes diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 354335b4ec0c7..ad7112360267c 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -50,6 +50,8 @@ BicycleTracker::BicycleTracker(const rclcpp::Time & time, const types::DynamicOb object_extension.y = object_model_.init_size.width; object_extension.z = object_model_.init_size.height; } + object_.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + // set maximum and minimum size limitObjectExtension(object_model_); @@ -126,13 +128,6 @@ bool BicycleTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -types::DynamicObject BicycleTracker::getUpdatingObject( - const types::DynamicObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) const -{ - return object; -} - bool BicycleTracker::measureWithPose(const types::DynamicObject & object) { // get measurement yaw angle to update @@ -199,9 +194,7 @@ bool BicycleTracker::measureWithShape(const types::DynamicObject & object) return true; } -bool BicycleTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) +bool BicycleTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) { // check time gap const double dt = motion_model_.getDeltaTime(time); @@ -214,9 +207,8 @@ bool BicycleTracker::measure( } // update object - const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); - measureWithPose(updating_object); - measureWithShape(updating_object); + measureWithPose(object); + measureWithShape(object); return true; } @@ -225,12 +217,12 @@ bool BicycleTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; + + // predict from motion model auto & pose = object.pose; auto & pose_cov = object.pose_covariance; auto & twist = object.twist; auto & twist_cov = object.twist_covariance; - - // predict from motion model if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "BicycleTracker::getTrackedObject: Failed to get predicted state."); return false; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index b23ca7386e1f1..2803f65a84d29 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -37,12 +37,10 @@ bool MultipleVehicleTracker::predict(const rclcpp::Time & time) return true; } -bool MultipleVehicleTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) +bool MultipleVehicleTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) { - big_vehicle_tracker_.measure(object, time, self_transform); - normal_vehicle_tracker_.measure(object, time, self_transform); + big_vehicle_tracker_.measure(object, time); + normal_vehicle_tracker_.measure(object, time); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index 252641a810223..27500ee942d7f 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -52,9 +52,7 @@ bool PassThroughTracker::predict(const rclcpp::Time & time) return true; } -bool PassThroughTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) +bool PassThroughTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) { prev_observed_object_ = object_; object_ = object; @@ -68,7 +66,6 @@ bool PassThroughTracker::measure( } last_update_time_ = time; - (void)self_transform; // currently do not use self vehicle position return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 486cf5e90c1d8..0c28205cc2958 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -34,11 +34,10 @@ bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) } bool PedestrianAndBicycleTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) + const types::DynamicObject & object, const rclcpp::Time & time) { - pedestrian_tracker_.measure(object, time, self_transform); - bicycle_tracker_.measure(object, time, self_transform); + pedestrian_tracker_.measure(object, time); + bicycle_tracker_.measure(object, time); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index 3058d410120c2..6543591bda22d 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -116,13 +116,6 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -types::DynamicObject PedestrianTracker::getUpdatingObject( - const types::DynamicObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) const -{ - return object; -} - bool PedestrianTracker::measureWithPose(const types::DynamicObject & object) { // update motion model @@ -170,16 +163,14 @@ bool PedestrianTracker::measureWithShape(const types::DynamicObject & object) return false; } + // update shape type + object_.shape.type = object.shape.type; + return true; } -bool PedestrianTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) +bool PedestrianTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) { - // keep the latest input object - object_ = object; - // check time gap const double dt = motion_model_.getDeltaTime(time); if (0.01 /*10msec*/ < dt) { @@ -191,11 +182,9 @@ bool PedestrianTracker::measure( } // update object - const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); - measureWithPose(updating_object); - measureWithShape(updating_object); + measureWithPose(object); + measureWithShape(object); - (void)self_transform; // currently do not use self vehicle position return true; } @@ -203,12 +192,12 @@ bool PedestrianTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; + + // predict from motion model auto & pose = object.pose; auto & pose_cov = object.pose_covariance; auto & twist = object.twist; auto & twist_cov = object.twist_covariance; - - // predict from motion model if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "PedestrianTracker::getTrackedObject: Failed to get predicted state."); return false; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp index 0d0dca86a4136..a8a52e59cf079 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -79,7 +79,7 @@ void Tracker::initializeExistenceProbabilities( bool Tracker::updateWithMeasurement( const types::DynamicObject & object, const rclcpp::Time & measurement_time, - const geometry_msgs::msg::Transform & self_transform, const types::InputChannel & channel_info) + const types::InputChannel & channel_info) { // Update existence probability { @@ -120,7 +120,7 @@ bool Tracker::updateWithMeasurement( } // Update object - measure(object, measurement_time, self_transform); + measure(object, measurement_time); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index cd27796557194..fec45f42bce3f 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -121,12 +121,6 @@ bool UnknownTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -types::DynamicObject UnknownTracker::getUpdatingObject( - const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) -{ - return object; -} - bool UnknownTracker::measureWithPose(const types::DynamicObject & object) { // update motion model @@ -146,12 +140,10 @@ bool UnknownTracker::measureWithPose(const types::DynamicObject & object) return is_updated; } -bool UnknownTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) +bool UnknownTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) { - // keep the latest input object - object_ = object; + // update object shape + object_.shape = object.shape; // check time gap const double dt = motion_model_.getDeltaTime(time); @@ -164,8 +156,7 @@ bool UnknownTracker::measure( } // update object - const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); - measureWithPose(updating_object); + measureWithPose(object); return true; } @@ -174,12 +165,12 @@ bool UnknownTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; + + // predict from motion model auto & pose = object.pose; auto & pose_cov = object.pose_covariance; auto & twist = object.twist; auto & twist_cov = object.twist_covariance; - - // predict from motion model if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "UnknownTracker::getTrackedObject: Failed to get predicted state."); return false; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index 8902725222882..9183ce3168ca5 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -60,6 +60,8 @@ VehicleTracker::VehicleTracker( object_extension.y = object_model_.init_size.width; object_extension.z = object_model_.init_size.height; } + object_.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + // set maximum and minimum size limitObjectExtension(object_model_); @@ -140,19 +142,6 @@ bool VehicleTracker::predict(const rclcpp::Time & time) return true; } -types::DynamicObject VehicleTracker::getUpdatingObject( - const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) -{ - types::DynamicObject updating_object = object; - - // get offset measurement - const geometry_msgs::msg::Point anchor_vector = - shapes::getNearestCornerOrSurface(object, self_transform); - shapes::calcAnchorPointOffset(object_, object, anchor_vector, tracking_offset_, updating_object); - - return updating_object; -} - bool VehicleTracker::measureWithPose(const types::DynamicObject & object) { // current (predicted) state @@ -240,13 +229,8 @@ bool VehicleTracker::measureWithShape(const types::DynamicObject & object) return true; } -bool VehicleTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) +bool VehicleTracker::measure(const types::DynamicObject & in_object, const rclcpp::Time & time) { - // keep the latest input object - object_ = object; - // check time gap const double dt = motion_model_.getDeltaTime(time); if (0.01 /*10msec*/ < dt) { @@ -258,7 +242,8 @@ bool VehicleTracker::measure( } // update object - const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); + types::DynamicObject updating_object = in_object; + shapes::calcAnchorPointOffset(object_, tracking_offset_, updating_object); measureWithPose(updating_object); measureWithShape(updating_object); @@ -269,12 +254,12 @@ bool VehicleTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; + + // predict from motion model auto & pose = object.pose; auto & pose_cov = object.pose_covariance; auto & twist = object.twist; auto & twist_cov = object.twist_covariance; - - // predict from motion model if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "VehicleTracker::getTrackedObject: Failed to get predicted state."); return false; diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 5ea6cd1ff790f..bf2c2de9b57e3 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -248,12 +248,6 @@ void MultiObjectTracker::runProcess(const types::DynamicObjectList & detected_ob const rclcpp::Time measurement_time = rclcpp::Time(detected_objects.header.stamp, this->now().get_clock_type()); - // Get the self transform - const auto self_transform = odometry_->getTransform(measurement_time); - if (!self_transform) { - return; - } - /* predict trackers to the measurement time */ processor_->predict(measurement_time); @@ -267,7 +261,7 @@ void MultiObjectTracker::runProcess(const types::DynamicObjectList & detected_ob reverse_assignment); /* tracker update */ - processor_->update(detected_objects, *self_transform, direct_assignment); + processor_->update(detected_objects, direct_assignment); /* tracker pruning */ processor_->prune(measurement_time); diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index 4b3147cabd5e5..045ff427dc823 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -52,6 +52,7 @@ void InputStream::onMessage( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) { const autoware_perception_msgs::msg::DetectedObjects & objects = *msg; + const rclcpp::Time timestamp = objects.header.stamp; types::DynamicObjectList dynamic_objects = types::toDynamicObjectList(objects, channel_.index); @@ -80,8 +81,12 @@ void InputStream::onMessage( } // else, it is bounding box and nothing to do - // calculate nearest point? - // calcAnchorPointOffset(object); + // calculate nearest point + const auto self_transform = odometry_->getTransform(timestamp); + if (!self_transform) { + return; + } + shapes::getNearestCornerOrSurface(*self_transform, object); // if object extension is not reliable, enlarge covariance of position and extend shape } diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index e60b795480484..bbe09baff150c 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -65,7 +65,6 @@ void TrackerProcessor::associate( void TrackerProcessor::update( const types::DynamicObjectList & detected_objects, - const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & direct_assignment) { int tracker_idx = 0; @@ -77,8 +76,7 @@ void TrackerProcessor::update( const auto & associated_object = detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); const types::InputChannel channel_info = channels_config_[associated_object.channel_index]; - (*(tracker_itr)) - ->updateWithMeasurement(associated_object, time, self_transform, channel_info); + (*(tracker_itr))->updateWithMeasurement(associated_object, time, channel_info); } else { // not found diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index 4c15f9c60cb83..9a86240487e9d 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -61,7 +61,6 @@ class TrackerProcessor std::unordered_map & reverse_assignment) const; void update( const types::DynamicObjectList & detected_objects, - const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & direct_assignment); void spawn( const types::DynamicObjectList & detected_objects,