/****************************************************************************/ /* Copyright (c) 2013 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : This process acts as a server to a TerrainNavClient object. */ /* The client/server arrangement allows the auv control system */ /* to use a remote TerrainNav object. */ /* Filename : trn_server.cpp */ /* Author : Rich Henthorn */ /* Project : */ /* Version : 1.0 */ /* Created : 02/17/2013 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ #include #include #include #include #include #include #include #include #include // For atoi() #include #include #include #include "structDefs.h" // Contains definitions of commsT class #include "TerrainNav.h" #include "genFilterDefs.h" // For the ARL debugging output below. Comment-out if // not desired // #define ARL_DEBUG #define NANOSEC_PER_SEC (1000000000L) #define TRN_DEBUG 0 #define MAX_RECV_ATTEMPTS 3 static TerrainNav* _tercom; static struct sockaddr_in _serv_addr; static int _servfd; // socket to bind static int _connfd; // socket to handle client static bool _connected; static struct commsT _ct; static struct commsT _ack(TRN_ACK); static struct commsT _nack(TRN_NACK); static struct sockaddr_in _server_addr; // Server Socket object static struct sockaddr_in _client_addr; static char _sock_buf[TRN_MSG_SIZE]; static char logbuf[2400]; static FILE* tlog; // Write the message to the log prepending a timestamp // void trn_log(const char* log_msg) { long t; time(&t); char* t_str = ctime(&t); t_str[strlen(t_str) - 1] = '\0'; fputs(t_str, tlog); fputs("=> ", tlog); fputs(log_msg, tlog); fputs("\n", tlog); fflush(tlog); } // Create a new log file each time a client connects // by appending 4-digit number to "trn.log" // void new_logfile(const char* directory) { char buf[200]; for (int i = 0; true; i++) { sprintf(buf, "%s/trn.log.%04d", directory, i); if (0 > access(buf, F_OK)) break; } // Create a new log file and write a header // if (tlog) { fclose(tlog); tlog = NULL; } printf("Opening log %s\n", buf); tlog = fopen(buf, "a"); trn_log("File created"); } // Return true/false if server has a connection to the client. // Uses select() to determine if the client has hung-up // bool is_connected() { // If we haven't been connected or the client closed the connection, // don't bother checking. // if(!_connected) { return _connected; } struct timeval tv; tv.tv_sec = 0; tv.tv_usec = 1000L; // Use select to see if the client closed the connection // fd_set clientfd; FD_ZERO(&clientfd); FD_SET(_connfd, &clientfd); char temp[5]; // If the socket is readable but there are no bytes, the client sent FIN // int nready = select(_connfd + 1, &clientfd, 0, 0, &tv); if(nready > 0) { if(0 == recv(_connfd, temp, 1, MSG_PEEK)) { _connected = false; printf("Client closed connection!\n"); trn_log("Client closed connection"); ::close(_connfd); } else { _connected = true; // Connected and there is data to read } } else { _connected = true; // Connected but no data to read } return _connected; } // Get a message from the socket connection. // Returns the length of the message packet read from the socket. // A length of zero indicates socket read timed-out. // int get_msg() { bool debug = false; int len = 0; // Get a message as long as client is still connected // if(is_connected()) { int ntries = MAX_RECV_ATTEMPTS; int sl = 0; for(len = 0; len < TRN_MSG_SIZE;) { if(ntries != 3) { trn_log("Get more after interrupted recv\n"); } sl = recv(_connfd, _sock_buf + sl, TRN_MSG_SIZE - sl, 0); if(sl <= 0) { sprintf(logbuf, "get_msg timeout, errno: %d, sl = %d", errno, sl); // or error perror(logbuf); trn_log(logbuf); if(errno == EINTR && ntries-- > 0) { // try again sprintf(logbuf, "%d: recv call interrupt after %d bytes.\n", MAX_RECV_ATTEMPTS - ntries, len); trn_log(logbuf); continue; } else { return 0; } } len += sl; } // Lengthly debugging output // if(len > 0 && (debug || TRN_DEBUG)) { for(int i = 0; i < 100; i++) { printf("%x ", _sock_buf[i]); } printf("\n"); } } return len; } // Sends a commsT object to client over socket connection. // int send_msg(commsT& msg) { int sl = 0; sprintf(logbuf, "Sending:%s", msg.to_s(_sock_buf, sizeof(_sock_buf))); trn_log(logbuf); // Check to see if client is still connected first // if(is_connected()) { memset(_sock_buf, 0, sizeof(_sock_buf)); msg.serialize(_sock_buf); // Send the whole message // for(sl = 0; sl < sizeof(_sock_buf);) { sl += send(_connfd, _sock_buf, sizeof(_sock_buf), 0); //printf("server:send_msg - sent %d bytes\n", sl); } } return sl; } // Initialize local TerrainNav object for operation. // int init() { // Destruct any existing current TerrainNav // if(_tercom) { delete _tercom; _tercom = 0; } // Construct a TerrainNav object using the info from the client // Use environment variables to find location of maps and datafiles. // char mapname[512], cfgname[512], particlename[512], logname[300]; char* mapPath = getenv("TRN_MAPFILES"); char* cfgPath = getenv("TRN_DATAFILES"); char dotSlash[] = "./"; if(!mapPath) { mapPath = dotSlash; //"./"; } if(!cfgPath) { cfgPath = dotSlash; //"./"; } /* if (!mapPath) mapPath = "./"; if (!cfgPath) cfgPath = "./"; */ sprintf(mapname, "%s/%s", mapPath, _ct.mapname); sprintf(cfgname, "%s/%s", cfgPath, _ct.cfgname); sprintf(particlename, "%s/%s", cfgPath, _ct.particlename); sprintf(logname, "%s", _ct.logname); // filter type and map type encoded in single integer // param = filter*100 + map // int mapType = _ct.parameter / 10; int filterType = _ct.parameter % 10; sprintf(logbuf, "Constructing tercom with map:%s, cfg:%s, map type: %d, and filter:%d", mapname, cfgname, mapType, filterType); trn_log(logbuf); printf("%s\n", logbuf); _tercom = new TerrainNav(mapname, cfgname, particlename, filterType, mapType, logname); // Acknowledge initialization if successful // if(_tercom->initialized()) { new_logfile(logname); sprintf(logbuf, "TerrainNav initialized, now do stuff"); trn_log(logbuf); send_msg(_ack); } else { sprintf(logbuf, "Failed to initialized TerrainNav object, map:%s cfg:%s", mapname, cfgname); printf("%s\n", logbuf); trn_log(logbuf); delete _tercom; // Uninitialized tercom is no good anyway _tercom = NULL; send_msg(_nack); } return 0; } // Forwarded Interpolated Measurement Attitude message // int set_ima() { sprintf(logbuf, "Setting IMA to %d", _ct.parameter); trn_log(logbuf); if(_tercom) { bool ima = _ct.parameter == 0 ? false : true; _tercom->setInterpMeasAttitude(ima); send_msg(_ack); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded Vehicle Drift Rate message // int set_vdr() { sprintf(logbuf, "Setting VDR to %f", _ct.vdr); trn_log(logbuf); if(_tercom) { _tercom->setVehicleDriftRate(_ct.vdr); send_msg(_ack); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded Modified Weighting message // int set_mw() { sprintf(logbuf, "Setting weighting to %d", _ct.parameter); trn_log(logbuf); if(_tercom) { bool mw = _ct.parameter == 0 ? false : true; _tercom->setModifiedWeighting(mw); send_msg(_ack); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded Filter Reinit message // int set_fr() { sprintf(logbuf, "Setting filter reinits to %d", _ct.parameter); trn_log(logbuf); if(_tercom) { bool fr = _ct.parameter == 0 ? false : true; _tercom->setFilterReinit(fr); send_msg(_ack); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded Map Interpolation message // int set_mim() { sprintf(logbuf, "Setting map interp method to %d", _ct.parameter); trn_log(logbuf); if(_tercom) { int mim = _ct.parameter; _tercom->setMapInterpMethod(mim); send_msg(_ack); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded Filter Gradient message // int filter_grd() { sprintf(logbuf, "Setting filter gradiant to %d", _ct.parameter); trn_log(logbuf); if(_tercom) { if(_ct.parameter == 0) { _tercom->useLowGradeFilter(); } else { _tercom->useHighGradeFilter(); } send_msg(_ack); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded Get Filter Type request // int filter_type() { trn_log("Returning filter type..."); if(_tercom) { _ack.parameter = _tercom->getFilterType(); sprintf(logbuf, "parameter = %d", _ack.parameter); trn_log(logbuf); send_msg(_ack); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded Filter State request // int filter_state() { trn_log("Returning filter state..."); if(_tercom) { _ack.parameter = _tercom->getFilterState(); sprintf(logbuf, "parameter = %d\n", _ack.parameter); trn_log(logbuf); send_msg(_ack); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded request for number of filter reinitializations // int num_reinits() { trn_log("Returning number of reinits..."); if(_tercom) { _ack.parameter = _tercom->getNumReinits(); sprintf(logbuf, "parameter = %d\n", _ack.parameter); trn_log(logbuf); send_msg(_ack); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded request for number of outstanding measurements // int out_meas() { trn_log("Returning outstanding measurements..."); if(_tercom) { if(_tercom->outstandingMeas()) { _ack.parameter = 1; } else { _ack.parameter = 0; } sprintf(logbuf, "parameter = %d", _ack.parameter); trn_log(logbuf); send_msg(_ack); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded request for last included measuerment // int last_meas() { trn_log("Returning last measurement..."); if(_tercom) { if(_tercom->lastMeasSuccessful()) { _ack.parameter = 1; } else { _ack.parameter = 0; } sprintf(logbuf, "parameter = %d\n", _ack.parameter); trn_log(logbuf); send_msg(_ack); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded request for convergence status // int is_conv() { trn_log("Returning converged"); if(_tercom) { if(_tercom->isConverged()) { _ack.parameter = 1; } else { _ack.parameter = 0; } sprintf(logbuf, "parameter = %d", _ack.parameter); trn_log(logbuf); send_msg(_ack); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } #ifdef ARL_DEBUG // Some debugging output from Stanford ARL // static poseT _currEst; #endif // Forwarded measure update message // int measure_update() { sprintf(logbuf, "Received measure update with time %f", _ct.mt.time); trn_log(logbuf); if(_tercom) { _ct.mt.alphas[0] = 0.123; _tercom->measUpdate(&_ct.mt, _ct.parameter); #ifdef ARL_DEBUG // Some debugging output from Stanford ARL // if (_tercom->lastMeasSuccessful()) { poseT mleEst, mmseEst; _tercom->estimatePose(&mleEst, 1); _tercom->estimatePose(&mmseEst, 2); printf("ARL : Estimation Bias(Max. Likelihood): (t = %.2f)\n", mleEst.time); printf("ARL : North: %.4f, East: %.4f, Depth: %.4f\n", mleEst.x-_currEst.x, mleEst.y-_currEst.y, mleEst.z-_currEst.z); printf("ARL : Estimation Bias (Mean) : (t = %.2f)\n", mmseEst.time); printf("ARL : North: %.4f, East: %.4f, Depth: %.4f\n", mmseEst.x-_currEst.x, mmseEst.y-_currEst.y, mmseEst.z-_currEst.z); } #endif // send_msg(_ack); // Send the measT object back to the client. The measT object // will contain the update alphas // // printf("Alphas[0] = %f\n", _ct.mt.alphas[0]); send_msg(_ct); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded multibeam update message // int multibeam_update() { sprintf(logbuf, "Received multibeam update with time %f", _ct.mb.time); trn_log(logbuf); sprintf(logbuf, "Received multibeam update with %d beams", _ct.mb.numBeams); trn_log(logbuf); sprintf(logbuf, "Received multibeam update with range of %.2f", _ct.mb.ranges[3]); trn_log(logbuf); if(_tercom) { sprintf(logbuf, "Received multibeam update with %d beams - nothing to do", _ct.mb.numBeams); trn_log(logbuf); //_tercom->mbUpdate(&_ct.mb, _ct.parameter); send_msg(_ack); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded motion update message // int motion_update() { sprintf(logbuf, "Received motion update with time %f", _ct.pt.time); trn_log(logbuf); if(_tercom) { _tercom->motionUpdate(&_ct.pt); #ifdef ARL_DEBUG _currEst = _ct.pt; // For debugging maintain the current position printf("INS : North: %.2f, East: %.2f, Depth: %.2f", _currEst.x, _currEst.y, _currEst.z); #endif send_msg(_ack); sprintf(logbuf, "motion update completed"); trn_log(logbuf); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded request for MLE estimated position // int send_mle() { trn_log("Client requests MLE..."); if(_tercom) { _tercom->estimatePose(&_ct.pt, 1); send_msg(_ct); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Forwarded request for MMSE estimated position // int send_mmse() { trn_log("Client requests MMSE..."); if(_tercom) { _tercom->estimatePose(&_ct.pt, 2); send_msg(_ct); } else { trn_log("No TRN object! Have you initialized yet?"); send_msg(_nack); } return 1; } // Main function for server process. // // Setup socket and listen for TRN client connection. When connected, // enter Message loop to read and handle messages from client. Loop is // exited when good-bye received or the connection is dropped by client. // Server returns to listening for connection. // int main(int argc, char** argv) { char c; int port = 27027; while((c = getopt(argc, argv, "p:")) != -1) switch(c) { case 'p': port = atoi(optarg); break; default: break; } tlog = NULL; _tercom = 0; int len = 0; // Socket setup section // _servfd = socket(AF_INET, SOCK_STREAM, 0); if(_servfd < 0) { exit(1); } memset(&_server_addr, '0', sizeof(_server_addr)); _server_addr.sin_family = AF_INET; _server_addr.sin_addr.s_addr = htonl(INADDR_ANY); _server_addr.sin_port = htons(port); len = bind(_servfd, (struct sockaddr*)&_server_addr, sizeof(_server_addr)); if(len < 0) { exit(1); } ///////////////////////////////////////////////////////////////////// // Server loop: Accept connection, service client until client is // done, repeat. // while(true) { time_t ticks; printf("Listen for TerrainNavClient connection with message size %d...\n", TRN_MSG_SIZE); listen(_servfd, 10); _connfd = accept(_servfd, (struct sockaddr*)NULL, NULL); _connected = true; struct timeval tv; tv.tv_sec = 180; tv.tv_usec = 0; new_logfile("./"); trn_log("Listen and accept"); setsockopt(_connfd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); trn_log("Client connected!"); /////////////////////////////////////////////////////////////////////// // Message loop: Receive and respond to messages from the client until // the client breaks the connection (closes the link or says goodbye). // while(_connected) { memset(_sock_buf, '0', sizeof(TRN_MSG_SIZE)); // Get a msg from the client // int len; if((len = get_msg()) < TRN_MSG_SIZE) { continue; } // Determine message type and respond // _ct.clean(); len = _ct.unserialize(_sock_buf, TRN_MSG_SIZE); sprintf(logbuf, "Server got %s", _ct.to_s(_sock_buf, TRN_MSG_SIZE)); trn_log(logbuf); // OK, we got a message, let's see if we have a tercom to // handle it // if (!_tercom && _ct.msg_type != TRN_INIT) { send_msg(_nack); printf("Not able to accept reqests: Server not initialized\n"); trn_log("Server not initialized"); continue; } switch(_ct.msg_type) { case TRN_BYE: printf("Client exiting connection\n"); //close(_connfd); //_connected = false; break; case TRN_INIT: init(); break; case TRN_SET_IMA: set_ima(); break; case TRN_SET_VDR: set_vdr(); break; case TRN_MEAS: measure_update(); break; case TRN_MB: multibeam_update(); break; case TRN_MOTN: motion_update(); break; case TRN_MLE: send_mle(); break; case TRN_MMSE: send_mmse(); break; case TRN_SET_MW: set_mw(); break; case TRN_SET_FR: set_fr(); break; case TRN_SET_MIM: set_mim(); break; case TRN_FILT_GRD: filter_grd(); break; case TRN_OUT_MEAS: out_meas(); break; case TRN_LAST_MEAS: last_meas(); break; case TRN_IS_CONV: is_conv(); break; case TRN_FILT_TYPE: filter_type(); break; case TRN_FILT_STATE: filter_state(); break; case TRN_N_REINITS: num_reinits(); break; case TRN_FILT_REINIT: _tercom->reinitFilter(true); sprintf(logbuf, "Filter Reinitialized.:%c", _ct.msg_type); printf("%s\n", logbuf); trn_log(logbuf); send_msg(_ack); break; case TRN_ACK: case TRN_NACK: default: sprintf(logbuf, "No handler for that message:%c", _ct.msg_type); printf("%s\n", logbuf); trn_log(logbuf); send_msg(_nack); } } // Exit after one go around for valgrind // exit(0); } return 0; }