Merge branch 'main' into refactor_elevation_map_loader

autowarefoundation · Oct 4, 2024 · b541490 · b541490
2 parents 9cdfd86 + 9e32e92
commit b541490
Show file tree

Hide file tree

Showing 17 changed files with 684 additions and 210 deletions.
diff --git a/codecov.yaml b/codecov.yaml
@@ -113,3 +113,8 @@ component_management:
         # - control/control_performance_analysis/**
         - control/obstacle_collision_checker/**
         # - control/predicted_path_checker/**
+
+    - component_id: perception-tier-iv-maintained-packages
+      name: Perception TIER IV Maintained Packages
+      paths:
+        - perception/[^lidar_apollo_instance_segmentation]/**
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;
 }
 

diff --git a/planning/autoware_mission_planner/CMakeLists.txt b/planning/autoware_mission_planner/CMakeLists.txt
@@ -38,7 +38,8 @@ pluginlib_export_plugin_description_file(autoware_mission_planner plugins/plugin
 
 if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
-    test/test_lanelet2_plugins_default_planner.cpp
+  test/test_lanelet2_plugins_default_planner.cpp
+  test/test_utility_functions.cpp
   )
   target_link_libraries(test_${PROJECT_NAME}
     ${PROJECT_NAME}_lanelet2_plugins