diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 95d33b78ff184..3b05af9601f1e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -124,7 +124,7 @@ class MultiObjectTracker : public rclcpp::Node const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) const; - void publish(const rclcpp::Time & time); + void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index cc662768ce197..1e684e78fc45a 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -441,7 +441,7 @@ inline bool MultiObjectTracker::shouldTrackerPublish( return true; } -void MultiObjectTracker::publish(const rclcpp::Time & time) +void MultiObjectTracker::publish(const rclcpp::Time & time) const { const auto subscriber_count = tracked_objects_pub_->get_subscription_count() + tracked_objects_pub_->get_intra_process_subscription_count(); diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index aea4c84d937e3..ef8a45f608098 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -317,7 +317,7 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c // Current state vector X t Eigen::MatrixXd X_t(DIM, 1); - getStateVector(X_t); + ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index b10fc70d1bba7..9e8327e381057 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -272,7 +272,7 @@ bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) cons // Current state vector X t Eigen::MatrixXd X_t(DIM, 1); - getStateVector(X_t); + ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW)); const double sin_yaw = std::sin(X_t(IDX::YAW)); diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index 28ba59f4f19f9..a9ad03e7875c5 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -209,7 +209,7 @@ bool CVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const // Current state vector X t Eigen::MatrixXd X_t(DIM, 1); - getStateVector(X_t); + ekf.getX(X_t); // Predict state vector X t+1 Eigen::MatrixXd X_next_t(DIM, 1); // predicted state diff --git a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp b/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp index b4af422fd2329..4d51021de634b 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp @@ -46,17 +46,19 @@ bool MotionModel::predictState(const rclcpp::Time & time) if (dt < 0.0) { return false; } + if (dt < 1e-6 /*1usec*/) { + return true; + } + + // multi-step prediction // if dt is too large, shorten dt and repeat prediction - const uint32_t repeat = std::ceil(dt / dt_max_); + const uint32_t repeat = std::floor(dt / dt_max_) + 1; const double dt_ = dt / repeat; for (uint32_t i = 0; i < repeat; ++i) { - if (!predictStateStep(dt_, ekf_)) { - return false; - } - // add interval to last_update_time_ + if (!predictStateStep(dt_, ekf_)) return false; last_update_time_ += rclcpp::Duration::from_seconds(dt_); } - // update last_update_time_ to the estimation time + // reset the last_update_time_ to the prediction time last_update_time_ = time; return true; } @@ -67,25 +69,22 @@ bool MotionModel::getPredictedState( // check if the state is initialized if (!checkInitialized()) return false; - // copy the predicted state and covariance - KalmanFilter tmp_ekf_for_no_update = ekf_; - - double dt = getDeltaTime(time); - if (dt < 0.0) { - // a naive way to handle the case when the required prediction time is in the past - dt = 0.0; + const double dt = getDeltaTime(time); + if (dt < 1e-6 /*1usec*/) { + // no prediction, return the current state + ekf_.getX(X); + ekf_.getP(P); + return true; } - // predict only when dt is small enough - if (0.001 /*1msec*/ < dt) { - // if dt is too large, shorten dt and repeat prediction - const uint32_t repeat = std::ceil(dt / dt_max_); - const double dt_ = dt / repeat; - for (uint32_t i = 0; i < repeat; ++i) { - if (!predictStateStep(dt_, tmp_ekf_for_no_update)) { - return false; - } - } + // copy the predicted state and covariance + KalmanFilter tmp_ekf_for_no_update = ekf_; + // multi-step prediction + // if dt is too large, shorten dt and repeat prediction + const uint32_t repeat = std::floor(dt / dt_max_) + 1; + const double dt_ = dt / repeat; + for (uint32_t i = 0; i < repeat; ++i) { + if (!predictStateStep(dt_, tmp_ekf_for_no_update)) return false; } tmp_ekf_for_no_update.getX(X); tmp_ekf_for_no_update.getP(P);