<commit_before>#include "SynchronousCameraQueue.hpp"

#include <ros/console.h>

#include "../../matlab/profiling.hpp"

SynchronousCameraQueue::SynchronousCameraQueue(long int arrivalDelay, long int maxDelay, long maxGroupInterval)
{
	this->maxDelay = maxDelay;
	this->arrivalDelay = arrivalDelay;
	this->maxGroupInterval = maxGroupInterval;
	lastArrivalTime = 0;
	minimumPictureTime = 0;
	
	ROS_INFO("SynchronousCameraQueue(): arrivalDelay = %.2fms, maxDelay = %.2fms, maxGroupInterval = %.2fms", arrivalDelay / 1e6, maxDelay / 1e6, maxGroupInterval / 1e6);
}

size_t SynchronousCameraQueue::getSize()
{
	return queue.size();
}

bool SynchronousCameraQueue::dataAvailable()
{
	long int currentTime = getNanoTime();
	
	if (queue.size() == 0) {
		return false;
	}
	
	if (queue.size() >= camNos.size()) {
		return true;
	}
	
	for (std::list<Bucket>::reverse_iterator it = queue.rbegin(); it != queue.rend(); it++) {
		if (currentTime - it->arrivalTime > maxDelay) {
			return true;
		}
		
		if (currentTime - it->arrivalTime > arrivalDelay && queue.size() > 2) {
			return true;
		}
	}
	
	return false;
}

void SynchronousCameraQueue::enqueueInternal(CameraData data)
{
	// Make it impossible to insert data for a time earlier than the last dequeue() result
	if (data.time <= minimumPictureTime) {
		ROS_DEBUG("Dropped data from cam %d, copter %d because it was too old", data.camNo, data.quadcopterId);
		return;
	} else {
		ROS_DEBUG("Inserting data from cam %d, copter %d", data.camNo, data.quadcopterId);
	}
	
	Bucket b;
	b.data = data;
	b.arrivalTime = getNanoTime();
	
	if (b.arrivalTime <= lastArrivalTime) {
		lastArrivalTime++;
		b.arrivalTime = lastArrivalTime;
	}
	
	std::list<Bucket> tmp;
	tmp.push_back(b);
	queue.merge(tmp);
	
	camNos.insert(data.camNo);
}<commit_msg>More debug output<commit_after>#include "SynchronousCameraQueue.hpp"

#include <ros/console.h>

#include "../../matlab/profiling.hpp"

SynchronousCameraQueue::SynchronousCameraQueue(long int arrivalDelay, long int maxDelay, long maxGroupInterval)
{
	this->maxDelay = maxDelay;
	this->arrivalDelay = arrivalDelay;
	this->maxGroupInterval = maxGroupInterval;
	lastArrivalTime = 0;
	minimumPictureTime = 0;
	
	ROS_INFO("SynchronousCameraQueue(): arrivalDelay = %.2fms, maxDelay = %.2fms, maxGroupInterval = %.2fms", arrivalDelay / 1e6, maxDelay / 1e6, maxGroupInterval / 1e6);
}

size_t SynchronousCameraQueue::getSize()
{
	return queue.size();
}

bool SynchronousCameraQueue::dataAvailable()
{
	long int currentTime = getNanoTime();
	
	if (queue.size() == 0) {
		return false;
	}
	
	if (queue.size() >= camNos.size()) {
		return true;
	}
	
	for (std::list<Bucket>::reverse_iterator it = queue.rbegin(); it != queue.rend(); it++) {
		if (currentTime - it->arrivalTime > maxDelay) {
			return true;
		}
		
		if (currentTime - it->arrivalTime > arrivalDelay && queue.size() > 2) {
			return true;
		}
	}
	
	return false;
}

void SynchronousCameraQueue::enqueueInternal(CameraData data)
{
	// Make it impossible to insert data for a time earlier than the last dequeue() result
	if (data.time <= minimumPictureTime) {
		ROS_DEBUG("Dropped data from cam %d, copter %d because it was too old (time: %ld)", data.camNo, data.quadcopterId, data.time);
		return;
	} else {
		ROS_DEBUG("Inserting data from cam %d, copter %d", data.camNo, data.quadcopterId);
	}
	
	Bucket b;
	b.data = data;
	b.arrivalTime = getNanoTime();
	
	if (b.arrivalTime <= lastArrivalTime) {
		lastArrivalTime++;
		b.arrivalTime = lastArrivalTime;
	}
	
	std::list<Bucket> tmp;
	tmp.push_back(b);
	queue.merge(tmp);
	
	camNos.insert(data.camNo);
}
{'fraction_non_alphanumeric': 0.1394700139470014, 'fraction_numerical': 0.007252440725244073, 'mean_word_length': 4.491577335375191, 'pattern_counts': {'":': 0, '<': 13, '<?xml version=': 0, '>': 27, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '364492', 'n_tokens_mistral': 1364, 'n_tokens_neox': 1240, 'n_words': 291}