ARIA Example: Use a robot task to log or print robot pose or other information periodically

From MobileRobots Research and Academic Customer Support

This example shows one technique for printing robot pose every second. You could also print any other information, save the information to a file on disk, etc.

In this technique, we will use a robot task callback, encapsulated in a small class. A similar example of using robot tasks is robotSyncTaskExample.cpp, in ARIA's example programs.

Before continuing, please read the "Robot Synchronization Cycle" or "Robot Task Cycle" section of the ARIA reference manual, to familiarize yourself with the robot task cycle.

First we create a functor object, an an instance of one of the subclasses of ArFunctor, which stores a pointer to a method or a function to call later. A robot task does not pass any arguments to the functor or exepect any return value, so use ArGlobalFunctor with a pointer to a plain global function, or use ArFunctorC<T> to store a pointer to an instance of the class of type "T" along with a pointer to one of the methods in that class.

In the function or method called by the functor, we will do the logging. However, a task is called every robot cycle, which is frequent (approximately every 100ms if the robot is sending SIP updates every 100ms). Since we only want to print a message every second, we will first check elapsed time since the last message was printed with an ArTime object.

An advantage of using a "Sensor interpretation" task for this is that sensor interpretation tasks are invoked right after a data update is received from the robot, so at the time of the user task call, any data in the ArRobot class obtained from the SIP (e.g. robot position), is very recent.

Example code:

/** This class creates a robot sensor task that prints the robot pose
   every second. It adds the new task to the given ArRobot object
   (on class instantiation), and removes it when destroyed.
*/
class Logger {
private:
  ArRobot *robot;
  ArTime lastLogTime;
  ArFunctorC<Logger> robotTaskFunc;
  void logTask();
public:
  Logger(ArRobot *r);
  ~Logger();
}; 

Logger::Logger(ArRobot *r) :
  robot(r),                              // store a pointer to the ArRobot object
  robotTaskFunc(this, &Logger::logTask)  // initialize the functor to be given added as an ArRobot 
                                         // user task with this instance and which method to  call
{
  // add our task functor to the robot object as a user task, 
  // to be invoked in every robot task cycle (approx. every 100ms):
  robot->addSensorInterpTask("Logger", 50, &robotTaskFunc);    
}

Logger::~Logger()
{
  // it is important to remove our task if this object is destroyed, otherwise 
  // ArRobot will hold an invalid ArFunctor pointer in its tasks list, resulting
  // in a crash when it tries to invoke it.
  robot->remSensorInterpTask(&robotTaskFunc);
}

// This is the method invoked as the user task
void Logger::logTask()
{
  if(lastLogTime.mSecSince() >= 1000)  // 1 second has passed since start or last log 
  {
     printf("%f %f %f\n", robot->getX(), robot->getY(), robot.getTh());
     lastLogTime.setToNow(); // reset timer
  }
}

int main(int argc, char **argv)
{
   Aria::init();
   ArRobot robot;
  
   ... do other initialization and connect to the robot here ... 

   Logger logger(&robot); // this object will last for the rest of the program, so the task will be active for the rest of the program.
   robot.run(true);
   Aria::exit(0);
}