Thanks for the information :)

So i think the best choice is to use a timeout treshold because i prefer to not send extra packets.

What's the best way to implement a timeout treshold in C/C++?
I think i should add this to the socket reading message block code, is it true?

Well, just measure the time when a packet arrives and compare that to the time the previous packet has arrived. If that difference is over your tresholöd, take measures accordingly.

clock_t prev = clock();
clock_t now;
double interval;

 for (;;)
  {
    char data[max_length];
    udp::endpoint sender_endpoint;
    size_t length = sock.receive_from(
    boost::asio::buffer(data, max_length), sender_endpoint);
    sock.send_to(boost::asio::buffer(data, length), sender_endpoint);

    now = clock();

    interval = (double)(now -prev) / CLOCKS_PER_SEC;

    if (interval > MAX_INTERVAL) { // define MAX_INTERVAL according to your needs

       // timeout, stop.
    }

    prev = now; // store for next comparison
    ..
    ... robot control routine ...
 }

Select allOpen in new window

Thanks a lot, jkr!

You are really great! :)

As soon as it is possible i test this solution on my robot: the problem is that sometimes there are some delays even when the client application is up so i have to test if the time checking over the packets can work and manage the delays but i think it can because UDP seems to be faster.

Unfortunately, this solution does not work with my code.

I tested it today with no good result because when the server receives data from client, both prev and now variables increase their values, but if i close the client, then these values doesn't change anymore and so i can't check if client is close or not.

This happens because prev and now are pretty similar and so interval is always 0 or 0.1 at max and when i close the client this value does not change.

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";
    prev = clock();
 for (;;)
  {
    char data[max_length];
    udp::endpoint sender_endpoint;
    size_t length = sock.receive_from(
    boost::asio::buffer(data, max_length), sender_endpoint);
    sock.send_to(boost::asio::buffer(data, length), sender_endpoint);
    now = clock();
    data[length] = 0;
    output = GetValueFromLine(data);
      interval = (double)(now - prev) / CLOCKS_PER_SEC;
    if (interval > MAX_INTERVAL){
    // stop
    }    
    prev = now;
printf("\nInterval %f Prev: %f Now %f\n", interval, (double)prev/CLOCKS_PER_SEC, (double) now / CLOCKS_PER_SEC);

// control routine
}



int main(int argc, char **argv)
{
//some other code here
boost::asio::io_service io_service;  
          server(io_service, porta);
         
  return 0;     
 }                        

Select allOpen in new window

Sorry, that should be on the receiving side of the connection.

yes, it is on the server where i receive packets from the client.

Well, your code does a 'sock.send_to()' - that does not really look like a receiving side ;o)

I'm sorry, i did a mistake with the posted code: that send_to() function is not present in the code.

I added it just for a test, but the server process does not send back any information to the client one.

Hmm, OK, I think I see the problem - while you're sitting there waiting for the next packet, the timeout can't take place. What's your OS?

Ubuntu 10.4

Yes, the strange it's that as soon as the server start, it prints: "Accepted connection" even if the client isn't up in that moment.

When the client closes, there is not timeout and it waits until next packets arrive in fact if i reconnect the client and i try to send packets, the prev and now variable continue to increase their value.

OK, then you can use 'alarm()' (http://unixhelp.ed.ac.uk/CGI/man-cgi?alarm+2) to be notified using 'signal()' (http://unixhelp.ed.ac.uk/CGI/man-cgi?signal+2) all N seconds and check the time that has elapsed since the last message, e.g.

clock_t prev = 0; // GLOBAL variable!

void alarm_handler_to_check_timeout(int);

//...

  
 // set handler
 signal(SIGALRM,alarm_handler_to_check_timeout);

 for (;;)
  {
    char data[max_length];
    udp::endpoint sender_endpoint;
    size_t length = sock.receive_from(
    boost::asio::buffer(data, max_length), sender_endpoint);
    sock.send_to(boost::asio::buffer(data, length), sender_endpoint);



    prev = clock(); // store for next comparison
    ..
    ... robot control routine ...
 }

void alarm_handler_to_check_timeout(int) {

  clock_t now;
  double interval;

    now = clock();

    interval = (double)(now -prev) / CLOCKS_PER_SEC;

    if (interval > MAX_INTERVAL) { // define MAX_INTERVAL according to your needs

       // timeout, stop.
    }
}

Select allOpen in new window

I'm having problems with this last solution.

Application seems to never reach the alarm_handler_to_check_timeout() function because i added a printf() in its body in order to verify if its code is executed, but nothing happens.

void alarm_handler_to_check_timeout(int) {

  clock_t now;
  double interval;

    now = clock();
  printf("\nNow: %f Prev: %f\n", (double)now, (double)prev);
    interval = (double)(now -prev) / CLOCKS_PER_SEC;

    if (interval > MAX_INTERVAL) { // define MAX_INTERVAL according to your needs

     printf("\nDentro STOP condition\n");  // timeout, stop.
    }

  
}

void server(boost::asio::io_service& io_service, short port)
{
 

  udp::socket sock(io_service, udp::endpoint(udp::v4(), port));
  std::cout << "\nConnection accepted\n";
  
  signal(SIGALRM,alarm_handler_to_check_timeout);

 for (;;)
  {
    char data[max_length];
    udp::endpoint sender_endpoint;
    size_t length = sock.receive_from(
    boost::asio::buffer(data, max_length), sender_endpoint);
    sock.send_to(boost::asio::buffer(data, length), sender_endpoint);
  
    data[length] = 0;
    output = GetValueFromLine(data);
   

      prev = clock();
// control routine
 }
}

Select allOpen in new window

Application never writes the printf statement in the alarm_handler_to_check_timeout() function.

I don't know why because if i well understood, when application signals SIGALRM it calls the timeout() function but this seems to not happen.

I set alarm to 1 second before the signal and now it seems to work, but i have to test it.

I'll get you back in a while :)

I tested it but the problem is still here: when i close the client, interval doesn't change it's value.

So, if while the client is running interval is for example interval = 0.4, then after i close the client, interval is still equals to 0.4 and it does not increase its value.

This is what happens:

Interval: 0.000000 Now: 0.000000 Prev: 0.000000


Interval: 0.000000 Now: 0.000000 Prev: 0.000000

Interval: 0.000000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000


Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 0.000000

Interval: 0.030000 Now: 30000.000000 Prev: 80000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 80000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 80000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 80000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 80000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 80000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 90000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 90000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 90000.000000
Warning: ArRobot sync tasks too long at 351 ms, (100 ms normal 250 ms warning)

Interval: 0.030000 Now: 30000.000000 Prev: 90000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 90000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 90000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 100000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 100000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 100000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 110000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 110000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 110000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 110000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 110000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 110000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 110000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 110000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 120000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 120000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 120000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 120000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 120000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 120000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 120000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 130000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 130000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 130000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 130000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 130000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 130000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 130000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 130000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 130000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 130000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 130000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 140000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 140000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 140000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 140000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 140000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 140000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 150000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 150000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 150000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 150000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 150000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 150000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 150000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000 <<< CLIENT DISCONNECTED

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000
Taking readings from the 181 laser values

Interval: 0.030000 Now: 30000.000000 Prev: 170000.000000

Select allOpen in new window

When client is disconnected, values does not change anymore so i can't detect if client is up or not.

This is the code:

void alarm_handler_to_check_timeout(int) {

    now = clock();
  printf("\nInterval: %f Now: %f Prev: %f\n", interval, (double)now, (double)prev);
    interval = (double)(now - prev) / CLOCKS_PER_SEC;

    if (interval > MAX_INTERVAL) { // define MAX_INTERVAL according to your needs

     printf("\nDentro STOP condition\n");  // timeout, stop.
    }
 
}

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";
  alarm(1);
  signal(SIGALRM,alarm_handler_to_check_timeout);
 

 for (;;)
  {
   
    char data[max_length];
    udp::endpoint sender_endpoint;
    size_t length = sock.receive_from(
    boost::asio::buffer(data, max_length), sender_endpoint);
   
  
    data[length] = 0;
    output = GetValueFromLine(data);
   

      prev = clock();
    }
 
}

Select allOpen in new window

Try to change that to

signal(SIGALRM,alarm_handler_to_check_timeout);

  alarm(1);

Select allOpen in new window

As you said, i tried:

void server(boost::asio::io_service& io_service, short port)
{
   signal(SIGALRM,alarm_handler_to_check_timeout);
   alarm(1);
  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";
 
 
 

 for (;;)
  {
   
    
    char data[max_length];
    udp::endpoint sender_endpoint;
    size_t length = sock.receive_from(
    boost::asio::buffer(data, max_length), sender_endpoint);
   
  
    data[length] = 0;
    output = GetValueFromLine(data);
   
      prev = clock();

Select allOpen in new window

but nothing change.

The problem is that after the first packet, the prev variable start to increase and now variable still have the same value which is equals to interval and not to interval = now - prev. It seems to display just interval = now.

When i close the client, interval is still equals to now and prev have the last value acquired before the client stop.

It would be fine to have a counter which counts how many seconds there are between prev and the global time in order to see if the difference between the last prev time and the global time and if it's greater than MAX_TIME then stop the robot.

Ooops, one simple thing missing - updating 'prev', i.e.

void alarm_handler_to_check_timeout(int) {

  clock_t now;
  double interval;

    now = clock();
  printf("\nNow: %f Prev: %f\n", (double)now, (double)prev);
    interval = (double)(now -prev) / CLOCKS_PER_SEC;

    if (interval > MAX_INTERVAL) { // define MAX_INTERVAL according to your needs

     printf("\nDentro STOP condition\n");  // timeout, stop.
    }

  prev = now; // this line should fix the issue
}

Select allOpen in new window

Unfortunately, it does not work even if i update the prev variable.

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); // it's the same if i unsert 
  

 for (;;)
  {
   
    char data[max_length];
    udp::endpoint sender_endpoint;
    size_t length = sock.receive_from(
    boost::asio::buffer(data, max_length), sender_endpoint);
   
  
    data[length] = 0;
    output = GetValueFromLine(data);
   
      prev = clock();
    
      comando = output.sName;
      valore = output.nNumber;
   // control routine     

  }
}


void alarm_handler_to_check_timeout(int) {
 

    now = clock();
    printf("\nHndler_Interval: %f Now: %f Prev: %f\n", interval, (double)now, (double)prev);
    interval = (double)(now - prev) / CLOCKS_PER_SEC;

    if (interval > MAX_INTERVAL) { // define MAX_INTERVAL according to your needs

     printf("\nDentro STOP condition\n");  // timeout, stop.
    }
    prev = now;
  
}

Select allOpen in new window

The problem is that interval  does not appear to update: when server starts, now variable count the first second and then it never changes its value while prev variable starts to increase the value but if i stop the client, prev remains to the last value and interval does not update.
In this way, i have only a global time value.

This is the output, last three lines are generated after the client closure.

Interval: 0.040000 Now: 0.040000 Prev: 0.040000

Interval: 0.040000 Now: 0.040000 Prev: 0.040000

Interval: 0.040000 Now: 0.040000 Prev: 0.290000

Interval: 0.040000 Now: 0.040000 Prev: 0.290000

Interval: 0.040000 Now: 0.040000 Prev: 0.290000

Interval: 0.040000 Now: 0.040000 Prev: 0.290000

Interval: 0.040000 Now: 0.040000 Prev: 0.290000
Taking readings from the 181 laser values

Interval: 0.040000 Now: 0.040000 Prev: 0.290000

Interval: 0.040000 Now: 0.040000 Prev: 0.290000

Interval: 0.040000 Now: 0.040000 Prev: 0.290000

Interval: 0.040000 Now: 0.040000 Prev: 0.290000
Taking readings from the 181 laser values

Interval: 0.040000 Now: 0.040000 Prev: 0.290000

Interval: 0.040000 Now: 0.040000 Prev: 0.300000

Interval: 0.040000 Now: 0.040000 Prev: 0.300000
Taking readings from the 181 laser values

Interval: 0.040000 Now: 0.040000 Prev: 0.300000

Interval: 0.040000 Now: 0.040000 Prev: 0.300000

Interval: 0.040000 Now: 0.040000 Prev: 0.300000

Interval: 0.040000 Now: 0.040000 Prev: 0.300000
Taking readings from the 181 laser values

Interval: 0.040000 Now: 0.040000 Prev: 0.310000

Interval: 0.040000 Now: 0.040000 Prev: 0.310000

Interval: 0.040000 Now: 0.040000 Prev: 0.310000

Interval: 0.040000 Now: 0.040000 Prev: 0.310000

Interval: 0.040000 Now: 0.040000 Prev: 0.310000
Taking readings from the 181 laser values

Interval: 0.040000 Now: 0.040000 Prev: 0.310000

Interval: 0.040000 Now: 0.040000 Prev: 0.330000

Interval: 0.040000 Now: 0.040000 Prev: 0.330000

Interval: 0.040000 Now: 0.040000 Prev: 0.330000

Interval: 0.040000 Now: 0.040000 Prev: 0.330000
Taking readings from the 181 laser values

Interval: 0.040000 Now: 0.040000 Prev: 0.330000

Interval: 0.040000 Now: 0.040000 Prev: 0.330000

Interval: 0.040000 Now: 0.040000 Prev: 0.340000

Interval: 0.040000 Now: 0.040000 Prev: 0.340000

Interval: 0.040000 Now: 0.040000 Prev: 0.340000

Interval: 0.040000 Now: 0.040000 Prev: 0.340000
Taking readings from the 181 laser values

Interval: 0.040000 Now: 0.040000 Prev: 0.340000
Warning: ArRobot sync tasks too long at 251 ms, (100 ms normal 250 ms warning)

Interval: 0.040000 Now: 0.040000 Prev: 0.340000
Taking readings from the 181 laser values

Interval: 0.040000 Now: 0.040000 Prev: 0.350000

Interval: 0.040000 Now: 0.040000 Prev: 0.350000
Taking readings from the 181 laser values

Interval: 0.040000 Now: 0.040000 Prev: 0.350000

Interval: 0.040000 Now: 0.040000 Prev: 0.350000

Interval: 0.040000 Now: 0.040000 Prev: 0.350000
Taking readings from the 181 laser values

Interval: 0.040000 Now: 0.040000 Prev: 0.350000

Interval: 0.040000 Now: 0.040000 Prev: 0.350000

Interval: 0.040000 Now: 0.040000 Prev: 0.350000

Interval: 0.040000 Now: 0.040000 Prev: 0.350000
Taking readings from the 181 laser values

Interval: 0.040000 Now: 0.040000 Prev: 0.350000

Interval: 0.040000 Now: 0.040000 Prev: 0.370000

Interval: 0.040000 Now: 0.040000 Prev: 0.370000

Interval: 0.040000 Now: 0.040000 Prev: 0.370000

Interval: 0.040000 Now: 0.040000 Prev: 0.370000
Taking readings from the 181 laser values

Interval: 0.040000 Now: 0.040000 Prev: 0.370000

Select allOpen in new window

It's getting quite strange so far, do you mind sharing the whole code?

Yes, no problem.

This is the code: http://lnx.mangaitalia.net/code/code.cpp

I used also gettimeofday in order to check if it can work, but it doesn't work.

OK, I *hope* to have found the issue (unfortunately, I can't check it, but try

#include <sys/time.h>
#include "Logger.h"
#include "Aria.h"
#include <cstdlib>
#include <iostream>
#include <boost/asio.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/config.hpp>
#include <boost/program_options/detail/config_file.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/bind.hpp>
#include <boost/smart_ptr.hpp>
#include <boost/thread.hpp>
#include <string>
#include <sstream>
#include <set>
#include <sstream>
#include <exception>
#include <fstream>
#include <limits>
#include <stdarg.h>
#include <fcntl.h>
#include <string.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <time.h>
#include <unistd.h>
#include <stdarg.h>
#include <signal.h>

#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<float>::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<short int>("Connessione.porta");
distanza_minima = pt.get<float>("Parametri.distanza_minima");
offset = pt.get<float>("Parametri.offset");
file = pt.get<std::string>("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: %f\n", robot.getX(), robot.getY());
          wrap_fprintf(file_open,"	%f,	%f,	\n",robot.getX(),robot.getY());
	    while(Aria::getRunning())
	    {
		if(discovery){
     // printf("\nRoutine Discovery Avviata\a\n");
      
      robot.setRotVel(4.0);
      }
                  printf("\nInterval: %f Now: %f Prev: %f\n", 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 + offset) && 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("\n\n**********\n\nAvanzo di %f\n******\n\n", delta);
									}
					}
					last_distance = temp_distance;

					}
				else if(temp_distance <= distanza_minima || is_equal(temp_distance, 0.0))
					robot.setVel(0.0);
				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 = %f\n",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("\n\nNONRICEVONULLA\n\n");

    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 %d\n", discovery);
    //  printf("\nLEFT\n");
     
      }
      else if(output.sName == "left"){
      robot.setRotVel(-5.0);
      robot.setVel(0.0);
    //printf("\nRIGHT\n");
      
      }
      else if (output.sName == "heading"){
      start = false;
      control.setHeading(output.nNumber);
   //   std::cout << "\nSto ricevendo: " << output.sName << "e heading: " << output.nNumber;
      if(output.nNumber > 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: " << output.nNumber;
      gettimeofday(&t1, NULL);
      }   
      else printf("\nReceived code error\n");                          
      gettimeofday(&tempo, NULL);

      wrap_fprintf(file_open,"	%f,	%f,	 %f	 %f      %ld\n",robot.getX(),robot.getY(), temp_distance, ArMath::radToDeg(temp_heading), tempo.tv_sec);

  }
   
}


void alarm_handler_to_check_timeout(int) {
 

    now = clock();
    printf("\nHndler_Interval: %f Now: %f Prev: %f\n", interval, (double)now, (double)prev);
    interval = (double)(now - prev) / CLOCKS_PER_SEC;

    if (interval > MAX_INTERVAL) { // define MAX_INTERVAL according to your needs

     printf("\nDentro STOP condition\n");  // 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 exit\n");

	  // 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... exiting\n");
	    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... exiting\n");
		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

Hello jkr, thanks a lot for your help.

Can i ask you what you changed? because i tried to check it and i found that you removed the "prev = now" statement,  it is correct? Or did you changed some other things?

Because i already tried to do this the other day but without success because it seems to not work.

Well, I moved it to a place where it seemed more reasonable to me. Overall, I have to say that I am puzzled, since from my perspective, it should work. I'll have another look tomorrow when I have more time on my hands.

I tried but it does not work: when i close the client, the timer on the server stops and it remains to the last value.
So i can't check it time is elapsing after the client closure.

I still don't get it, that should work - could you post the output of

    printf("\nHndler_Interval: %f Now: %f Prev: %f\n", interval, (double)now, (double)prev);

again (maybe with the commands received)?

Still having issues?

Yes, i'll post the output following your last answer as soon as i can.

I have to collect these data in next days because during this week i had some problems at home and i didn't work on my system.

I've requested that this question be deleted for the following reason:

This question has been classified as abandoned and is closed as part of the Cleanup Program. See the recommendation for more details.

I'll object just to give the answer the chance to get back - it's holiday season, maybe not the best time for cleanups.

This question has been reviewed by several Experts and Zone Advisors, and after that review I'm accepting this comment as the solution.

SouthMod
EE Moderator