Skip to content

Commit

Permalink
fix: remove debug messages
Browse files Browse the repository at this point in the history
  • Loading branch information
technolojin committed May 13, 2024
1 parent 1f533f0 commit 725a49e
Show file tree
Hide file tree
Showing 2 changed files with 0 additions and 52 deletions.
38 changes: 0 additions & 38 deletions perception/multi_object_tracker/src/multi_object_tracker_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -255,44 +255,6 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
}
// process end
debugger_->endMeasurementTime(this->now());

/* DEBUG */
const rclcpp::Time latest_time(objects_list.back().second.header.stamp);
RCLCPP_INFO(
this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f",
(current_time - latest_time).seconds(), (current_time - oldest_time).seconds());

// count objects on each channel
std::vector<int> object_counts;
object_counts.resize(input_channel_size_);
// initialize to zero
for (size_t i = 0; i < input_channel_size_; i++) {
object_counts[i] = 0;
}
for (const auto & objects_data : objects_list) {
object_counts[objects_data.first] += 1;
}
// print result
std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: ";
for (size_t i = 0; i < input_channel_size_; i++) {
object_counts_str +=
input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + " ";
}
RCLCPP_INFO(this->get_logger(), object_counts_str.c_str());

std::string object_info_str = "";
object_info_str += "MultiObjectTracker::onMessage Objects channel and time: \n";
for (const auto & objects_data : objects_list) {
const auto & objects = objects_data.second;
const auto & channel_index = objects_data.first;
const auto & time = rclcpp::Time(objects.header.stamp);
// print object information
object_info_str += input_channels_[channel_index].long_name + " " +
std::to_string((current_time - time).seconds()) + " \n";
}
RCLCPP_INFO(this->get_logger(), object_info_str.c_str());

/* DEBUG END */
}

void MultiObjectTracker::runProcess(
Expand Down
14 changes: 0 additions & 14 deletions perception/multi_object_tracker/src/processor/input_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -253,20 +253,6 @@ void InputManager::optimizeTimings()
selected_stream_interval = interval_mean;
selected_stream_interval_std = std::sqrt(interval_var);
}

/* DEBUG */
std::string long_name, short_name;
rclcpp::Time latest_measurement_time, latest_message_time;
input_stream->getNames(long_name, short_name);
input_stream->getTimestamps(latest_measurement_time, latest_message_time);
double latency_message = (node_.now() - latest_message_time).seconds();
double latency_measurement = (node_.now() - latest_measurement_time).seconds();
RCLCPP_INFO(
node_.get_logger(),
"InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: "
"%f, std: %f, latest measurement latency: %f, latest message latency: %f",
long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean,
std::sqrt(interval_var), latency_measurement, latency_message);
}
}

Expand Down

0 comments on commit 725a49e

Please sign in to comment.