ArSickLogger.cpp

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     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 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 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 "ArRobotJoyHandler.h"
#include "ariaInternal.h"

AREXPORT ArSickLogger::ArSickLogger(ArRobot *robot, ArSick *sick, 
                    double distDiff, double degDiff, 
                    const char *fileName, bool addGoals,
                    ArJoyHandler *joyHandler,
                    const char *baseDirectory, 
                    bool useReflectorValues,
                    ArRobotJoyHandler *robotJoyHandler) :
  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;
  myUseReflectorValues = useReflectorValues;
  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);

  myLoopPacketHandlerCB.setName("ArSickLogger");
  myRobot->addPacketHandler(&myLoopPacketHandlerCB);

  myAddGoals = addGoals;
  myJoyHandler = joyHandler;
  myRobotJoyHandler = robotJoyHandler;
  myTakeReadingExplicit = false;
  myAddGoalExplicit = false;
  myAddGoalKeyboard = false;
  myLastAddGoalKeyboard = false;
  myLastJoyButton = false;
  myLastRobotJoyButton = false;
  myFirstGoalTaken = false;
  myNumGoal = 1;
  myLastLoops = 0;
  // only add goals from the keyboard if there's already a keyboard handler
  if (myAddGoals && (keyHandler = Aria::getKeyHandler()) != NULL)
  {
    // 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.");
  }
}

AREXPORT ArSickLogger::~ArSickLogger()
{
  myRobot->remUserTask(&myTaskCB);
  myRobot->remPacketHandler(&myLoopPacketHandlerCB);
  myRobot->comStr(94, "");
  if (myFile != NULL)
  {
    fprintf(myFile, "# End of log\n");
    fclose(myFile);
  }
}
  
AREXPORT 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;
}

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

AREXPORT 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);
}


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

AREXPORT 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)
{
  bool joyButton;
  bool robotJoyButton;

  // 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;
  
  if (myJoyHandler != NULL)
    joyButton = (myJoyHandler->getButton(2) || 
           myJoyHandler->getButton(3) || 
           myJoyHandler->getButton(4));
  else
    joyButton = (myRobot->getFlags() & ArUtil::BIT9);
  
  if (myRobotJoyHandler != NULL)
    robotJoyButton = myRobotJoyHandler->getButton2();
  else
    robotJoyButton = false;

  // see if we want to add a goal... note that if the button is pushed
  // it must have been unpushed at one point to cause the goal to
  // trigger
  if (myRobot->isConnected() && 
      (myAddGoalExplicit ||
       (myAddGoalKeyboard && !myLastAddGoalKeyboard) ||
       (joyButton && !myLastJoyButton) ||
       (robotJoyButton && !myLastRobotJoyButton)))
  {
    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);
    ArLog::log(ArLog::Normal, "Goal %d taken", myNumGoal);
    myNumGoal++;
  }
  myLastAddGoalKeyboard = myAddGoalKeyboard;
  myLastJoyButton = joyButton;
  myLastRobotJoyButton = robotJoyButton;
      
  // 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;
  bool usingAdjustedReadings;

  // 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();
    if ((readings = mySick->getAdjustedRawReadings()) != NULL)
    {
      usingAdjustedReadings = true;
    }
    else
    {
      usingAdjustedReadings = false;
      readings = mySick->getRawReadings();
    }
    if (readings == NULL || (it = readings->begin()) == readings->end() ||
    myFile == NULL)
    {
      mySick->unlockDevice();
      return;
    }
    myTakeReadingExplicit = false;
    myScanNumber++;
    if (usingAdjustedReadings)
      ArLog::log(ArLog::Normal, 
         "Taking adjusted readings from the %d laser values", 
         readings->size());
    else
      ArLog::log(ArLog::Normal, 
         "Taking readings from the %d laser values", 
         readings->size());
    myFirstTaken = true;
    myLast = myRobot->getEncoderPose();
    poseTaken = (*readings->begin())->getEncoderPoseTaken();
    myLastVel = myRobot->getVel();
    msec = myStartTime.mSecSince();
    fprintf(myFile, "scan1Id: %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 (myUseReflectorValues)
    {
      fprintf(myFile, "reflector1: ");
      
      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, "%d ", reading->getExtraInt());
      else
        fprintf(myFile, "0 ");
    }
      }
      else
      {
    for (rit = readings->rbegin(); rit != readings->rend(); rit++)
    {
      reading = (*rit);
      if (!reading->getIgnoreThisReading())
        fprintf(myFile, "%d ", reading->getExtraInt());
      else
        fprintf(myFile, "0 ");
    }
      }
      fprintf(myFile, "\n");
    }
    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());
}

AREXPORT 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;
}

---