<commit_before>#include "TrackingWorker.hpp"

#include <boost/chrono/duration.hpp>
#include <ros/console.h>
#include <opencv2/core/core.hpp>
#include <map>

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

TrackingWorker::TrackingWorker(IPositionReceiver *receiver) : errorGraph(100, "Difference")
{
	assert(receiver != 0);
	
	this->receiver = receiver;
	stop = false;
	
	std::map<int, cv::Scalar> colors;
	colors[0] = cv::Scalar(0, 255, 0);
	colors[1] = cv::Scalar(0, 255, 255);
	colors[2] = cv::Scalar(255, 0, 255);
	errorGraph.setColors(colors);
	
	thread = new boost::thread(boost::bind(&TrackingWorker::run, this));
}

TrackingWorker::~TrackingWorker()
{
	stop = true;
	thread->join();
}

void TrackingWorker::run()
{
	ROS_INFO("Started tracking thread");
	
	bool receivedFirstPosition = false;
	int emptyCount = 0;
	
	while (!stop) {
		std::vector<CameraData> data = dequeue();
		
		if (data.size() > 0) {
			emptyCount = 0;
			
			if (!receivedFirstPosition) {
				receivedFirstPosition = true;
				ROS_INFO("Found quadcopter %d", data[0].quadcopterId);
			}
			
			// ROS_DEBUG("Got info from camera %d: [%.2f, %.2f, %.2f]", data[0].camNo, data[0].cameraVector.getV1(), data[0].cameraVector.getV2(), data[0].cameraVector.getV3());
			
			Vector position = tracker.updatePosition(data);
			
			for (size_t i = 0; i < data.size(); i++) {
				errorGraph.nextPoint(tracker.getDistance(), data[i].camNo);
			}
			
			
			if (position.isValid()) {
				// Invert x axis for controller
				position.setV1(-position.getV1());
				
				std::vector<Vector> positions;
				std::vector<int> ids;
				std::vector<int> updates;
				positions.push_back(position);
				ids.push_back(data[0].quadcopterId);
				updates.push_back(1);
				
				receiver->updatePositions(positions, ids, updates);
			}
		} else if (receivedFirstPosition) {
			emptyCount++;
			
			if (emptyCount == 5) {
				ROS_WARN("Position update buffer is empty!");
				emptyCount = 0;
			}<commit_msg>Changed position buffer empty message threshold<commit_after>#include "TrackingWorker.hpp"

#include <boost/chrono/duration.hpp>
#include <ros/console.h>
#include <opencv2/core/core.hpp>
#include <map>

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

TrackingWorker::TrackingWorker(IPositionReceiver *receiver) : errorGraph(100, "Difference")
{
	assert(receiver != 0);
	
	this->receiver = receiver;
	stop = false;
	
	std::map<int, cv::Scalar> colors;
	colors[0] = cv::Scalar(0, 255, 0);
	colors[1] = cv::Scalar(0, 255, 255);
	colors[2] = cv::Scalar(255, 0, 255);
	errorGraph.setColors(colors);
	
	thread = new boost::thread(boost::bind(&TrackingWorker::run, this));
}

TrackingWorker::~TrackingWorker()
{
	stop = true;
	thread->join();
}

void TrackingWorker::run()
{
	ROS_INFO("Started tracking thread");
	
	bool receivedFirstPosition = false;
	int emptyCount = 0;
	
	while (!stop) {
		std::vector<CameraData> data = dequeue();
		
		if (data.size() > 0) {
			emptyCount = 0;
			
			if (!receivedFirstPosition) {
				receivedFirstPosition = true;
				ROS_INFO("Found quadcopter %d", data[0].quadcopterId);
			}
			
			// ROS_DEBUG("Got info from camera %d: [%.2f, %.2f, %.2f]", data[0].camNo, data[0].cameraVector.getV1(), data[0].cameraVector.getV2(), data[0].cameraVector.getV3());
			
			Vector position = tracker.updatePosition(data);
			
			for (size_t i = 0; i < data.size(); i++) {
				errorGraph.nextPoint(tracker.getDistance(), data[i].camNo);
			}
			
			
			if (position.isValid()) {
				// Invert x axis for controller
				position.setV1(-position.getV1());
				
				std::vector<Vector> positions;
				std::vector<int> ids;
				std::vector<int> updates;
				positions.push_back(position);
				ids.push_back(data[0].quadcopterId);
				updates.push_back(1);
				
				receiver->updatePositions(positions, ids, updates);
			}
		} else if (receivedFirstPosition) {
			emptyCount++;
			
			if (emptyCount == 50) {
				ROS_WARN("Position update buffer is empty!");
				emptyCount = 0;
			}
{'fraction_non_alphanumeric': 0.1687420584498094, 'fraction_numerical': 0.024650571791613722, 'mean_word_length': 4.00763358778626, 'pattern_counts': {'":': 0, '<': 23, '<?xml version=': 0, '>': 29, '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': '2544914', 'n_tokens_mistral': 1647, 'n_tokens_neox': 1354, 'n_words': 271}