void Ev3LcmCommunicator::sendStatusMessage(){
pthread_mutex_lock(&mutex);
//cout << "--> Ev3 Send Status" << endl;
IntBuffer buffer;
buffer.push_back(STATUS_MESSAGE);
StatusList statuses;
ev3Manager->writeStatus(statuses);
buffer.push_back(statuses.size());
for(uint i = 0; i < statuses.size(); i++){
statuses[i].packStatus(buffer);
}
// Create + send outgoing buffer
uchar* outBuffer;
uint buf_size;
packBuffer(buffer, outBuffer, buf_size);
wrapper.publish(outChannel, (void*)outBuffer, buf_size);
delete [] outBuffer;
//cout << "<-- Ev3 Send Status" << endl;
pthread_mutex_unlock(&mutex);
}
void SoarManager::readStatus(StatusList& statuses){
for(unsigned int i = 0; i < statuses.size(); i++){
Ev3Status& status = statuses[i];
uint offset = 0;
//cout << "STATUS FOR: " << status.dev << endl;
if(status.dev == BRICK_DEV){
brick->readStatus(status.info, offset);
} else if(status.dev == OUTPUT_MAN_DEV){
readMotorsStatus(status.info);
} else if(status.dev == INPUT_MAN_DEV){
readSensorsStatus(status.info);
} else if(status.dev == MANAGER_DEV){
} else {
cout << "UNKNOWN DEVICE " << status.dev << endl;
}
}
}