Skip to content

Commit 1f533f0

Browse files
committed
feat: implement spawn switch
1 parent 34a3777 commit 1f533f0

File tree

3 files changed

+19
-2
lines changed

3 files changed

+19
-2
lines changed

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

+7
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,8 @@ class InputStream
6565
long_name = long_name_;
6666
short_name = short_name_;
6767
}
68+
bool isSpawnEnabled() const { return is_spawn_enabled_; }
69+
6870
std::string getLongName() const { return long_name_; }
6971
size_t getObjectsCount() const { return objects_que_.size(); }
7072
void getTimeStatistics(
@@ -92,6 +94,7 @@ class InputStream
9294
std::string input_topic_;
9395
std::string long_name_;
9496
std::string short_name_;
97+
bool is_spawn_enabled_;
9598

9699
size_t que_size_{30};
97100
std::deque<DetectedObjects> objects_que_;
@@ -125,6 +128,10 @@ class InputManager
125128
rclcpp::Time & object_oldest_time) const;
126129
void optimizeTimings();
127130
bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list);
131+
bool isChannelSpawnEnabled(const uint & index) const
132+
{
133+
return input_streams_[index]->isSpawnEnabled();
134+
}
128135

129136
private:
130137
rclcpp::Node & node_;

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

+3-1
Original file line numberDiff line numberDiff line change
@@ -342,7 +342,9 @@ void MultiObjectTracker::runProcess(
342342
processor_->prune(measurement_time);
343343

344344
/* spawn new tracker */
345-
processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index);
345+
if (input_manager_->isChannelSpawnEnabled(channel_index)) {
346+
processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index);
347+
}
346348
}
347349

348350
void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time)

perception/multi_object_tracker/src/processor/input_manager.cpp

+9-1
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@ void InputStream::init(const InputChannel & input_channel)
3131
input_topic_ = input_channel.input_topic;
3232
long_name_ = input_channel.long_name;
3333
short_name_ = input_channel.short_name;
34+
is_spawn_enabled_ = input_channel.is_spawn_enabled;
3435

3536
// Initialize queue
3637
objects_que_.clear();
@@ -166,15 +167,17 @@ void InputManager::init(const std::vector<InputChannel> & input_channels)
166167
return;
167168
}
168169

170+
// Initialize input streams
169171
sub_objects_array_.resize(input_size_);
170-
172+
bool is_any_spawn_enabled = false;
171173
for (size_t i = 0; i < input_size_; i++) {
172174
uint index(i);
173175
InputStream input_stream(node_, index);
174176
input_stream.init(input_channels[i]);
175177
input_stream.setTriggerFunction(
176178
std::bind(&InputManager::onTrigger, this, std::placeholders::_1));
177179
input_streams_.push_back(std::make_shared<InputStream>(input_stream));
180+
is_any_spawn_enabled |= input_streams_.at(i)->isSpawnEnabled();
178181

179182
// Set subscription
180183
RCLCPP_INFO(
@@ -186,6 +189,11 @@ void InputManager::init(const std::vector<InputChannel> & input_channels)
186189
input_channels[i].input_topic, rclcpp::QoS{1}, func);
187190
}
188191

192+
// Check if any spawn enabled input streams
193+
if (!is_any_spawn_enabled) {
194+
RCLCPP_ERROR(node_.get_logger(), "InputManager::init No spawn enabled input streams");
195+
return;
196+
}
189197
is_initialized_ = true;
190198
}
191199

0 commit comments

Comments
 (0)