fix(multi_object_tracker): bicycle motion model - set minimum wheel-t…

…o-center length (autowarefoundation#6337)

* fix: bicycle motion model - set minimum wheel-to-center length for stability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* chore: align comments

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

---------

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp

@@ -143,8 +143,8 @@ BicycleTracker::BicycleTracker(
   ekf_.init(X, P);
 
   // Set lf, lr
-  lf_ = bounding_box_.length * 0.3;  // 30% front from the center
-  lr_ = bounding_box_.length * 0.3;  // 30% rear from the center
+  lf_ = std::max(bounding_box_.length * 0.3, 0.3);  // 30% front from the center, minimum of 0.3m
+  lr_ = std::max(bounding_box_.length * 0.3, 0.3);  // 30% rear from the center, minimum of 0.3m
 }
 
 bool BicycleTracker::predict(const rclcpp::Time & time)
@@ -425,8 +425,8 @@ bool BicycleTracker::measureWithShape(
     bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z};
 
   // update lf, lr
-  lf_ = bounding_box_.length * 0.3;  // 30% front from the center
-  lr_ = bounding_box_.length * 0.3;  // 30% rear from the center
+  lf_ = std::max(bounding_box_.length * 0.3, 0.3);  // 30% front from the center, minimum of 0.3m
+  lr_ = std::max(bounding_box_.length * 0.3, 0.3);  // 30% rear from the center, minimum of 0.3m
 
   return true;
 }

perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp

@@ -158,8 +158,8 @@ BigVehicleTracker::BigVehicleTracker(
   setNearestCornerOrSurfaceIndex(self_transform);  // this index is used in next measure step
 
   // Set lf, lr
-  lf_ = bounding_box_.length * 0.3;   // 30% front from the center
-  lr_ = bounding_box_.length * 0.25;  // 25% rear from the center
+  lf_ = std::max(bounding_box_.length * 0.3, 1.5);   // 30% front from the center, minimum of 1.5m
+  lr_ = std::max(bounding_box_.length * 0.25, 1.5);  // 25% rear from the center, minimum of 1.5m
 }
 
 bool BigVehicleTracker::predict(const rclcpp::Time & time)
@@ -457,9 +457,8 @@ bool BigVehicleTracker::measureWithShape(
     bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z};
 
   // update lf, lr
-  lf_ = bounding_box_.length * 0.3;   // 30% front from the center
-  lr_ = bounding_box_.length * 0.25;  // 25% rear from the center
-
+  lf_ = std::max(bounding_box_.length * 0.3, 1.5);   // 30% front from the center, minimum of 1.5m
+  lr_ = std::max(bounding_box_.length * 0.25, 1.5);  // 25% rear from the center, minimum of 1.5m
   return true;
 }
 

perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp

@@ -158,8 +158,8 @@ NormalVehicleTracker::NormalVehicleTracker(
   setNearestCornerOrSurfaceIndex(self_transform);  // this index is used in next measure step
 
   // Set lf, lr
-  lf_ = bounding_box_.length * 0.3;   // 30% front from the center
-  lr_ = bounding_box_.length * 0.25;  // 25% rear from the center
+  lf_ = std::max(bounding_box_.length * 0.3, 1.0);   // 30% front from the center, minimum of 1.0m
+  lr_ = std::max(bounding_box_.length * 0.25, 1.0);  // 25% rear from the center, minimum of 1.0m
 }
 
 bool NormalVehicleTracker::predict(const rclcpp::Time & time)
@@ -457,8 +457,8 @@ bool NormalVehicleTracker::measureWithShape(
     bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z};
 
   // update lf, lr
-  lf_ = bounding_box_.length * 0.3;   // 30% front from the center
-  lr_ = bounding_box_.length * 0.25;  // 25% rear from the center
+  lf_ = std::max(bounding_box_.length * 0.3, 1.0);   // 30% front from the center, minimum of 1.0m
+  lr_ = std::max(bounding_box_.length * 0.25, 1.0);  // 25% rear from the center, minimum of 1.0m
 
   return true;
 }