feat(autoware_multi_object_tracker): Set maximum reverse velocity to …

…bicycle and crtv motion models (autowarefoundation#9019) * feat: Add maximum reverse velocity to bicycle and CTRV motion models revert the tracker orientation when the velocity exceed the maximum reverse velocity Signed-off-by: Taekjin LEE <[email protected]> refactor: Update motion model parameters for bicycle and CTRV motion models * refactor: check the max_reverse_vel configuration is correct max_reverse_vel is expected to be negative Signed-off-by: Taekjin LEE <[email protected]> * refactor: remove config checker in the initializer Signed-off-by: Taekjin LEE <[email protected]> --------- Signed-off-by: Taekjin LEE <[email protected]>
technolojin · Oct 4, 2024 · 6c82525 · 6c82525
1 parent 3288946
commit 6c82525
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 11 deletions.
diff --git a/...acker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/...acker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp
@@ -64,6 +64,8 @@ class BicycleMotionModel : public MotionModel
     double lr_min = 1.0;       // [m] minimum distance from the center to the rear wheel
     double max_vel = 27.8;     // [m/s] maximum velocity, 100km/h
     double max_slip = 0.5236;  // [rad] maximum slip angle, 30deg
+    double max_reverse_vel =
+      -1.389;  // [m/s] maximum reverse velocity, -5km/h. The value is expected to be negative
   } motion_params_;
 
 public:

diff --git a/..._tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/..._tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp
@@ -45,13 +45,14 @@ class CTRVMotionModel : public MotionModel
   // motion parameters: process noise and motion limits
   struct MotionParams
   {
-    double q_cov_x = 0.025;     // [m^2/s^2] uncertain position in x, 0.5m/s
-    double q_cov_y = 0.025;     // [m^2/s^2] uncertain position in y, 0.5m/s
-    double q_cov_yaw = 0.1218;  // [rad^2/s^2] uncertain yaw angle, 20deg/s
-    double q_cov_vel = 8.6436;  // [m^2/s^4] uncertain velocity, 0.3G m/s^2
-    double q_cov_wz = 0.5236;   // [rad^2/s^4] uncertain yaw rate, 30deg/s^2
-    double max_vel = 2.78;      // [m/s] maximum velocity
-    double max_wz = 0.5236;     // [rad/s] maximum yaw rate, 30deg/s
+    double q_cov_x = 0.025;          // [m^2/s^2] uncertain position in x, 0.5m/s
+    double q_cov_y = 0.025;          // [m^2/s^2] uncertain position in y, 0.5m/s
+    double q_cov_yaw = 0.1218;       // [rad^2/s^2] uncertain yaw angle, 20deg/s
+    double q_cov_vel = 8.6436;       // [m^2/s^4] uncertain velocity, 0.3G m/s^2
+    double q_cov_wz = 0.5236;        // [rad^2/s^4] uncertain yaw rate, 30deg/s^2
+    double max_vel = 2.78;           // [m/s] maximum velocity
+    double max_wz = 0.5236;          // [rad/s] maximum yaw rate, 30deg/s
+    double max_reverse_vel = -1.38;  // [m/s] maximum reverse velocity, -5km/h
   } motion_params_;
 
 public:

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
@@ -226,15 +226,26 @@ bool BicycleMotionModel::limitStates()
   Eigen::MatrixXd P_t(DIM, DIM);
   ekf_.getX(X_t);
   ekf_.getP(P_t);
-  X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW));
+
+  // maximum reverse velocity
+  if (motion_params_.max_reverse_vel < 0 && X_t(IDX::VEL) < motion_params_.max_reverse_vel) {
+    // rotate the object orientation by 180 degrees
+    X_t(IDX::VEL) = -X_t(IDX::VEL);
+    X_t(IDX::YAW) = X_t(IDX::YAW) + M_PI;
+  }
+  // maximum velocity
   if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) {
     X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel;
   }
+  // maximum slip angle
   if (!(-motion_params_.max_slip <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= motion_params_.max_slip)) {
     X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -motion_params_.max_slip : motion_params_.max_slip;
   }
-  ekf_.init(X_t, P_t);
+  // normalize yaw
+  X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW));
 
+  // overwrite state
+  ekf_.init(X_t, P_t);
   return true;
 }
 

diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp
@@ -203,15 +203,26 @@ bool CTRVMotionModel::limitStates()
   Eigen::MatrixXd P_t(DIM, DIM);
   ekf_.getX(X_t);
   ekf_.getP(P_t);
-  X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW));
+
+  // maximum reverse velocity
+  if (X_t(IDX::VEL) < 0 && X_t(IDX::VEL) < motion_params_.max_reverse_vel) {
+    // rotate the object orientation by 180 degrees
+    X_t(IDX::VEL) = -X_t(IDX::VEL);
+    X_t(IDX::YAW) = X_t(IDX::YAW) + M_PI;
+  }
+  // maximum velocity
   if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) {
     X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel;
   }
+  // maximum yaw rate
   if (!(-motion_params_.max_wz <= X_t(IDX::WZ) && X_t(IDX::WZ) <= motion_params_.max_wz)) {
     X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -motion_params_.max_wz : motion_params_.max_wz;
   }
-  ekf_.init(X_t, P_t);
+  // normalize yaw
+  X_t(IDX::YAW) = autoware::universe_utils::normalizeRadian(X_t(IDX::YAW));
 
+  // overwrite state
+  ekf_.init(X_t, P_t);
   return true;
 }