feat: implement spawn switch

autowarefoundation · May 13, 2024 · 1f533f0 · 1f533f0
1 parent 34a3777
commit 1f533f0
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 2 deletions.
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -65,6 +65,8 @@ class InputStream
     long_name = long_name_;
     short_name = short_name_;
   }
+  bool isSpawnEnabled() const { return is_spawn_enabled_; }
+
   std::string getLongName() const { return long_name_; }
   size_t getObjectsCount() const { return objects_que_.size(); }
   void getTimeStatistics(
@@ -92,6 +94,7 @@ class InputStream
   std::string input_topic_;
   std::string long_name_;
   std::string short_name_;
+  bool is_spawn_enabled_;
 
   size_t que_size_{30};
   std::deque<DetectedObjects> objects_que_;
@@ -125,6 +128,10 @@ class InputManager
     rclcpp::Time & object_oldest_time) const;
   void optimizeTimings();
   bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list);
+  bool isChannelSpawnEnabled(const uint & index) const
+  {
+    return input_streams_[index]->isSpawnEnabled();
+  }
 
 private:
   rclcpp::Node & node_;

diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -342,7 +342,9 @@ void MultiObjectTracker::runProcess(
   processor_->prune(measurement_time);
 
   /* spawn new tracker */
-  processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index);
+  if (input_manager_->isChannelSpawnEnabled(channel_index)) {
+    processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index);
+  }
 }
 
 void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time)

diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -31,6 +31,7 @@ void InputStream::init(const InputChannel & input_channel)
   input_topic_ = input_channel.input_topic;
   long_name_ = input_channel.long_name;
   short_name_ = input_channel.short_name;
+  is_spawn_enabled_ = input_channel.is_spawn_enabled;
 
   // Initialize queue
   objects_que_.clear();
@@ -166,15 +167,17 @@ void InputManager::init(const std::vector<InputChannel> & input_channels)
     return;
   }
 
+  // Initialize input streams
   sub_objects_array_.resize(input_size_);
-
+  bool is_any_spawn_enabled = false;
   for (size_t i = 0; i < input_size_; 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));
     input_streams_.push_back(std::make_shared<InputStream>(input_stream));
+    is_any_spawn_enabled |= input_streams_.at(i)->isSpawnEnabled();
 
     // Set subscription
     RCLCPP_INFO(
@@ -186,6 +189,11 @@ void InputManager::init(const std::vector<InputChannel> & input_channels)
       input_channels[i].input_topic, rclcpp::QoS{1}, func);
   }
 
+  // Check if any spawn enabled input streams
+  if (!is_any_spawn_enabled) {
+    RCLCPP_ERROR(node_.get_logger(), "InputManager::init No spawn enabled input streams");
+    return;
+  }
   is_initialized_ = true;
 }