fix: define type for object list

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

Author: technolojin

perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp

@@ -55,9 +55,9 @@
 namespace multi_object_tracker
 {
 
-using autoware_auto_perception_msgs::msg::DetectedObject;
-using autoware_auto_perception_msgs::msg::DetectedObjects;
-using autoware_auto_perception_msgs::msg::TrackedObjects;
+using DetectedObject = autoware_auto_perception_msgs::msg::DetectedObject;
+using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects;
+using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects;
 
 class MultiObjectTracker : public rclcpp::Node
 {
@@ -94,7 +94,7 @@ class MultiObjectTracker : public rclcpp::Node
   // callback functions
   void onTimer();
   void onTrigger();
-  void onMessage(const std::vector<std::pair<size_t, DetectedObjects>> & objects_data);
+  void onMessage(const ObjectsList & objects_list);
 
   // publish processes
   void runProcess(const DetectedObjects & input_objects_msg);

perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp

@@ -28,7 +28,8 @@
 
 namespace multi_object_tracker
 {
-using autoware_auto_perception_msgs::msg::DetectedObjects;
+using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects;
+using ObjectsList = std::vector<std::pair<uint, DetectedObjects>>;
 
 struct InputChannel
 {
@@ -43,12 +44,12 @@ struct InputChannel
 class InputStream
 {
 public:
-  explicit InputStream(rclcpp::Node & node, size_t & index);
+  explicit InputStream(rclcpp::Node & node, uint & index);
 
   void init(const InputChannel & input_channel);
 
   void setQueueSize(size_t que_size) { que_size_ = que_size; }
-  void setTriggerFunction(std::function<void(const size_t &)> func_trigger)
+  void setTriggerFunction(std::function<void(const uint &)> func_trigger)
   {
     func_trigger_ = func_trigger;
   }
@@ -57,7 +58,7 @@ class InputStream
 
   void getObjectsOlderThan(
     const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
-    std::vector<std::pair<size_t, DetectedObjects>> & objects);
+    ObjectsList & objects_list);
   void getNames(std::string & long_name, std::string & short_name)
   {
     long_name = long_name_;
@@ -78,7 +79,7 @@ class InputStream
 
 private:
   rclcpp::Node & node_;
-  size_t index_;
+  uint index_;
 
   std::string input_topic_;
   std::string long_name_;
@@ -87,7 +88,7 @@ class InputStream
   size_t que_size_{30};
   std::deque<DetectedObjects> objects_que_;
 
-  std::function<void(const size_t &)> func_trigger_;
+  std::function<void(const uint &)> func_trigger_;
 
   bool is_time_initialized_{false};
   double expected_rate_;
@@ -108,12 +109,11 @@ class InputManager
   void init(const std::vector<InputChannel> & input_channels);
   void setTriggerFunction(std::function<void()> func_trigger) { func_trigger_ = func_trigger; }
 
-  void onTrigger(const size_t & index) const;
+  void onTrigger(const uint & index) const;
 
   void getObjectTimeInterval(
     const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time);
-  bool getObjects(
-    const rclcpp::Time & now, std::vector<std::pair<size_t, DetectedObjects>> & objects_list);
+  bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list);
 
 private:
   rclcpp::Node & node_;

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -87,16 +87,6 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   std::vector<std::string> input_names_long = get_parameter("input_names_long").as_string_array();
   std::vector<std::string> input_names_short = get_parameter("input_names_short").as_string_array();
 
-  input_channels_.resize(input_topic_names.size());
-  assert(input_topic_names.size() == input_names_long.size());
-  assert(input_topic_names.size() == input_names_short.size());
-  for (size_t i = 0; i < input_topic_names.size(); i++) {
-    input_channels_[i].index = i;
-    input_channels_[i].input_topic = input_topic_names[i];
-    input_channels_[i].long_name = input_names_long[i];
-    input_channels_[i].short_name = input_names_short[i];
-  }
-
   // ROS interface - Publisher
   tracked_objects_pub_ = create_publisher<TrackedObjects>("output", rclcpp::QoS{1});
 
@@ -106,6 +96,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     return;
   }
   input_topic_size_ = input_topic_names.size();
+  input_channels_.resize(input_topic_size_);
+  assert(input_names_long.size() == input_topic_size_);
+  assert(input_names_short.size() == input_topic_size_);
+  for (size_t i = 0; i < input_topic_size_; i++) {
+    input_channels_[i].index = i;
+    input_channels_[i].input_topic = input_topic_names[i];
+    input_channels_[i].long_name = input_names_long[i];
+    input_channels_[i].short_name = input_names_short[i];
+  }
 
   // Initialize input manager
   input_manager_ = std::make_unique<InputManager>(*this);
@@ -174,7 +173,7 @@ void MultiObjectTracker::onTrigger()
 {
   const rclcpp::Time current_time = this->now();
   // get objects from the input manager and run process
-  std::vector<std::pair<size_t, DetectedObjects>> objects_list;
+  std::vector<std::pair<uint, DetectedObjects>> objects_list;
   const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list);
   if (!is_objects_ready) return;
 
@@ -206,7 +205,7 @@ void MultiObjectTracker::onTimer()
   if (elapsed_time < maximum_publish_latency) return;
 
   // get objects from the input manager and run process
-  std::vector<std::pair<size_t, DetectedObjects>> objects_list;
+  ObjectsList objects_list;
   const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list);
   if (is_objects_ready) {
     onMessage(objects_list);
@@ -216,8 +215,7 @@ void MultiObjectTracker::onTimer()
   checkAndPublish(current_time);
 }
 
-void MultiObjectTracker::onMessage(
-  const std::vector<std::pair<size_t, DetectedObjects>> & objects_list)
+void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
 {
   const rclcpp::Time current_time = this->now();
   const rclcpp::Time oldest_time(objects_list.front().second.header.stamp);

perception/multi_object_tracker/src/processor/input_manager.cpp

@@ -19,7 +19,7 @@
 namespace multi_object_tracker
 {
 
-InputStream::InputStream(rclcpp::Node & node, size_t & index) : node_(node), index_(index)
+InputStream::InputStream(rclcpp::Node & node, uint & index) : node_(node), index_(index)
 {
 }
 
@@ -104,7 +104,7 @@ void InputStream::onMessage(
 
 void InputStream::getObjectsOlderThan(
   const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
-  std::vector<std::pair<size_t, DetectedObjects>> & objects_list)
+  ObjectsList & objects_list)
 {
   assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds());
 
@@ -119,7 +119,7 @@ void InputStream::getObjectsOlderThan(
 
     // Add the object if the object is older than the specified latest time
     if (object_latest_time >= object_time) {
-      std::pair<size_t, DetectedObjects> object_pair(index_, object);
+      std::pair<uint, DetectedObjects> object_pair(index_, object);
       objects_list.push_back(object_pair);
       // remove the object from the queue
       objects_que_.pop_front();
@@ -147,7 +147,8 @@ void InputManager::init(const std::vector<InputChannel> & input_channels)
   sub_objects_array_.resize(input_size_);
 
   for (size_t i = 0; i < input_size_; i++) {
-    InputStream input_stream(node_, i);
+    uint index(i);
+    InputStream input_stream(node_, index);
     input_stream.init(input_channels[i]);
     input_stream.setTriggerFunction(
       std::bind(&InputManager::onTrigger, this, std::placeholders::_1));
@@ -166,10 +167,10 @@ void InputManager::init(const std::vector<InputChannel> & input_channels)
   is_initialized_ = true;
 }
 
-void InputManager::onTrigger(const size_t & index) const
+void InputManager::onTrigger(const uint & index) const
 {
   // input stream index of 0 is the target(main) input stream
-  const size_t target_idx = 0;
+  const uint target_idx = 0;
 
   // when the main stream triggers, call the trigger function
   if (index == target_idx && func_trigger_) {
@@ -203,11 +204,11 @@ void InputManager::getObjectTimeInterval(
   }
 
   // Get proper latency
-  constexpr double target_latency = 0.15;  // [s], measurement to tracking latency
+  constexpr double target_latency = 0.18;  // [s], measurement to tracking latency
                                            // process latency of a main detection + margin
-
   constexpr double acceptable_latency =
-    0.35;  // [s], acceptable band from the target latency, larger than the target latency
+    0.32;  // [s], acceptable band from the target latency, larger than the target latency
+
   object_latest_time = now - rclcpp::Duration::from_seconds(target_latency);
   object_oldest_time = now - rclcpp::Duration::from_seconds(acceptable_latency);
 
@@ -217,8 +218,7 @@ void InputManager::getObjectTimeInterval(
     object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_;
 }
 
-bool InputManager::getObjects(
-  const rclcpp::Time & now, std::vector<std::pair<size_t, DetectedObjects>> & objects_list)
+bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_list)
 {
   if (!is_initialized_) {
     RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized");
@@ -241,7 +241,7 @@ bool InputManager::getObjects(
   // Sort objects by timestamp
   std::sort(
     objects_list.begin(), objects_list.end(),
-    [](const std::pair<size_t, DetectedObjects> & a, const std::pair<size_t, DetectedObjects> & b) {
+    [](const std::pair<uint, DetectedObjects> & a, const std::pair<uint, DetectedObjects> & b) {
       return (rclcpp::Time(a.second.header.stamp) - rclcpp::Time(b.second.header.stamp)).seconds() <
              0;
     });