void SendReceiver::Start ()
{
try {
tcp::iostream socketStream (ip.c_str(), port.c_str() ); // Trying to connect
if (!socketStream.fail() ) {
cout << "SendReceiver: Connected!" << endl; // TODO: write in log
#ifdef ENABLE_LOGGING
RAW_LOG (INFO, "SendReceiver: Connected!");
#endif
Sleep(6000);
while ( true ) {
boost::archive::xml_iarchive ia(socketStream); // We will receive Send objects in XML
boost::shared_ptr<Send> sendData (new Send); // Creating new Send object
ia >> BOOST_SERIALIZATION_NVP(sendData); // Receiving
sendBuffer->Enqueue (sendData); // Adding Send in buffer
}
}
}
catch (exception& e) {
cout << "SendReceiver: Exception: " << e.what () << endl; // TODO: write in log
#ifdef ENABLE_LOGGING
RAW_LOG (INFO, "SendReceiver: Exception: %s", e.what());
#endif
}
}
void CommandMaker::Start ()
{
try {
tcp::iostream socketStream (ip.c_str(), port.c_str() ); // Trying to connect
if (!socketStream.fail() ) {
cout << "CommandMaker: Connected!" << endl; // TODO: write in log
#ifdef ENABLE_LOGGING
RAW_LOG (INFO, "CommandMaker: Connected!");
#endif
boost::archive::xml_oarchive oa(socketStream); // We want to send commands in XML
Command com;
while (!socketStream.fail() ) {
// Reading command
cout << "input command type (int):" << endl;
cin >> com.ComType;
cout << "input command condition (int):" << endl;
cin >> com.ComCondition;
cout << "input condition value (float):" << endl;
cin >> com.Value;
oa << BOOST_SERIALIZATION_NVP(com); // Sending command
}
}
}
catch (exception& e) {
cout << "CommandMaker: Exception: " << e.what () << endl; // TODO: write in log
#ifdef ENABLE_LOGGING
RAW_LOG (INFO, "CommandMaker: Exception: %s", e.what());
#endif
}
}
void ClientReceiver::Start ()
{
try {
boost::asio::io_service io_service;
tcp::endpoint endpoint (tcp::v4 (), atoi(port.c_str()) );
tcp::acceptor acceptor (io_service, endpoint);
tcp::iostream socketStream;
cout << "ClientReceiver: Waiting for connection.." << endl; //TODO: write in log
RAW_LOG (INFO, "ClientReceiver: Waiting for connection..");
acceptor.accept (*socketStream.rdbuf ()); // waiting from connection from any IP
cout << "ClientReceiver: Connected!" << endl; //TODO: write in log
RAW_LOG (INFO, "ClientReceiver: Connected!");
boost::archive::xml_iarchive ia(socketStream); // We will receive objects in XML
Command com;
while (!socketStream.fail() ) {
ia >> BOOST_SERIALIZATION_NVP(com);
cout << "New command: " << com.ComType << " " << com.ComCondition << " " << com.Value << endl; // TODO: command buffer
RAW_LOG (INFO, "New command: %d %d %f", com.ComType, com.ComCondition, com.Value);
}
}
catch (exception& e) {
cout << "ClientReceiver: Exception: " << e.what () << endl; //TODO: write in log
RAW_LOG (INFO, "ClientReceiver: Exception: %s", e.what());
}
}
void KinectReceiver::Start ()
{
try {
tcp::iostream socketStream (ip.c_str(), port.c_str() ); // Trying to connect
if (!socketStream.fail() ) {
cout << "KinectReceiver: Connected!" << endl; // TODO: write in log
#ifdef ENABLE_LOGGING
RAW_LOG (INFO, "KinectReceiver: Connected!");
#endif
Sleep (5000);
while (true ) {
boost::shared_ptr<KinectData> kData (new KinectData); // Creating new KinectData
socketStream >> kData->Time; // Receivig time
octreeCoder->decodePointCloud (socketStream, kData->Cloud); // Then receiving point cloud
kinectBuffer->Enqueue (kData); // adding KinectData in KinectBuffer
}
}
}
catch (exception& e) {
cout << "KinectReceiver: Exception: " << e.what () << endl; // TODO: write in log
#ifdef ENABLE_LOGGING
RAW_LOG (INFO, "KinectReceiver: Exception: %s", e.what());
#endif
}
}
void CommandMaker::Start ()
{
try {
tcp::iostream socketStream (ip.toStdString().c_str(), port.toStdString().c_str() ); // Trying to connect
if (!socketStream.fail() ) {
cout << "CommandMaker: Connected!" << endl; // TODO: write in log
#ifdef ENABLE_LOGGING
RAW_LOG (INFO, "CommandMaker: Connected!");
#endif
boost::archive::xml_oarchive oa(socketStream); // We want to send commands in XML
Command com;
while (!socketStream.fail() ) {
// Reading command
cout << "input command type (int):" << endl;
cin >> com.ComType;
cout << "input command condition (int):" << endl;
cin >> com.ComCondition;
cout << "input condition value (float):" << endl;
cin >> com.Value;
QByteArray block;
QDataStream sendStream(&block, QIODevice::ReadWrite);
sendStream << quint16(0) << com;
sendStream.device()->seek(0);
sendStream << (quint16)(block.size() - sizeof(quint16));
socketStream.write(block,block.size());
//oa << BOOST_SERIALIZATION_NVP(com); // Sending command
}
}
}
catch (exception& e) {
cout << "CommandMaker: Exception: " << e.what () << endl; // TODO: write in log
#ifdef ENABLE_LOGGING
RAW_LOG (INFO, "CommandMaker: Exception: %s", e.what());
#endif
}
}
void SendSender::Start ()
{
try {
boost::asio::io_service io_service;
tcp::endpoint endpoint (tcp::v4 (), port);
tcp::acceptor acceptor (io_service, endpoint);
tcp::iostream socketStream;
cout << "SendSender: Waiting for connection.." << endl; //TODO: write in log
#ifdef ENABLE_LOGGING
RAW_LOG (INFO, "SendSender: Waiting for connection..");
#endif
acceptor.accept (*socketStream.rdbuf ()); // waiting from connection from any IP
cout << "SendSender: Connected!" << endl; //TODO: write in log
#ifdef ENABLE_LOGGING
RAW_LOG (INFO, "SendSender: Connected!");
#endif
while (!socketStream.fail() ) {
//boost::archive::xml_oarchive oa (socketStream); // We want to send objects in XML
boost::shared_ptr<Send> sendData;
sendData = buffer->Dequeue(); // Reading Send object from buffer
QByteArray block;
QDataStream sendStream(&block, QIODevice::ReadWrite);
sendStream << quint16(0) << sendData;
sendStream.device()->seek(0);
sendStream << (quint16)(block.size() - sizeof(quint16));
socketStream.write(block,block.size());
//oa << BOOST_SERIALIZATION_NVP (sendData); // Serializing and sending it
}
}
catch (exception& e) {
cout << "SendSender: Exception: " << e.what () << endl; //TODO: write in log
#ifdef ENABLE_LOGGING
RAW_LOG (INFO, "SendSender: Exception: %s", e.what());
#endif
}
}
inline void handler(int signal, siginfo_t *siginfo, void *context)
{
if (signal == SIGTERM) {
if (siginfo->si_code == SI_USER ||
siginfo->si_code == SI_QUEUE ||
siginfo->si_code <= 0) {
RAW_LOG(WARNING, "Received signal SIGTERM from process %d of user %d; "
"exiting", siginfo->si_pid, siginfo->si_uid);
} else {
RAW_LOG(WARNING, "Received signal SIGTERM; exiting");
}
// Setup the default handler for SIGTERM so that we don't print
// a stack trace.
os::signals::reset(signal);
raise(signal);
} else {
RAW_LOG(FATAL, "Unexpected signal in signal handler: %d", signal);
}
}
// NOTE: We use RAW_LOG instead of LOG because RAW_LOG doesn't
// allocate any memory or grab locks. And according to
// https://code.google.com/p/google-glog/issues/detail?id=161
// it should work in 'most' cases in signal handlers.
void handler(int signal)
{
if (signal == SIGTERM) {
RAW_LOG(WARNING, "Received signal SIGTERM; exiting.");
// Setup the default handler for SIGTERM so that we don't print
// a stack trace.
struct sigaction action;
memset(&action, 0, sizeof(action));
sigemptyset(&action.sa_mask);
action.sa_handler = SIG_DFL;
sigaction(signal, &action, NULL);
raise(signal);
} else if (signal == SIGPIPE) {
RAW_LOG(WARNING, "Received signal SIGPIPE; escalating to SIGABRT");
raise(SIGABRT);
} else {
RAW_LOG(FATAL, "Unexpected signal in signal handler: %d", signal);
}
}
// NOTE: We use RAW_LOG instead of LOG because RAW_LOG doesn't
// allocate any memory or grab locks. And according to
// https://code.google.com/p/google-glog/issues/detail?id=161
// it should work in 'most' cases in signal handlers.
inline void handler(int signal)
{
RAW_LOG(FATAL, "Unexpected signal in signal handler: %d", signal);
}
/**
* sending_buffer_ contains one block from every partition (e.t. socket
* connection), if one block is send completely, supply one from
* partitioned_data_buffer_
*/
void* ExchangeSenderPipeline::Sender(void* arg) {
ExchangeSenderPipeline* Pthis =
reinterpret_cast<ExchangeSenderPipeline*>(arg);
pthread_testcancel();
// LOG(INFO) << "(exchange_id = " << Pthis->state_.exchange_id_
// << " , partition_offset = " << Pthis->state_.partition_offset_
// << " ) sender thread created successfully!";
RAW_LOG(INFO,
"exchange_id= %d, par_off= %d sender thread is created successfully!",
Pthis->state_.exchange_id_, Pthis->state_.partition_offset_);
Pthis->sending_buffer_->Initialized();
Pthis->sendedblocks_ = 0;
try {
while (true) {
pthread_testcancel();
bool consumed = false;
BlockContainer* block_for_sending = NULL;
int partition_id =
Pthis->sending_buffer_->getBlockForSending(block_for_sending);
if (partition_id >= 0) {
// get one block from sending_buffer which isn't empty
pthread_testcancel();
if (block_for_sending->GetRestSizeToHandle() > 0) {
int recvbytes;
recvbytes =
send(Pthis->socket_fd_upper_list_[partition_id],
reinterpret_cast<char*>(block_for_sending->getBlock()) +
block_for_sending->GetCurSize(),
block_for_sending->GetRestSizeToHandle(), MSG_DONTWAIT);
if (recvbytes == -1) {
if (errno == EAGAIN) {
continue;
}
LOG(ERROR) << "(exchange_id = " << Pthis->state_.exchange_id_
<< " , partition_offset = "
<< Pthis->state_.partition_offset_
<< " ) sender send error: " << errno
<< " fd = " << Pthis->socket_fd_upper_list_[partition_id]
<< std::endl;
break;
} else {
if (recvbytes < block_for_sending->GetRestSizeToHandle()) {
/* the block is not entirely sent. */
#ifdef GLOG_STATUS
LOG(INFO)
<< "(exchange_id = " << Pthis->state_.exchange_id_
<< " , partition_offset = " << Pthis->state_.partition_offset_
<< " ) doesn't send a block completely, actual send bytes = "
<< recvbytes << " rest bytes = "
<< block_for_sending->GetRestSizeToHandle() << std::endl;
#endif
block_for_sending->IncreaseActualSize(recvbytes);
continue;
} else {
/** the block is sent in entirety. **/
block_for_sending->IncreaseActualSize(recvbytes);
++Pthis->sendedblocks_;
/*
can not be executed in case of abort in glog in this phase
one of the following should be executed after rewriting
Pthis->logging_->log(
"[%llu,%u]Send the %u block(bytes=%d, rest
size=%d) to [%d]",
Pthis->state_.exchange_id_,
Pthis->state_.partition_offset_,
Pthis->sendedblocks_, recvbytes,
block_for_sending_->GetRestSize(),
Pthis->state_.upper_id_list_[partition_id]);
LOG(INFO) << "[ExchangeEagerLower]: "
<< "[" << Pthis->state_.exchange_id_ << ","
<< Pthis->state_.partition_offset_ << "]Send the "
<< Pthis->sendedblocks_ << " block(bytes=" << recvbytes
<< ", rest size=" <<
block_for_sending->GetRestSizeToHandle()
<< ") to ["
<< Pthis->state_.upper_id_list_[partition_id] << "]"
<< std::endl;
cout << "[ExchangeEagerLower]: " << "["
<< Pthis->state_.exchange_id_ << ","
<< Pthis->state_.partition_offset_ << "]Send
the "
<< Pthis->sendedblocks_ << " block(bytes=" <<
recvbytes
<< ", rest size=" <<
block_for_sending_->GetRestSize()
<< ") to [" <<
Pthis->state_.upper_id_list_[partition_id]
<< "]" << std::endl;
*/
consumed = true;
}
}
} else {
consumed = true;
}
} else {
/* "partition_id<0" means that block_for_sending_ is empty, so we get
* one block from the buffer into the block_for_sending_
*/
unsigned index = Pthis->partitioned_data_buffer_->getBlock(
*Pthis->block_for_sending_buffer_);
Pthis->block_for_sending_buffer_->reset();
Pthis->sending_buffer_->insert(index, Pthis->block_for_sending_buffer_);
}
if (consumed == true) {
/* In the current loop, we have sent an entire block to the Receiver,
* so we should get a new block into the block_for_sender_, but note
* one empty block is also appended in partitioned_data_buffer_ in
* next()
*/
pthread_testcancel();
if (Pthis->partitioned_data_buffer_->getBlock(
*Pthis->block_for_sending_buffer_, partition_id)) {
Pthis->block_for_sending_buffer_->reset();
Pthis->sending_buffer_->insert(partition_id,
Pthis->block_for_sending_buffer_);
} else {
/**
* TODO: test the effort of the following sleeping statement and
* consider whether it should be replaced by conditioned wait
**/
usleep(1);
}
}
}
} catch (std::exception& e) {
pthread_cancel(pthread_self());
}
}
void NanoController::Start(void)
{
#ifdef ENABLE_LOGGING
RAW_LOG(INFO, "NanoController: started");
#endif
#ifdef NANO_FPS_TEST
int count4 = 0;
time_t FirstData;
#endif
unsigned char *ReadenData;
unsigned short *PShortData;
unsigned char DataPacket[10];
int NextByte;
int stage = -4;
int DataLength;
unsigned int Counter = 0;
while (true){
std::this_thread::sleep_for(std::chrono::milliseconds(1));
Counter++;
if ((readyToNewMessage == false)&&(Counter > 100)){
DataLength = 0;
while (dataToSend[DataLength] != '\0') DataLength++;
#ifdef ENABLE_LOGGING
RAW_LOG(INFO, "NanoController: updating GPS message...");
#endif
nanoCom->Write(dataToSend, DataLength);
Counter = 0;
readyToNewMessage = true;
}
ReadenData = (unsigned char *)nanoCom->Read();
if (nanoCom->GetOutSize() > 0){
for (int i = 0; i<nanoCom->GetOutSize(); i++){
#ifdef NANO_INPUT_DATA_TEST
printf("%c",ReadenData[i]);
#endif
printf("!");
if (stage == -4){
if (ReadenData[i] == 131){
stage++;
continue;
}else{
continue;
}
}
if (stage == -3){
if (ReadenData[i] == 130){
stage++;
continue;
}else{
stage = -4;
continue;
}
}
if (stage == -2){
if (ReadenData[i] == 129){
stage++;
continue;
}else{
stage = -4;
continue;
}
}
if (stage == -1){
if (ReadenData[i] == 128){
stage++;
NextByte = 0;
continue;
}else{
stage = -4;
continue;
}
}
if (stage == 0){
if (NextByte<10){
DataPacket[NextByte] = ReadenData[i];
NextByte++;
}else{
stage = -4;
PShortData = (unsigned short *)&DataPacket[0];
#ifdef BOOST
boost::shared_ptr<NanoReceived> NanoData (new NanoReceived());
#else
std::shared_ptr<NanoReceived> NanoData (new NanoReceived());
#endif // BOOST
NanoData->FrontSonicSensor = PShortData[0];
NanoData->RightSonicSensor = PShortData[1];
NanoData->BackSonicSensor = PShortData[2];
NanoData->LeftSonicSensor = PShortData[3];
NanoData->TopSonicSensor = PShortData[4];
#ifdef ENABLE_LOGGING
RAW_LOG(INFO, "NanoController: got data - front = %d, right = %d, back = %d, left = %d, top = %d",PShortData[0],PShortData[1],PShortData[2],PShortData[3],PShortData[4]);
#endif
#ifdef NANO_FPS_TEST
if (count4 == 0){
FirstData = time(NULL);
}else{
if (count4 % 64 == 63){
time_t cur = time(NULL);
printf("%f\n",(float)count4/difftime(cur,FirstData));
}
}
count4++;
#endif
#ifdef NANO_TELEMETRY_TEST
printf("%d %d %d %d %d\n",PShortData[0],PShortData[1],PShortData[2],PShortData[3],PShortData[4]);
#endif
NanoData->Time = time(NULL);
buffer->Enqueue(NanoData);
}
}
}
lastReadTime = time(NULL);
}else{
if (difftime(time(NULL),lastReadTime)>NANO_SECONDS_TO_RECONNECT){
nanoCom->~SerialCom();
#ifdef ENABLE_LOGGING
RAW_LOG(INFO, "NanoController: reconnecting...");
#endif
char *cstr = new char[nanoPort.length() + 1];
strcpy(cstr, nanoPort.c_str());
nanoCom = new SerialCom(cstr, NANO_BAUD_RATE);
lastReadTime = time(NULL);
stage = -4;
}
}
}
}
