Solved: How to detect lost connection on UDP Server with

OK, I *hope* to have found the issue (unfortunately, I can’t check it, but try#include #include “Logger.h” #include “Aria.h” #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define MAX_INTERVAL 1 using boost::asio::ip::udp; using namespace std; float temp_heading, temp_distance; struct DataLine { float nNumber; std::string sName; }; DataLine output; clock_t prev = 0; clock_t now; float distanza_minima = 1500; float offset = 900; std::string comando; float valore; float heading_prova = 0; float distance_prova = 0; const int max_length = 100; int integer_value; short int porta; float last_distance = 0.0; bool discovery = true; bool right_disc = false; bool left_disc = false; bool start = false; std::string file; FILE *file_open; bool call_fprintf = true; double interval; timeval t1, t2, tempo; double elapsedTime; bool is_equal(float a, float b) { return (fabs(a – b) < std::numeric_limits::epsilon()); } ArRobot robot; ArSick sick; bool newGoal=false; ArPose m_goal; void alarm_handler_to_check_timeout(int); void load_from_file(void){ boost::property_tree::ptree pt; boost::property_tree::ini_parser::read_ini(“parametri.ini”, pt); porta = pt.get(“Connessione.porta”); distanza_minima = pt.get(“Parametri.distanza_minima”); offset = pt.get(“Parametri.offset”); file = pt.get(“File.file”); } int wrap_fprintf(FILE* str, const char* format, …) { int result = 1; if (call_fprintf) { va_list args; va_start(args, format); result = vfprintf(str, format, args); va_end(args); } else { /* do nothing */ } return result; } /* A subclass of ArASyncTask, to contain a method that runs in a new thread */ class CControl : public ArASyncTask { ArMutex myMutex; int myCounter; float m_heading, m_distance; bool first; bool flag_heading, flag_distance; private: public: CControl() { first=false; flag_heading=true; flag_distance=true; } void setHeading(float heading) { myMutex.lock(); m_heading=heading; //first=false; flag_heading=false; myMutex.unlock(); } void setDistance(float distance) { myMutex.lock(); m_distance=distance; //first=false; flag_distance=false; myMutex.unlock(); } void* runThread(void*) { printf(“nSpostamento X: %f, Y: %fn”, robot.getX(), robot.getY()); wrap_fprintf(file_open,” %f, %f, n”,robot.getX(),robot.getY()); while(Aria::getRunning()) { if(discovery){ // printf(“nRoutine Discovery Avviataan”); robot.setRotVel(4.0); } printf(“nInterval: %f Now: %f Prev: %fn”, interval, (double)now/CLOCKS_PER_SEC, (double)prev/CLOCKS_PER_SEC); elapsedTime = (t2.tv_sec – t1.tv_sec) * 1000.0; // sec to ms elapsedTime += (t2.tv_usec – t1.tv_usec) / 1000.0; // us to ms cout << "n" << elapsedTime << " ms.n"; if( !flag_heading && !flag_distance) { myMutex.lock(); temp_heading=m_heading; temp_distance=m_distance; myMutex.unlock(); gettimeofday(&t2, NULL); if (is_equal(temp_heading, 0.0)){ robot.setRotVel(0.0); } else robot.setRotVel(-ArMath::radToDeg(temp_heading)); if( temp_distance distanza_minima){ if (last_distance == 0) last_distance = temp_distance; float delta = temp_distance – last_distance; if (is_equal(delta, 0.0)) continue; // skip sending else { if ( delta > 0.0){ robot.setVel(float(delta/10)); // printf(“nn**********nnAvanzo di %fn******nn”, delta); } } last_distance = temp_distance; } else if(temp_distance (distanza_minima + offset) ) robot.setVel(float(temp_distance/20)); // printf(“nrunThread:: getX=%f getY=%f heading= %f distance = %f rob_vel = %f rob_rot_vel = %fn”,robot.getX(), robot.getY(), ArMath::radToDeg(temp_heading),temp_distance, robot.getVel(),robot.getRotVel()); flag_heading = true; flag_distance = true; } ArUtil::sleep(100); } } /* Lock the mutex object. */ void lockMutex() { myMutex.lock(); } /* Unlock the mutex object. */ void unlockMutex() { myMutex.unlock(); } }; CControl control; DataLine GetValueFromLine(const std::string& sData) { std::string sName, sInteger; std::stringstream ss; DataLine Result; size_t sz = sData.find(‘@’); sName = sData.substr(0,sz); // Just in case you need it later Result.sName = sName; sInteger = sData.substr(sz + 1,sData.length() – sz); ss.str(sInteger); ss >> Result.nNumber; if (ss.fail()) { // something went wrong, probably not an integer } return Result; } void server(boost::asio::io_service& io_service, short port) { udp::socket sock(io_service, udp::endpoint(udp::v4(), port)); std::cout << "n**Connessione Accettata**n"; std::cout << "nOffset: " << offset << "nDistanza Minima: " << distanza_minima << "n"; signal(SIGALRM,alarm_handler_to_check_timeout); alarm(1); for (;;) { char data[max_length]; udp::endpoint sender_endpoint; prev = clock(); size_t length = sock.receive_from( boost::asio::buffer(data, max_length), sender_endpoint); if(length == 0) printf("nnNONRICEVONULLAnn"); data[length] = 0; output = GetValueFromLine(data); comando = output.sName; valore = output.nNumber; if (output.sName == "right"){ robot.setRotVel(5.0); robot.setVel(0.0); // std::cout << "nSto ricevendo: " << output.sName; // printf("nValore di discovery %dn", discovery); // printf("nLEFTn"); } else if(output.sName == "left"){ robot.setRotVel(-5.0); robot.setVel(0.0); //printf("nRIGHTn"); } else if (output.sName == "heading"){ start = false; control.setHeading(output.nNumber); // std::cout << "nSto ricevendo: " << output.sName << "e heading: " < 0) { discovery = false; right_disc = false; left_disc = true; } else if(output.nNumber < 0) { discovery = false; left_disc = false; right_disc = true; } } else if (output.sName == "distance"){ start = false; control.setDistance(output.nNumber); // std::cout << "nSto ricevendo: " << output.sName << "e distance: " < MAX_INTERVAL) { // define MAX_INTERVAL according to your needs printf(“nDentro STOP conditionn”); // timeout, stop. } } int main(int argc, char **argv) { load_from_file(); file_open = fopen(file.c_str(), “w”); if(file_open == NULL){ perror(“Error opening the reports file”); } wrap_fprintf(file_open,” X[mm], Y[mm] Distance[mm]n”); Aria::init(); ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArSimpleConnector simpleConnector(&parser); ArActionGoto gotoPoseAction(“goto”); gotoPoseAction.setCloseDist(50); ArSonarDevice sonar; ArAnalogGyro gyro(&robot); robot.addRangeDevice(&sick); robot.addRangeDevice(&sonar); // Make a key handler, so that escape will shut down the program cleanly ArKeyHandler keyHandler; Aria::setKeyHandler(&keyHandler); robot.attachKeyHandler(&keyHandler); printf(“main:: You may press escape to exitn”); // Collision avoidance actions at higher priority ArActionLimiterForwards limiterAction(“speed limiter near”, 300, 600, 250); ArActionLimiterForwards limiterFarAction(“speed limiter far”, 300, 1100, 400); ArActionLimiterTableSensor tableLimiterAction; robot.addAction(&tableLimiterAction, 100); robot.addAction(&limiterAction, 95); robot.addAction(&limiterFarAction, 90); // Goto action at lower priority robot.addAction(&gotoPoseAction, 50); // Stop action at lower priority, so the robot stops if it has no goal ArActionStop stopAction(“stop”); robot.addAction(&stopAction, 40); // Parse all command line arguments if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); exit(1); } // Connect to the robot if (!simpleConnector.connectRobot(&robot)) { printf(“main:: Could not connect to robot… exitingn”); Aria::exit(1); } robot.runAsync(true); // turn on the motors, turn off amigobot sounds robot.enableMotors(); robot.comInt(ArCommands::SOUNDTOG, 0); simpleConnector.setupLaser(&sick); sick.runAsync(); if (!sick.blockingConnect()) { ArLog::log(ArLog::Terse, “main: Could not connect to SICK laser… exitingn”); Aria::shutdown(); Aria::exit(1); } CLogger logger(&robot, &sick, 10, 1, “laserOdo.log”); control.runAsync(); try{ boost::asio::io_service io_service; server(io_service, porta);} catch (std::exception& e) { std::cerr << "Exception: " << e.what() << "n"; } return 0; }

Select allOpen in new window

Source