Commit 38743b64 authored by Ian McInerney's avatar Ian McInerney

Added logging of agent position in computation log

parent 4e761445
......@@ -25,6 +25,10 @@ class LocalizationLogger{
private:
computation_localization_node_data *nodeData;
float *agentPositionX;
float *agentPositionY;
float *agentPositionZ;
int numAgents;
float targetX;
......@@ -57,6 +61,8 @@ public:
static void VRPN_CALLBACK vrpnTrackerCallback(void *logger, const vrpn_TRACKERCB t);
void setAgentPosition(int agent, float x, float y, float z);
void setTargetPosition(float x, float y, float z);
void logData();
......
......@@ -42,6 +42,7 @@ public:
void clearNetworkQueue(int node);
void setNetworkLogger(LocalizationLogger *logger);
LocalizationLogger* getNetworkLogger();
/*
* Add an edge to the graph
......
......@@ -151,6 +151,12 @@ void CCrazyflie::setCameraPosition(lateralPosition cameraPosition) {
this->m_currentLocalPosition.y = cameraPosition.y - this->m_localOrigin.y;
this->m_currentLocalPosition.z = cameraPosition.z - this->m_localOrigin.z;
// Update the position in the network logger
this->m_network->getNetworkLogger()->setAgentPosition(this->m_quadNum,
cameraPosition.x,
cameraPosition.y,
cameraPosition.z);
this->m_sendPosition = true;
pthread_mutex_unlock( &(this->cameraDataMutex) );
}
......
......@@ -30,6 +30,11 @@ LocalizationLogger::LocalizationLogger(int numAgents, char* fileName, double log
this->startTime = (tsTime.tv_sec + double(tsTime.tv_nsec) / NSEC_PER_SEC);
this->nextLogTime = this->startTime;
// Create the arrays to hold the agent position
this->agentPositionX = new float[numAgents]();
this->agentPositionY = new float[numAgents]();
this->agentPositionZ = new float[numAgents]();
// Create the array to hold the data from the nodes and zero out the data
this->nodeData = new computation_localization_node_data[numAgents]();
for (int i=0; i < numAgents; i++) {
......@@ -40,6 +45,10 @@ LocalizationLogger::LocalizationLogger(int numAgents, char* fileName, double log
this->nodeData[i].alpha = 0;
this->nodeData[i].beta = 0;
this->nodeData[i].gamma = 0;
this->agentPositionX[i] = 0;
this->agentPositionY[i] = 0;
this->agentPositionZ[i] = 0;
}
// Open the file
......@@ -69,6 +78,9 @@ LocalizationLogger::LocalizationLogger(int numAgents, char* fileName, double log
// Create the field names
this->file_log << "\%Time\t\tTargetX\t\tTargetY\t\tTargetZ";
for (int i = 0; i < numAgents; i++) {
this->file_log << "\t\tNode" << i << "_positionX";
this->file_log << "\t\tNode" << i << "_positionY";
this->file_log << "\t\tNode" << i << "_positionZ";
this->file_log << "\t\tNode" << i << "_measuredRadius";
this->file_log << "\t\tNode" << i << "_estimatedX";
this->file_log << "\t\tNode" << i << "_estimatedY";
......@@ -82,6 +94,9 @@ LocalizationLogger::LocalizationLogger(int numAgents, char* fileName, double log
// Create the field units
this->file_log << "&sec\t\tmeters\t\tmeters\t\tmeters";
for (int i = 0; i < numAgents; i++) {
this->file_log << "\t\tmeters";
this->file_log << "\t\tmeters";
this->file_log << "\t\tmeters";
this->file_log << "\t\tmeters";
this->file_log << "\t\tmeters";
this->file_log << "\t\tmeters";
......@@ -97,7 +112,10 @@ LocalizationLogger::LocalizationLogger(int numAgents, char* fileName, double log
LocalizationLogger::~LocalizationLogger() {
// Delete the array holding the data
delete[] nodeData;
delete[] this->nodeData;
delete[] this->agentPositionX;
delete[] this->agentPositionY;
delete[] this->agentPositionZ;
// Close the logger file
this->file_log.close();
......@@ -115,6 +133,16 @@ void VRPN_CALLBACK LocalizationLogger::vrpnTrackerCallback(void *logger, const v
log->setTargetPosition(x, y, z);
}
void LocalizationLogger::setAgentPosition(int agent, float x, float y, float z) {
// Make sure that the agent actually exists before setting the position
if (agent < this->numAgents) {
this->agentPositionX[agent] = x;
this->agentPositionY[agent] = y;
this->agentPositionZ[agent] = z;
}
}
void LocalizationLogger::setTargetPosition(float x, float y, float z) {
this->targetX = x;
this->targetY = y;
......@@ -148,6 +176,10 @@ void LocalizationLogger::logData() {
// Log the agent data
for (int i = 0; i < numAgents; i++) {
this->file_log << "\t\t" << this->agentPositionX[i];
this->file_log << "\t\t" << this->agentPositionY[i];
this->file_log << "\t\t" << this->agentPositionZ[i];
this->file_log << "\t\t" << this->nodeData[i].measuredRadius;
this->file_log << "\t\t" << this->nodeData[i].estimatedX;
this->file_log << "\t\t" << this->nodeData[i].estimatedY;
......
......@@ -74,6 +74,9 @@ void NetworkForwarding::setNetworkLogger(LocalizationLogger *logger) {
this->networkLogger = logger;
}
LocalizationLogger* NetworkForwarding::getNetworkLogger() {
return( this->networkLogger );
}
/*
* Add an edge to the graph
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment