ScanMatching.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP 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 3 of the License, or
// (at your option) any later version.
//
// MIP 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 MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------







#ifndef __SCAN_MATCHING_H_
#define __SCAN_MATCHING_H_

#ifdef MIP_HOST_APPLE
#include <applePatch.h>
#endif

#include <fstream>

#include <baselib.h>
#include <ScanMap.h>
#include <Task.h>
#include <Resource.h>
#include <MobileRob.h>
#include <SharedMem.h>
#include <Keyboard.h>
#include <Time.h>
#include <DraWin.h>

#include <ExternScanMatch.h>
#include <CommonOptions.h>


using namespace MipResources;


namespace MipTasks{
 
 /* @{ */
 
 
 class ScanMatchingOptions : public Options {
  public:
   DecimalOption* minSamplTime;
   DecimalOption* maxLinDist;
   DecimalOption* maxAngDist;
   DecimalOption* maxPosCorrection;
   DecimalOption* maxAngCorrection;
   DecimalOption* maxReading;
   DecimalOption* minReading;
   DecimalOption* preprocTresh;
   StringOption*  mapFileName;
   DecimalOption* maxMapLinDist;
   DecimalOption* maxMapAngDist;
   IntOption*   customDebugLevel;
   StringOption*  correct;
   
   ScanMatchingOptions();

   string getObjectName() const {
    return "ScanMatchingOptions";
   }
 };
 
 
 class ScanMatchingSharedMem : public SharedMem{
  public:
   void initSharedMem(){
   }
 };
 
 
 enum ScanMatchingPhases{
  SCANMATCH_GET_FIRST_SCAN_ONLY,
  SCANMATCH_WAIT_MIN_SAMPL_TIME_OR_GET_SCAN_AND_START_MATCH,
  SCANMATCH_WAIT_RESULT_OR_WRITE_IT,
  SCANMATCH_NUM
 };
 
 
 class ScanMatching : public Task{
  private:
   static const TaskPlate _plate = SCM_TASK;
   static const long int  _maxDurSec  = 0,     _maxDurUsec = 200000;
   static const long int  _maxSamplPeriodSec = 1, _maxSamplPeriodUsec = 0;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 100000;
   
   MobileRob* _mobileRob;
   
   ScanMatchingSharedMem *_sharedMem;
   ExternScanMatch  *_externScanMatcher;
   Keyboard *_keyboard;
   
 #ifndef MIP_PLATFORM_ARM
   DraWin* _draWin;
   unsigned int _drawList;
   unsigned int _robotDrawList;
 #endif
   
   Timer _drawTimer;
   
   string _keyCommand;
   
   Timer _minSamplTimeTimer;
   Time _minSamplTime;
   Decimal _maxLinDist;
   Decimal _maxAngDist;
   Decimal _maxPosCorrection;
   Decimal _maxAngCorrection;
   Decimal _maxReading;
   Decimal _minReading;
   Decimal _preprocTresh;
   
   bool _checkTime;
   bool _checkLinDist;
   bool _checkAngDist;
   bool _checkPosCorrection;
   bool _checkAngCorrection;
   bool _doPreprocessing;
   bool _useMap;
   bool _correct;
   
   Timer _smExecStatTimer;
   
   ScanMatchingPhases _status;
   ScanMatchingOptions _options;
   
   Pose _firstPose;
   MotionModuleTState _firstTState;
   Scan _firstScan;
   MotionModuleTState _secondTState;
   Pose _secondPose;
   Scan _secondScan;
   
 //   Pose _mapFirstPose;
 //   MotionModuleTState _mapFirstTState;
 //   Scan _mapFirstScan;
   
   Scan _backuppedFS;
   Scan _backuppedSS;
   
   Pose _globalPoseError;
   Pose _stepPoseError;
   
   Pose _correctedSecondPose;
   
   Decimal _linRangeMax;
   int _scanNumRays;
   
   ScanMap* _map;
   int _mapBisquit;
   Decimal _maxMapLinDist;
   Decimal _maxMapAngDist;
   
   stringstream _debugss;
   
   Pose _voidCorrections(Pose& firstGuessedPose,Pose& proposedCorrectedPose, bool validResult);
   void _voidExceedingReadings(Scan &s);
   void _voidReciprocalFarReadings(Scan& firstScan,Pose& firstPose,Scan& secondScan,Pose& secondPose);
   
   void _drawFirstScan();
   void _drawScanPair();
   void _drawCurrentRobot();
   void _drawCurrentRobotAndScan();
   
   Pose _predictedPose(Pose currentPose);
   
   void _preprocessingIn();
   void _preprocessingOut();
   
   void _getFirstScanOnly();
   void _waitMinSamplTimeOrGetScanAndStartMatch();
   void _waitResultOrWriteIt();
   void _recovery();
   
  public:
   ScanMatching(ResourcePointers resources,int argc, const char* argv[]);
   
   ~ScanMatching();
   
   TaskPlate getPlate() const{
    return _plate;
   }
   
   Time getMaxDuration(){
    return Time(_maxDurSec,_maxDurUsec);
   }
   
   Time getMaxSamplPeriod(){
    return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
   }
   
   Time getMinSamplPeriod(){
    return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
   }
   
   TaskOutputs run();
 };
 
 /* @} */
 
};// end namespace MipTasks

#endif