From e30350c358531a2c5dc2a132de9ad4f85fa370dc Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 12 Dec 2024 11:30:06 +0900 Subject: [PATCH] refactor(autoware_multi_object_tracker): merge normal_vehicle_tracker and big_vehicle_tracker (#9613) * refactor: define object_model_ at initialization * refactor: combine normal and big vehicle tracker --- .../CMakeLists.txt | 3 +- .../model/multiple_vehicle_tracker.hpp | 7 +- .../tracker/model/normal_vehicle_tracker.hpp | 78 ---- ...ehicle_tracker.hpp => vehicle_tracker.hpp} | 18 +- .../multi_object_tracker/tracker/tracker.hpp | 3 +- .../lib/tracker/model/big_vehicle_tracker.cpp | 362 ------------------ .../model/multiple_vehicle_tracker.cpp | 8 +- ...ehicle_tracker.cpp => vehicle_tracker.cpp} | 32 +- .../src/processor/processor.cpp | 9 +- 9 files changed, 41 insertions(+), 479 deletions(-) delete mode 100644 perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp rename perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/{big_vehicle_tracker.hpp => vehicle_tracker.hpp} (82%) delete mode 100644 perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp rename perception/autoware_multi_object_tracker/lib/tracker/model/{normal_vehicle_tracker.cpp => vehicle_tracker.cpp} (92%) diff --git a/perception/autoware_multi_object_tracker/CMakeLists.txt b/perception/autoware_multi_object_tracker/CMakeLists.txt index 73edabaa09429..298b6605192a5 100644 --- a/perception/autoware_multi_object_tracker/CMakeLists.txt +++ b/perception/autoware_multi_object_tracker/CMakeLists.txt @@ -36,8 +36,7 @@ set(${PROJECT_NAME}_lib lib/tracker/motion_model/ctrv_motion_model.cpp lib/tracker/motion_model/cv_motion_model.cpp lib/tracker/model/tracker_base.cpp - lib/tracker/model/big_vehicle_tracker.cpp - lib/tracker/model/normal_vehicle_tracker.cpp + lib/tracker/model/vehicle_tracker.cpp lib/tracker/model/multiple_vehicle_tracker.cpp lib/tracker/model/bicycle_tracker.cpp lib/tracker/model/pedestrian_tracker.cpp 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 868aa1606d4ff..8b4fa1babf652 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 @@ -20,9 +20,8 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" -#include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" -#include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp" #include @@ -32,8 +31,8 @@ namespace autoware::multi_object_tracker class MultipleVehicleTracker : public Tracker { private: - NormalVehicleTracker normal_vehicle_tracker_; - BigVehicleTracker big_vehicle_tracker_; + VehicleTracker normal_vehicle_tracker_; + VehicleTracker big_vehicle_tracker_; public: MultipleVehicleTracker( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp deleted file mode 100644 index 0bfc065c7cc68..0000000000000 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ /dev/null @@ -1,78 +0,0 @@ -// 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 AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ -#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ - -#include "autoware/kalman_filter/kalman_filter.hpp" -#include "autoware/multi_object_tracker/object_model/object_model.hpp" -#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" -#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" - -namespace autoware::multi_object_tracker -{ - -class NormalVehicleTracker : public Tracker -{ -private: - autoware_perception_msgs::msg::DetectedObject object_; - rclcpp::Logger logger_; - - object_model::ObjectModel object_model_ = object_model::normal_vehicle; - - double velocity_deviation_threshold_; - - double z_; - - struct BoundingBox - { - double length; - double width; - double height; - }; - BoundingBox bounding_box_; - Eigen::Vector2d tracking_offset_; - - BicycleMotionModel motion_model_; - using IDX = BicycleMotionModel::IDX; - -public: - NormalVehicleTracker( - const rclcpp::Time & time, const autoware_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 geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; - -private: - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); -}; - -} // namespace autoware::multi_object_tracker - -#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp similarity index 82% rename from perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index 489f656f750cb..f3708fd1ff905 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Yukihiro Saito // -#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ -#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__VEHICLE_TRACKER_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__VEHICLE_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" @@ -27,16 +27,15 @@ namespace autoware::multi_object_tracker { -class BigVehicleTracker : public Tracker +class VehicleTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + object_model::ObjectModel object_model_; rclcpp::Logger logger_; - object_model::ObjectModel object_model_ = object_model::big_vehicle; - double velocity_deviation_threshold_; + autoware_perception_msgs::msg::DetectedObject object_; double z_; struct BoundingBox @@ -52,8 +51,9 @@ class BigVehicleTracker : public Tracker using IDX = BicycleMotionModel::IDX; public: - BigVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, + VehicleTracker( + const object_model::ObjectModel & object_model, const rclcpp::Time & time, + const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index); @@ -75,4 +75,4 @@ class BigVehicleTracker : public Tracker } // namespace autoware::multi_object_tracker -#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__VEHICLE_TRACKER_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp index ea1e60d4c7e17..ecf727c36936c 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp @@ -20,13 +20,12 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ #include "model/bicycle_tracker.hpp" -#include "model/big_vehicle_tracker.hpp" #include "model/multiple_vehicle_tracker.hpp" -#include "model/normal_vehicle_tracker.hpp" #include "model/pass_through_tracker.hpp" #include "model/pedestrian_and_bicycle_tracker.hpp" #include "model/pedestrian_tracker.hpp" #include "model/tracker_base.hpp" #include "model/unknown_tracker.hpp" +#include "model/vehicle_tracker.hpp" #endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp deleted file mode 100644 index 349ffb1eec634..0000000000000 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp +++ /dev/null @@ -1,362 +0,0 @@ -// 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 -// -#define EIGEN_MPL2_ONLY - -#include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" - -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" - -#include -#include - -#include -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -namespace autoware::multi_object_tracker -{ -using Label = autoware_perception_msgs::msg::ObjectClassification; - -BigVehicleTracker::BigVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) -: Tracker(time, object.classification, channel_size), - logger_(rclcpp::get_logger("BigVehicleTracker")), - z_(object.kinematics.pose_with_covariance.pose.position.z), - tracking_offset_(Eigen::Vector2d::Zero()) -{ - object_ = object; - - // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); - - // velocity deviation threshold - // if the predicted velocity is close to the observed velocity, - // the observed velocity is used as the measurement. - velocity_deviation_threshold_ = autoware::universe_utils::kmph2mps(10); // [m/s] - - // OBJECT SHAPE MODEL - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - } else { - autoware_perception_msgs::msg::DetectedObject bbox_object; - if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { - RCLCPP_WARN( - logger_, - "BigVehicleTracker::BigVehicleTracker: Failed to convert convex hull to bounding box."); - bounding_box_ = { - object_model_.init_size.length, object_model_.init_size.width, - object_model_.init_size.height}; // default value - } else { - bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; - } - } - // set maximum and minimum size - bounding_box_.length = std::clamp( - bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); - bounding_box_.width = std::clamp( - bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); - bounding_box_.height = std::clamp( - bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); - - // Set motion model parameters - { - const double q_stddev_acc_long = object_model_.process_noise.acc_long; - const double q_stddev_acc_lat = object_model_.process_noise.acc_lat; - const double q_stddev_yaw_rate_min = object_model_.process_noise.yaw_rate_min; - const double q_stddev_yaw_rate_max = object_model_.process_noise.yaw_rate_max; - const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_stddev_min; - const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_stddev_max; - const double q_max_slip_angle = object_model_.bicycle_state.slip_angle_max; - const double lf_ratio = object_model_.bicycle_state.wheel_pos_ratio_front; - const double lf_min = object_model_.bicycle_state.wheel_pos_front_min; - const double lr_ratio = object_model_.bicycle_state.wheel_pos_ratio_rear; - const double lr_min = object_model_.bicycle_state.wheel_pos_rear_min; - motion_model_.setMotionParams( - q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, - q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, - lr_min); - } - - // Set motion limits - { - const double max_vel = object_model_.process_limit.vel_long_max; - const double max_slip = object_model_.bicycle_state.slip_angle_max; - motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle - } - - // Set initial state - { - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - const double x = object.kinematics.pose_with_covariance.pose.position.x; - const double y = object.kinematics.pose_with_covariance.pose.position.y; - const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - - auto pose_cov = object.kinematics.pose_with_covariance.covariance; - if (!object.kinematics.has_position_covariance) { - // initial state covariance - const auto & p0_cov_x = object_model_.initial_covariance.pos_x; - const auto & p0_cov_y = object_model_.initial_covariance.pos_y; - const auto & p0_cov_yaw = object_model_.initial_covariance.yaw; - - const double cos_yaw = std::cos(yaw); - const double sin_yaw = std::sin(yaw); - const double sin_2yaw = std::sin(2.0 * yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; - pose_cov[XYZRPY_COV_IDX::Y_Y] = p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = p0_cov_yaw; - } - - double vel = 0.0; - double vel_cov = object_model_.initial_covariance.vel_long; - if (object.kinematics.has_twist) { - vel = object.kinematics.twist_with_covariance.twist.linear.x; - } - if (object.kinematics.has_twist_covariance) { - vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; - } - - const double slip = 0.0; - const double slip_cov = object_model_.bicycle_state.init_slip_angle_cov; - const double & length = bounding_box_.length; - - // initialize motion model - motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); - } -} - -bool BigVehicleTracker::predict(const rclcpp::Time & time) -{ - return motion_model_.predictState(time); -} - -autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) -{ - autoware_perception_msgs::msg::DetectedObject updating_object = object; - - // OBJECT SHAPE MODEL - // convert to bounding box if input is convex shape - autoware_perception_msgs::msg::DetectedObject bbox_object = object; - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { - RCLCPP_WARN( - logger_, - "BigVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); - bbox_object = object; - } - } - - // current (predicted) state - const double tracked_x = motion_model_.getStateElement(IDX::X); - const double tracked_y = motion_model_.getStateElement(IDX::Y); - const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); - - // get offset measurement - const int nearest_corner_index = utils::getNearestCornerOrSurface( - tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); - utils::calcAnchorPointOffset( - bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, - updating_object, tracking_offset_); - - return updating_object; -} - -bool BigVehicleTracker::measureWithPose( - const autoware_perception_msgs::msg::DetectedObject & object) -{ - // current (predicted) state - const double tracked_vel = motion_model_.getStateElement(IDX::VEL); - - // velocity capability is checked only when the object has velocity measurement - // and the predicted velocity is close to the observed velocity - bool is_velocity_available = false; - if (object.kinematics.has_twist) { - const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) { - // Velocity deviation is small - is_velocity_available = true; - } - } - - // update - bool is_updated = false; - { - const double x = object.kinematics.pose_with_covariance.pose.position.x; - const double y = object.kinematics.pose_with_covariance.pose.position.y; - const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const double vel = object.kinematics.twist_with_covariance.twist.linear.x; - - if (is_velocity_available) { - is_updated = motion_model_.updateStatePoseHeadVel( - x, y, yaw, object.kinematics.pose_with_covariance.covariance, vel, - object.kinematics.twist_with_covariance.covariance); - } else { - is_updated = motion_model_.updateStatePoseHead( - x, y, yaw, object.kinematics.pose_with_covariance.covariance); - } - motion_model_.limitStates(); - } - - // position z - constexpr double gain = 0.1; - z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - - return is_updated; -} - -bool BigVehicleTracker::measureWithShape( - const autoware_perception_msgs::msg::DetectedObject & object) -{ - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - // do not update shape if the input is not a bounding box - return false; - } - - // check object size abnormality - constexpr double size_max = 35.0; // [m] - constexpr double size_min = 1.0; // [m] - bool is_size_valid = - (object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max && - object.shape.dimensions.x >= size_min && object.shape.dimensions.y >= size_min); - if (!is_size_valid) { - return false; - } - - // update object size - constexpr double gain = 0.5; - constexpr double gain_inv = 1.0 - gain; - bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; - bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; - bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; - - // set maximum and minimum size - bounding_box_.length = std::clamp( - bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); - bounding_box_.width = std::clamp( - bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); - bounding_box_.height = std::clamp( - bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); - - // update motion model - motion_model_.updateExtendedState(bounding_box_.length); - - // update offset into object position - { - // rotate back the offset vector from object coordinate to global coordinate - const double yaw = motion_model_.getStateElement(IDX::YAW); - const double offset_x_global = - tracking_offset_.x() * std::cos(yaw) - tracking_offset_.y() * std::sin(yaw); - const double offset_y_global = - tracking_offset_.x() * std::sin(yaw) + tracking_offset_.y() * std::cos(yaw); - motion_model_.adjustPosition(-gain * offset_x_global, -gain * offset_y_global); - // update offset (object coordinate) - tracking_offset_.x() = gain_inv * tracking_offset_.x(); - tracking_offset_.y() = gain_inv * tracking_offset_.y(); - } - - return true; -} - -bool BigVehicleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) -{ - // keep the latest input object - object_ = object; - - // update classification - const auto & current_classification = getClassification(); - if ( - autoware::object_recognition_utils::getHighestProbLabel(object.classification) == - Label::UNKNOWN) { - setClassification(current_classification); - } - - // check time gap - const double dt = motion_model_.getDeltaTime(time); - if (0.01 /*10msec*/ < dt) { - RCLCPP_WARN( - logger_, - "BigVehicleTracker::measure There is a large gap between predicted time and measurement " - "time. (%f)", - dt); - } - - // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); - measureWithPose(updating_object); - measureWithShape(updating_object); - - return true; -} - -bool BigVehicleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const -{ - object = autoware::object_recognition_utils::toTrackedObject(object_); - object.object_id = getUUID(); - object.classification = getClassification(); - - auto & pose_with_cov = object.kinematics.pose_with_covariance; - auto & twist_with_cov = object.kinematics.twist_with_covariance; - - // predict from motion model - if (!motion_model_.getPredictedState( - time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, - twist_with_cov.covariance)) { - RCLCPP_WARN(logger_, "BigVehicleTracker::getTrackedObject: Failed to get predicted state."); - return false; - } - - // position - pose_with_cov.pose.position.z = z_; - - // set shape - object.shape.dimensions.x = bounding_box_.length; - object.shape.dimensions.y = bounding_box_.width; - object.shape.dimensions.z = bounding_box_.height; - const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); - const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); - object.shape.footprint = - autoware::universe_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); - - return true; -} - -} // namespace autoware::multi_object_tracker 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 4751e3d1c894f..ff06544bd509c 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 @@ -18,6 +18,8 @@ #include "autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" + namespace autoware::multi_object_tracker { @@ -28,8 +30,10 @@ MultipleVehicleTracker::MultipleVehicleTracker( const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), - normal_vehicle_tracker_(time, object, self_transform, channel_size, channel_index), - big_vehicle_tracker_(time, object, self_transform, channel_size, channel_index) + normal_vehicle_tracker_( + object_model::normal_vehicle, time, object, self_transform, channel_size, channel_index), + big_vehicle_tracker_( + object_model::big_vehicle, time, object, self_transform, channel_size, channel_index) { // initialize existence probability initializeExistenceProbabilities(channel_index, object.existence_probability); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp similarity index 92% rename from perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp rename to perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index b4be5e42b0397..2d1f48a3ad5ee 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -17,7 +17,7 @@ // #define EIGEN_MPL2_ONLY -#include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" #include "autoware/object_recognition_utils/object_recognition_utils.hpp" @@ -45,12 +45,14 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; -NormalVehicleTracker::NormalVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, +VehicleTracker::VehicleTracker( + const object_model::ObjectModel & object_model, const rclcpp::Time & time, + const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), - logger_(rclcpp::get_logger("NormalVehicleTracker")), + object_model_(object_model), + logger_(rclcpp::get_logger("VehicleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { @@ -73,7 +75,7 @@ NormalVehicleTracker::NormalVehicleTracker( if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, - "NormalVehicleTracker::NormalVehicleTracker: Failed to convert convex hull to bounding " + "VehicleTracker::VehicleTracker: Failed to convert convex hull to bounding " "box."); bounding_box_ = { object_model_.init_size.length, object_model_.init_size.width, @@ -160,12 +162,12 @@ NormalVehicleTracker::NormalVehicleTracker( } } -bool NormalVehicleTracker::predict(const rclcpp::Time & time) +bool VehicleTracker::predict(const rclcpp::Time & time) { return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingObject( +autoware_perception_msgs::msg::DetectedObject VehicleTracker::getUpdatingObject( const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform) { @@ -178,7 +180,7 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, - "NormalVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); + "VehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); bbox_object = object; } } @@ -198,8 +200,7 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO return updating_object; } -bool NormalVehicleTracker::measureWithPose( - const autoware_perception_msgs::msg::DetectedObject & object) +bool VehicleTracker::measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object) { // current (predicted) state const double tracked_vel = motion_model_.getStateElement(IDX::VEL); @@ -241,8 +242,7 @@ bool NormalVehicleTracker::measureWithPose( return is_updated; } -bool NormalVehicleTracker::measureWithShape( - const autoware_perception_msgs::msg::DetectedObject & object) +bool VehicleTracker::measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object) { if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box @@ -294,7 +294,7 @@ bool NormalVehicleTracker::measureWithShape( return true; } -bool NormalVehicleTracker::measure( +bool VehicleTracker::measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { @@ -314,7 +314,7 @@ bool NormalVehicleTracker::measure( if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( logger_, - "NormalVehicleTracker::measure There is a large gap between predicted time and measurement " + "VehicleTracker::measure There is a large gap between predicted time and measurement " "time. (%f)", dt); } @@ -328,7 +328,7 @@ bool NormalVehicleTracker::measure( return true; } -bool NormalVehicleTracker::getTrackedObject( +bool VehicleTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { object = autoware::object_recognition_utils::toTrackedObject(object_); @@ -342,7 +342,7 @@ bool NormalVehicleTracker::getTrackedObject( if (!motion_model_.getPredictedState( time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, twist_with_cov.covariance)) { - RCLCPP_WARN(logger_, "NormalVehicleTracker::getTrackedObject: Failed to get predicted state."); + RCLCPP_WARN(logger_, "VehicleTracker::getTrackedObject: Failed to get predicted state."); return false; } diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index f4202c1f6e413..ebc11ea63e199 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -14,6 +14,7 @@ #include "processor.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/tracker.hpp" #include "autoware/object_recognition_utils/object_recognition_utils.hpp" @@ -102,14 +103,14 @@ std::shared_ptr TrackerProcessor::createNewTracker( return std::make_shared( time, object, self_transform, channel_size_, channel_index); if (tracker == "big_vehicle_tracker") - return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + return std::make_shared( + object_model::big_vehicle, time, object, self_transform, channel_size_, channel_index); if (tracker == "multi_vehicle_tracker") return std::make_shared( time, object, self_transform, channel_size_, channel_index); if (tracker == "normal_vehicle_tracker") - return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + return std::make_shared( + object_model::normal_vehicle, time, object, self_transform, channel_size_, channel_index); if (tracker == "pass_through_tracker") return std::make_shared( time, object, self_transform, channel_size_, channel_index);