ArSickLogger.cpp

/*
ActivMedia Robotics Interface for Applications (ARIA)
Copyright (C) 2004,2005 ActivMedia Robotics, LLC


     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
ActivMedia Robotics for information about a commercial version of ARIA at 
robots@activmedia.com or 
ActivMedia Robotics, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481

*/

#include <stdarg.h>

#include "ArExport.h"
#include "ariaOSDef.h"
#include "ArSickLogger.h"
#include "ArRobot.h"
#include "ArSick.h"
#include "ArJoyHandler.h"
#include "ariaInternal.h"

ArSickLogger::ArSickLogger(ArRobot *robot, ArSick *sick, 
                                    double distDiff, double degDiff, 
                                    const char *fileName, bool addGoals,
                                    ArJoyHandler *joyHandler,
                                    const char *baseDirectory) :
  mySectors(18), 
  myTaskCB(this, &ArSickLogger::robotTask),
  myGoalKeyCB(this, &ArSickLogger::goalKeyCallback), 
  myLoopPacketHandlerCB(this, &ArSickLogger::loopPacketHandler)
{
  ArKeyHandler *keyHandler;
  
  ArSick::Degrees degrees;
  ArSick::Increment increment;
  double deg, incr;

  myOldReadings = false;
  myNewReadings = true;
  myWrote = false;
  myRobot = robot;
  mySick = sick;
  if (baseDirectory != NULL && strlen(baseDirectory) > 0)
    myBaseDirectory = baseDirectory;
  else
    myBaseDirectory = "";
  std::string realFileName;
  if (fileName[0] == '/' || fileName[0] == '\\')
  {
    realFileName = fileName;
  }
  else
  {
    realFileName = myBaseDirectory;
    realFileName += fileName;
  }
  myFileName = realFileName;
  
  myFile = fopen(realFileName.c_str(), "w+");
  degrees = mySick->getDegrees();
  increment = mySick->getIncrement();
  if (degrees == ArSick::DEGREES180)
    deg = 180;
  else 
    deg = 100;
  if (increment == ArSick::INCREMENT_ONE)
    incr = 1;
  else
    incr = .5;
  if (myFile != NULL)
  {
    const ArRobotParams *params;
    params = robot->getRobotParams();
    fprintf(myFile, "LaserOdometryLog\n");
    fprintf(myFile, "#Created by ARIA's ArSickLogger\n");
    fprintf(myFile, "version: 2\n");
    fprintf(myFile, "sick1pose: %d %d %.2f\n", params->getLaserX(), 
            params->getLaserY(), params->getLaserTh());
    fprintf(myFile, "sick1conf: %d %d %d\n", 
            ArMath::roundInt(0.0 - deg / 2.0),
            ArMath::roundInt(deg / 2.0), ArMath::roundInt(deg / incr + 1.0));
  }
  else
    ArLog::log(ArLog::Terse, "ArSickLogger cannot write to file %s", 
               myFileName.c_str());

  myDistDiff = distDiff;
  myDegDiff = degDiff;
  myFirstTaken = false;
  myScanNumber = 0;
  myLastVel = 0;
  myStartTime.setToNow();
  myRobot->addUserTask("Sick Logger", 1, &myTaskCB);

  char uCFileName[15];
  strncpy(uCFileName, fileName, 14);
  uCFileName[14] = '\0';
  myRobot->comStr(94, uCFileName);

  myRobot->addPacketHandler(&myLoopPacketHandlerCB);

  myAddGoals = addGoals;
  myJoyHandler = joyHandler;
  myTakeReadingExplicit = false;
  myAddGoalExplicit = false;
  myAddGoalKeyboard = false;
  myFirstGoalTaken = false;
  myNumGoal = 1;
  myLastLoops = 0;
  // if we're adding goals hook into everything
  if (myAddGoals)
  {
    // do it for the keyboard first
    // see if there is already a keyhandler, if not make one for ourselves
    if ((keyHandler = Aria::getKeyHandler()) == NULL)
    {
      keyHandler = new ArKeyHandler;
      Aria::setKeyHandler(keyHandler);
      myRobot->attachKeyHandler(keyHandler);
    }
    // now that we have a key handler, add our keys as callbacks, print out big
    // warning messages if they fail
    if (!keyHandler->addKeyHandler('g', &myGoalKeyCB))
      ArLog::log(ArLog::Terse, "The key handler already has a key for g, sick logger goal handling will not work correctly.");
    if (!keyHandler->addKeyHandler('G', &myGoalKeyCB))
      ArLog::log(ArLog::Terse, "The key handler already has a key for g, sick logger goal handling will not work correctly.");
    
    // the joystick checks will happen down in the task
  }


}

ArSickLogger::~ArSickLogger()
{
  myRobot->remUserTask(&myTaskCB);
  myRobot->remPacketHandler(&myLoopPacketHandlerCB);
  myRobot->comStr(94, "");
  if (myFile != NULL)
  {
    fprintf(myFile, "# End of log\n");
    fclose(myFile);
  }
}
  
bool ArSickLogger::loopPacketHandler(ArRobotPacket *packet)
{
  unsigned char loops;
  if (packet->getID() != 0x96)
    return false;
  loops = packet->bufToUByte();
  unsigned char bit;
  int num;
  if (loops != myLastLoops)
  {
    for (bit = 1, num = 1; num <= 8; bit *= 2, num++)
    {
      if ((loops & bit) && !(myLastLoops & bit))
      {
        addTagToLog("loop: start %d", num);
        ArLog::log(ArLog::Normal, "Starting loop %d", num);
      }
      else if (!(loops & bit) && (myLastLoops & bit))
      {
        addTagToLog("loop: end %d", num);
        ArLog::log(ArLog::Normal, "Ending loop %d", num);
      }
    }
  }
  myLastLoops = loops;
  return true;
}

void ArSickLogger::addTagToLogPlain(const char *str)
{
  myTags.push_back(str);
}

void ArSickLogger::addTagToLog(const char *str, ...)
{
  char buf[2048];
  va_list ptr;
  va_start(ptr, str);
  vsprintf(buf, str, ptr);
  addTagToLogPlain(buf);
  va_end(ptr);
}


void ArSickLogger::addInfoToLogPlain(const char *str)
{
  myInfos.push_back(str);
}

void ArSickLogger::addInfoToLog(const char *str, ...)
{
  char buf[2048];
  va_list ptr;
  va_start(ptr, str);
  vsprintf(buf, str, ptr);
  addInfoToLogPlain(buf);
  va_end(ptr);
}

void ArSickLogger::goalKeyCallback(void)
{
  myAddGoalKeyboard = true;
}

void ArSickLogger::internalAddGoal(void)
{
  // this check is for if we're not adding goals return... but if
  // we're not adding goals and one was requested explicitly then add
  // that one
  if (!myAddGoals && !myAddGoalExplicit) 
    return;

  // make sure we'd even want to add a goal if we could
  if (myRobot->isConnected() && 
      (!myFirstGoalTaken || 
       myAddGoalExplicit ||
       myLastGoalTakenPose.findDistanceTo(myRobot->getEncoderPose()) > 300 ||
       myLastGoalTakenPose.findAngleTo(myRobot->getEncoderPose()) > 90 ||
       myLastGoalTakenTime.secSince() > 10))
       
  {
    // now see if we want to, first by second button on the joystick
    // connected to microcontroller... then by if a goal was requested
    // (by keypress), then if a goal was requested explicitly then
    // finally by the joystick attached to computer
    
    if ((myRobot->getFlags() & ArUtil::BIT9) || 
        myAddGoalKeyboard ||
        myAddGoalExplicit || 
        (myJoyHandler != NULL && (myJoyHandler->getButton(2) || 
                                  myJoyHandler->getButton(3) || 
                                  myJoyHandler->getButton(4))))
    {
      myFirstGoalTaken = true;
      myAddGoalExplicit = false;
      myLastGoalTakenTime.setToNow();
      myLastGoalTakenPose = myRobot->getEncoderPose();
      // call addTagToLog not do it directly so we get additional info
      // needed
      addTagToLog("cairn: GoalWithHeading \"\" ICON_GOALWITHHEADING \"goal%d\"", myNumGoal);
      printf("Goal %d taken\n", myNumGoal);
      myNumGoal++;
    }
  }
  // reset this here for if they held the key down a little, so it
  // gets reset and doesn't hit multiple goals
  myAddGoalKeyboard = false;

}

void ArSickLogger::internalWriteTags(void)
{
  time_t msec;

  // now put the tags into the file
  while (myInfos.size() > 0)
  {
    if (myFile != NULL)
    {
      myWrote = true;
      fprintf(myFile, "%s\n", (*myInfos.begin()).c_str());
    }
    myInfos.pop_front();
  }


  // now put the tags into the file
  while (myTags.size() > 0)
  {
    if (myFile != NULL)
    {
      myWrote = true;
      msec = myStartTime.mSecSince();
      fprintf(myFile, "time: %ld.%ld\n", msec / 1000, msec % 1000);
      internalPrintPos(myRobot->getEncoderPose());
      fprintf(myFile, "%s\n", (*myTags.begin()).c_str());
    }
    myTags.pop_front();
  }
}

void ArSickLogger::internalTakeReading(void)
{
  const std::list<ArSensorReading *> *readings;
  std::list<ArSensorReading *>::const_iterator it;
  std::list<ArSensorReading *>::const_reverse_iterator rit;
  ArPose poseTaken;
  time_t msec;
  ArSensorReading *reading;
  
  // we take readings in any of the following cases if we haven't
  // taken one yet or if we've been explicitly told to take one or if
  // we've gone further than myDistDiff if we've turned more than
  // myDegDiff if we've switched sign on velocity and gone more than
  // 50 mm (so it doesn't oscilate and cause us to trigger)

  if (myRobot->isConnected() && (!myFirstTaken || myTakeReadingExplicit || 
      myLast.findDistanceTo(myRobot->getEncoderPose()) > myDistDiff ||
      fabs(ArMath::subAngle(myLast.getTh(), 
                            myRobot->getEncoderPose().getTh())) > myDegDiff ||
      ((myLastVel < 0 && myRobot->getVel() > 0 ||
        myLastVel > 0 && myRobot->getVel() < 0) && 
       myLast.findDistanceTo(myRobot->getEncoderPose()) > 50)))
  {
    myWrote = true;
    mySick->lockDevice();
    readings = mySick->getRawReadings();
    if (readings == NULL || (it = readings->begin()) == readings->end() ||
        myFile == NULL)
    {
      mySick->unlockDevice();
      return;
    }
    myTakeReadingExplicit = false;
    myScanNumber++;
    ArLog::log(ArLog::Normal, 
               "Taking a reading from the %d laser values", 
               readings->size());
    myFirstTaken = true;
    myLast = myRobot->getEncoderPose();
    poseTaken = (*readings->begin())->getEncoderPoseTaken();
    myLastVel = myRobot->getVel();
    msec = myStartTime.mSecSince();
    fprintf(myFile, "#Scan %d\n", myScanNumber);
    fprintf(myFile, "time: %ld.%ld\n", msec / 1000, msec % 1000);
    /* ScanStudio isn't using these yet so don't print them
      fprintf(myFile, "velocities: %.2f %.2f\n", myRobot->getRotVel(),
      myRobot->getVel());*/
    internalPrintPos(poseTaken);

    if (myOldReadings)
    {
      fprintf(myFile, "sick1: ");
      
      if (!mySick->isLaserFlipped())
      {
        // make sure that the list is in increasing order
        for (it = readings->begin(); it != readings->end(); it++)
        {
          reading = (*it);
          fprintf(myFile, "%d ", reading->getRange());
        }
      }
      else
      {
        for (rit = readings->rbegin(); rit != readings->rend(); rit++)
        {
          reading = (*rit);
          fprintf(myFile, "%d ", reading->getRange());
        }
      }
      fprintf(myFile, "\n");
    }
    if (myNewReadings)
    {
      fprintf(myFile, "scan1: ");
      
      if (!mySick->isLaserFlipped())
      {
        // make sure that the list is in increasing order
        for (it = readings->begin(); it != readings->end(); it++)
        {
          reading = (*it);
          if (!reading->getIgnoreThisReading())
            fprintf(myFile, "%.0f %.0f  ", 
                    reading->getLocalX() - mySick->getSensorPositionX(), 
                    reading->getLocalY() - mySick->getSensorPositionY());
          else
            fprintf(myFile, "0 0  ");
        }
      }
      else
      {
        for (rit = readings->rbegin(); rit != readings->rend(); rit++)
        {
          reading = (*rit);
          if (!reading->getIgnoreThisReading())
            fprintf(myFile, "%.0f %.0f  ", 
                    reading->getLocalX() - mySick->getSensorPositionX(), 
                    reading->getLocalY() - mySick->getSensorPositionY());
          else
            fprintf(myFile, "0 0  ");
        }
      }
      fprintf(myFile, "\n");
    }
    mySick->unlockDevice();
  }
}

void ArSickLogger::internalPrintPos(ArPose poseTaken)
{
  if (myFile == NULL)
    return;
  ArPose encoderPose = myRobot->getEncoderPose();
  ArPose rawEncoderPose = myRobot->getRawEncoderPose();
  ArTransform normalToRaw(rawEncoderPose, encoderPose);

  ArPose rawPose;
  rawPose = normalToRaw.doInvTransform(poseTaken);
  fprintf(myFile, "#rawRobot: %.0f %.0f %.2f %.0f %.0f\n", 
          rawPose.getX(), 
          rawPose.getY(), 
          rawPose.getTh(), 
          myRobot->getVel(),
          myRobot->getRotVel());
  fprintf(myFile, "robot: %.0f %.0f %.2f %.0f %.0f\n", 
          poseTaken.getX(), 
          poseTaken.getY(), 
          poseTaken.getTh(), 
          myRobot->getVel(),
          myRobot->getRotVel());
}

void ArSickLogger::robotTask(void)
{

  // call our function to check goals
  internalAddGoal();
  
  // call our function to dump tags
  internalWriteTags();
  
  // call our function to take a reading
  internalTakeReading();

  // now make sure the files all out to disk
  if (myWrote)
  {
    fflush(myFile);
#ifndef WIN32
    fsync(fileno(myFile));
#endif
  }
  myWrote = false;
}

---