/****************************************************************************/ /* 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 // For atoi() #include #include #include #include "structDefs.h" // Contains definitions of commsT class #include "TerrainNav.h" #include "genFilterDefs.h" #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; void trn_log(const char* log_msg) { long t; time(&t); char *t_str = ctime(&t); t_str[strlen(t_str)-1] = '\0'; if (!tlog) { tlog = fopen("trn.log", "a"); fputs("\n\n\t\t************ ", tlog); fputs(t_str, tlog); fputs(" ************\n\n", tlog); } fputs(t_str, tlog); fputs("=> ", tlog); fputs(log_msg, tlog); fputs("\n", tlog); fflush(tlog); } // 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]; char *mapPath = getenv("TRN_MAPFILES"); char *cfgPath = getenv("TRN_DATAFILES"); if (!mapPath) mapPath = "./"; if (!cfgPath) cfgPath = "./"; sprintf(mapname, "%s/%s", mapPath, _ct.mapname); sprintf(cfgname, "%s/%s", cfgPath, _ct.cfgname); // 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, filterType, mapType); // Acknowledge initialization if successful // if (_tercom->initialized()) { 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; } // Forwarded measure update message // int measure_update() { sprintf(logbuf, "Received measure update with time %f", _ct.mt.time); trn_log(logbuf); if (_tercom) { _tercom->measUpdate(&_ct.mt, _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); 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...\n"); trn_log("Listen and accept"); listen(_servfd, 10); _connfd = accept(_servfd, (struct sockaddr*)NULL, NULL); _connected = true; struct timeval tv; tv.tv_sec = 180; tv.tv_usec = 0; setsockopt(_connfd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); /////////////////////////////////////////////////////////////////////// // 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_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_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); } } } return 0; }