Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(multi_object_tracker): tracker refactoring #7271

Merged
Merged
Changes from 1 commit
Commits
Show all changes
31 commits
Select commit Hold shift + click to select a range
d6e50a9
feat: separate filters
technolojin Jun 6, 2024
24f66cd
fix: object validator to modular
technolojin Jun 7, 2024
c5eeaa5
fix: replace xyzrpy covariance index
technolojin May 30, 2024
68d9964
chore: refactor tracker class members
technolojin Jun 4, 2024
6a752a8
chore: refactor
technolojin Jun 4, 2024
a75daa8
chore: refactoring destructor
technolojin Jun 4, 2024
0f9089b
chore: refactoring getUpdatingObject
technolojin Jun 4, 2024
4b0e6cf
chore: refactoring object size checker
technolojin Jun 4, 2024
b963c09
fix: debugger delay calculation order is fixed
technolojin Jun 4, 2024
34057df
fix: bicycle size update bug fix
technolojin Jun 5, 2024
daa4e16
fix: renamed message package
technolojin Jun 5, 2024
8cf586a
chore: refactoring
technolojin Jun 10, 2024
74f08da
feat: object parameter class
technolojin Jun 10, 2024
80806ce
style(pre-commit): autofix
pre-commit-ci[bot] Jun 10, 2024
b316282
chore: refactor includes
technolojin Jun 13, 2024
9fb235c
feat: initial impl. of object model
technolojin Jun 13, 2024
cbb1a58
feat: impl. object model to bicycle, big, normal
technolojin Jun 13, 2024
cb895a9
feat: impl. object model to pedestrian
technolojin Jun 13, 2024
716d1b8
feat: impl. cont
technolojin Jun 13, 2024
2e86d09
style(pre-commit): autofix
pre-commit-ci[bot] Jun 13, 2024
9bdd911
chore: fix missing refactoring
technolojin Jun 13, 2024
ad97751
fix: align unit of angles, yaw rates
technolojin Jun 13, 2024
724dc8e
fix: rename tier4_autoware_utils to autoware_universe_utils
technolojin Jun 18, 2024
b63596d
fix: missing unit conversion
technolojin Jun 18, 2024
8cbb2f3
style(pre-commit): autofix
pre-commit-ci[bot] Jun 18, 2024
0f62e75
fix: sources
technolojin Jun 18, 2024
b139ffd
chore: Update include and import statements
technolojin Jun 19, 2024
0e84b4d
fix: unit convert bug
technolojin Jun 20, 2024
10fdb99
fix: update to autoware::universe_utils
technolojin Jun 20, 2024
3f20c98
fix: mis-implementation of process noise
technolojin Jun 20, 2024
893fdc5
Merge branch 'main' into feat/mot_tracker_refactoring
technolojin Jun 21, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
fix: rename tier4_autoware_utils to autoware_universe_utils
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
technolojin committed Jun 20, 2024
commit 724dc8e7d7b99e3806db9d577e522a0ba1f7c98f
Original file line number Diff line number Diff line change
@@ -16,7 +16,7 @@
#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_

#include "multi_object_tracker/tracker/model/tracker_base.hpp"
#include "tier4_autoware_utils/ros/uuid_helper.hpp"
#include "autoware/universe_utils/ros/uuid_helper.hpp"

#include <autoware/universe_utils/ros/uuid_helper.hpp>
#include <rclcpp/rclcpp.hpp>
Original file line number Diff line number Diff line change
@@ -16,8 +16,8 @@
#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_

#include "multi_object_tracker/debugger/debug_object.hpp"
#include "tier4_autoware_utils/ros/debug_publisher.hpp"
#include "tier4_autoware_utils/ros/published_time_publisher.hpp"
#include "autoware/universe_utils/ros/debug_publisher.hpp"
#include "autoware/universe_utils/ros/published_time_publisher.hpp"

#include <autoware/universe_utils/ros/debug_publisher.hpp>
#include <autoware/universe_utils/ros/published_time_publisher.hpp>
Original file line number Diff line number Diff line change
@@ -21,10 +21,10 @@

#include "multi_object_tracker/utils/utils.hpp"
#include "object_recognition_utils/object_recognition_utils.hpp"
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
#include "tier4_autoware_utils/math/normalization.hpp"
#include "tier4_autoware_utils/math/unit_conversion.hpp"
#include "tier4_autoware_utils/ros/msg_covariance.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 <Eigen/Core>
#include <Eigen/Geometry>
@@ -100,7 +100,7 @@ BicycleTracker::BicycleTracker(

// Set initial state
{
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
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);
@@ -172,7 +172,7 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject(
autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;

// fill covariance matrix
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance;
const double cos_yaw = std::cos(pose_yaw);
const double sin_yaw = std::sin(pose_yaw);
Original file line number Diff line number Diff line change
@@ -21,10 +21,10 @@

#include "multi_object_tracker/utils/utils.hpp"
#include "object_recognition_utils/object_recognition_utils.hpp"
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
#include "tier4_autoware_utils/math/normalization.hpp"
#include "tier4_autoware_utils/math/unit_conversion.hpp"
#include "tier4_autoware_utils/ros/msg_covariance.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 <Eigen/Core>
#include <Eigen/Geometry>
@@ -116,7 +116,7 @@ BigVehicleTracker::BigVehicleTracker(

// Set initial state
{
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
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);
@@ -212,7 +212,7 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje
autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;

// fill covariance matrix
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance;
const double cos_yaw = std::cos(pose_yaw);
const double sin_yaw = std::sin(pose_yaw);
Original file line number Diff line number Diff line change
@@ -21,10 +21,10 @@

#include "multi_object_tracker/utils/utils.hpp"
#include "object_recognition_utils/object_recognition_utils.hpp"
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
#include "tier4_autoware_utils/math/normalization.hpp"
#include "tier4_autoware_utils/math/unit_conversion.hpp"
#include "tier4_autoware_utils/ros/msg_covariance.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 <Eigen/Core>
#include <Eigen/Geometry>
@@ -117,7 +117,7 @@ NormalVehicleTracker::NormalVehicleTracker(

// Set initial state
{
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
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);
@@ -213,7 +213,7 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO
autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;

// fill covariance matrix
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance;
const double cos_yaw = std::cos(pose_yaw);
const double sin_yaw = std::sin(pose_yaw);
Original file line number Diff line number Diff line change
@@ -20,7 +20,7 @@

#include "multi_object_tracker/utils/utils.hpp"
#include "object_recognition_utils/object_recognition_utils.hpp"
#include "tier4_autoware_utils/ros/msg_covariance.hpp"
#include "autoware/universe_utils/ros/msg_covariance.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>
@@ -87,7 +87,7 @@ bool PassThroughTracker::measure(
bool PassThroughTracker::getTrackedObject(
const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const
{
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
object = object_recognition_utils::toTrackedObject(object_);
object.object_id = getUUID();
object.classification = getClassification();
Original file line number Diff line number Diff line change
@@ -21,10 +21,10 @@

#include "multi_object_tracker/utils/utils.hpp"
#include "object_recognition_utils/object_recognition_utils.hpp"
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
#include "tier4_autoware_utils/math/normalization.hpp"
#include "tier4_autoware_utils/math/unit_conversion.hpp"
#include "tier4_autoware_utils/ros/msg_covariance.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 <Eigen/Core>
#include <Eigen/Geometry>
@@ -99,7 +99,7 @@ PedestrianTracker::PedestrianTracker(

// Set initial state
{
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
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);
@@ -164,7 +164,7 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje
autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;

// fill covariance matrix
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance;
const double cos_yaw = std::cos(pose_yaw);
const double sin_yaw = std::sin(pose_yaw);
Original file line number Diff line number Diff line change
@@ -16,10 +16,10 @@
#include "multi_object_tracker/tracker/model/unknown_tracker.hpp"

#include "multi_object_tracker/utils/utils.hpp"
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
#include "tier4_autoware_utils/math/normalization.hpp"
#include "tier4_autoware_utils/math/unit_conversion.hpp"
#include "tier4_autoware_utils/ros/msg_covariance.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 <Eigen/Core>
#include <Eigen/Geometry>
@@ -73,7 +73,7 @@ UnknownTracker::UnknownTracker(

// Set initial state
{
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
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;
auto pose_cov = object.kinematics.pose_with_covariance.covariance;
@@ -148,7 +148,7 @@ autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject(
// UNCERTAINTY MODEL
if (!object.kinematics.has_position_covariance) {
// fill covariance matrix
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
const double & r_cov_x = ekf_params_.r_cov_x;
const double & r_cov_y = ekf_params_.r_cov_y;
auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance;
Original file line number Diff line number Diff line change
@@ -21,17 +21,17 @@

#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp"
#include "multi_object_tracker/utils/utils.hpp"
#include "tier4_autoware_utils/math/normalization.hpp"
#include "tier4_autoware_utils/math/unit_conversion.hpp"
#include "tier4_autoware_utils/ros/msg_covariance.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 <Eigen/Core>
#include <Eigen/Geometry>

// cspell: ignore CTRV
// Bicycle CTRV motion model
// CTRV : Constant Turn Rate and constant Velocity
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;

BicycleMotionModel::BicycleMotionModel() : logger_(rclcpp::get_logger("BicycleMotionModel"))
{
@@ -45,14 +45,14 @@ void BicycleMotionModel::setDefaultParams()
constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration
constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration
constexpr double q_stddev_yaw_rate_min =
tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate
autoware_universe_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate
constexpr double q_stddev_yaw_rate_max =
tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate
autoware_universe_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate
constexpr double q_stddev_slip_rate_min =
tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate
autoware_universe_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate
constexpr double q_stddev_slip_rate_max =
tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate
constexpr double q_max_slip_angle = tier4_autoware_utils::deg2rad(30.0); // [rad] max slip angle
autoware_universe_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate
constexpr double q_max_slip_angle = autoware_universe_utils::deg2rad(30.0); // [rad] max slip angle
// extended state parameters
constexpr double lf_ratio = 0.3; // 30% front from the center
constexpr double lf_min = 1.0; // minimum of 1.0m
Original file line number Diff line number Diff line change
@@ -21,16 +21,16 @@

#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp"
#include "multi_object_tracker/utils/utils.hpp"
#include "tier4_autoware_utils/math/normalization.hpp"
#include "tier4_autoware_utils/math/unit_conversion.hpp"
#include "tier4_autoware_utils/ros/msg_covariance.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 <Eigen/Core>
#include <Eigen/Geometry>

// cspell: ignore CTRV
// Constant Turn Rate and constant Velocity (CTRV) motion model
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;

CTRVMotionModel::CTRVMotionModel() : logger_(rclcpp::get_logger("CTRVMotionModel"))
{
Original file line number Diff line number Diff line change
@@ -21,9 +21,9 @@

#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp"
#include "multi_object_tracker/utils/utils.hpp"
#include "tier4_autoware_utils/math/normalization.hpp"
#include "tier4_autoware_utils/math/unit_conversion.hpp"
#include "tier4_autoware_utils/ros/msg_covariance.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 <Eigen/Core>
#include <Eigen/Geometry>
@@ -32,7 +32,7 @@

// cspell: ignore CV
// Constant Velocity (CV) motion model
using tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
using autoware_universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;

CVMotionModel::CVMotionModel() : logger_(rclcpp::get_logger("CVMotionModel"))
{