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 098ad39dd3df9..69a9edd9c9d9e 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 @@ -208,11 +208,8 @@ bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::Detec constexpr double size_min = 0.1; // [m] if ( object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max || - object.shape.dimensions.z > size_max) { - return false; - } else if ( - object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min || - object.shape.dimensions.z < size_min) { + object.shape.dimensions.z > size_max || object.shape.dimensions.x < size_min || + object.shape.dimensions.y < size_min || object.shape.dimensions.z < size_min) { return false; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 89258835f920b..c65efffe99a47 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -315,7 +315,7 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c double w = vel * sin_slip / lr_; const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + const double vv_dtdt_lr = vel * vel * dt * dt / lr_; // Predict state vector X t+1 Eigen::MatrixXd X_next_t(DIM, 1); // predicted state @@ -332,11 +332,11 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt_lr; A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt_lr; A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; 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 fa333ea06a4b9..d5b12ed7b2b82 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -263,11 +263,11 @@ void InputManager::getObjectTimeInterval( // The default object_earliest_time is to have a 1-second time interval const rclcpp::Time object_earliest_time_default = object_latest_time - rclcpp::Duration::from_seconds(1.0); - if (latest_exported_object_time_ < object_earliest_time_default) { - // if the latest exported object time is too old, set to the default - object_earliest_time = object_earliest_time_default; - } else if (latest_exported_object_time_ > object_latest_time) { - // if the latest exported object time is newer than the object_latest_time, set to the default + if ( + latest_exported_object_time_ < object_earliest_time_default || + latest_exported_object_time_ > object_latest_time) { + // if the latest exported object time is too old or newer than the object_latest_time, + // set to the default object_earliest_time = object_earliest_time_default; } else { // The object_earliest_time is the latest exported object time