fix(multi_object_tracker): add missing trailer tracker (autowarefound…

…ation#1885) Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>
tier4 · Oct 7, 2022 · a91f0a0 · a91f0a0
1 parent d06401c
commit a91f0a0
Show file tree

Hide file tree

Showing 6 changed files with 14 additions and 4 deletions.
diff --git a/perception/multi_object_tracker/config/default_tracker.param.yaml b/perception/multi_object_tracker/config/default_tracker.param.yaml
@@ -3,6 +3,7 @@
     car_tracker: "multi_vehicle_tracker"
     truck_tracker: "multi_vehicle_tracker"
     bus_tracker: "multi_vehicle_tracker"
+    trailer_tracker: "multi_vehicle_tracker"
     pedestrian_tracker: "pedestrian_and_bicycle_tracker"
     bicycle_tracker: "pedestrian_and_bicycle_tracker"
     motorcycle_tracker: "pedestrian_and_bicycle_tracker"
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp
@@ -90,6 +90,13 @@ enum MSG_COV_IDX {
   YAW_PITCH = 34,
   YAW_YAW = 35
 };
+
+inline bool isLargeVehicleLabel(const uint8_t label)
+{
+  using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
+  return label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER;
+}
+
 }  // namespace utils
 
 #endif  // MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -136,7 +136,7 @@ bool isSpecificAlivePattern(
     tracker->getTotalMeasurementCount() /
     (tracker->getTotalNoMeasurementCount() + tracker->getTotalMeasurementCount());
 
-  const bool big_vehicle = (label == Label::TRUCK || label == Label::BUS);
+  const bool big_vehicle = utils::isLargeVehicleLabel(label);
 
   const bool slow_velocity = getVelocity(object) < max_velocity;
 
@@ -202,6 +202,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     std::make_pair(Label::TRUCK, this->declare_parameter<std::string>("truck_tracker")));
   tracker_map_.insert(
     std::make_pair(Label::BUS, this->declare_parameter<std::string>("bus_tracker")));
+  tracker_map_.insert(
+    std::make_pair(Label::TRAILER, this->declare_parameter<std::string>("trailer_tracker")));
   tracker_map_.insert(
     std::make_pair(Label::PEDESTRIAN, this->declare_parameter<std::string>("pedestrian_tracker")));
   tracker_map_.insert(

diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
@@ -232,7 +232,7 @@ bool BigVehicleTracker::measureWithPose(
     constexpr float r_stddev_y = 0.8;  // [m]
     r_cov_x = std::pow(r_stddev_x, 2.0);
     r_cov_y = std::pow(r_stddev_y, 2.0);
-  } else if (label == Label::TRUCK || label == Label::BUS) {
+  } else if (utils::isLargeVehicleLabel(label)) {
     r_cov_x = ekf_params_.r_cov_x;
     r_cov_y = ekf_params_.r_cov_y;
   } else {

diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp
@@ -55,7 +55,7 @@ bool MultipleVehicleTracker::getTrackedObject(
 
   if (label == Label::CAR) {
     normal_vehicle_tracker_.getTrackedObject(time, object);
-  } else if (label == Label::BUS || label == Label::TRUCK) {
+  } else if (utils::isLargeVehicleLabel(label)) {
     big_vehicle_tracker_.getTrackedObject(time, object);
   }
   object.object_id = getUUID();

diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp
@@ -230,7 +230,7 @@ bool NormalVehicleTracker::measureWithPose(
   if (label == Label::CAR) {
     r_cov_x = ekf_params_.r_cov_x;
     r_cov_y = ekf_params_.r_cov_y;
-  } else if (label == Label::TRUCK || label == Label::BUS) {
+  } else if (utils::isLargeVehicleLabel(label)) {
     constexpr float r_stddev_x = 8.0;  // [m]
     constexpr float r_stddev_y = 0.8;  // [m]
     r_cov_x = std::pow(r_stddev_x, 2.0);