<commit_before>#include <Riostream.h>
#include "TVirtualMCApplication.h"
#include "TGeoMaterial.h"
#include "TGeoManager.h"
#include "TFlukaCerenkov.h"

#include "TFluka.h"

#include "Fdimpar.h"  //(DIMPAR) fluka include
#include "Ftrackr.h"  //(TRACKR) fluka common
#ifndef WIN32
# define endraw endraw_
#else
# define endraw ENDRAW
#endif
extern "C" {
void endraw(Int_t& icode, Int_t& mreg, Double_t& rull, Double_t& xsco, Double_t& ysco, Double_t& zsco)
{
  TFluka* fluka = (TFluka*) gMC;
  Int_t verbosityLevel = fluka->GetVerbosityLevel();
  Bool_t debug = (verbosityLevel >= 3)? kTRUE : kFALSE;
  fluka->SetCaller(3);
  fluka->SetRull(rull);
  fluka->SetXsco(xsco);
  fluka->SetYsco(ysco);
  fluka->SetZsco(zsco);
  fluka->SetMreg(mreg);
  if (icode == 11) {
    if (debug) cout << " For icode=" << icode << " Stepping is NOT called" << endl;
    return;
  }
  if (TRACKR.jtrack == -1) {
// Handle quantum efficiency the G3 way
      if (debug) printf("endraw: Cerenkov photon depositing energy: %d %e\n", mreg, rull);
      TGeoMaterial* material = (gGeoManager->GetCurrentVolume())->GetMaterial();
      // Int_t nmat = material->GetIndex();
      TFlukaCerenkov*  cerenkov = dynamic_cast<TFlukaCerenkov*> (material->GetCerenkovProperties());
      if (cerenkov) {
	  Double_t eff = (cerenkov->GetQuantumEfficiency(rull));
	  if (gRandom->Rndm() > eff) {
	      rull = 0.;
	      fluka->SetRull(rull);
	  }
      }
  }
  if (debug) printf("endraw: Depositing energy for : %d %e icode: %d \n", TRACKR.ispusr[mkbmx2-1], rull, icode);

  (TVirtualMCApplication::Instance())->Stepping();
  //
  // for icode 21,22 the particle has fallen below thresshold
  // This has to be signalled to the StepManager()
  //
  fluka->SetTrackIsNew(kFALSE);
  fluka->SetIcode(icode);
  (TVirtualMCApplication::Instance())->Stepping();

} // end of endraw
} // end of extern "C"

<commit_msg>In case of double step, set deposited energy to 0 in first step.<commit_after>#include <Riostream.h>
#include "TVirtualMCApplication.h"
#include "TGeoMaterial.h"
#include "TGeoManager.h"
#include "TFlukaCerenkov.h"

#include "TFluka.h"

#include "Fdimpar.h"  //(DIMPAR) fluka include
#include "Ftrackr.h"  //(TRACKR) fluka common
#ifndef WIN32
# define endraw endraw_
#else
# define endraw ENDRAW
#endif
extern "C" {
void endraw(Int_t& icode, Int_t& mreg, Double_t& rull, Double_t& xsco, Double_t& ysco, Double_t& zsco)
{
  TFluka* fluka = (TFluka*) gMC;
  Int_t verbosityLevel = fluka->GetVerbosityLevel();
  Bool_t debug = (verbosityLevel >= 3)? kTRUE : kFALSE;
  fluka->SetCaller(3);
  fluka->SetRull(rull);
  fluka->SetXsco(xsco);
  fluka->SetYsco(ysco);
  fluka->SetZsco(zsco);
  fluka->SetMreg(mreg);

  Float_t edep = rull;

  if (icode == 11) {
    if (debug) cout << " For icode=" << icode << " Stepping is NOT called" << endl;
    return;
  }
  if (TRACKR.jtrack == -1) {
// Handle quantum efficiency the G3 way
      if (debug) printf("endraw: Cerenkov photon depositing energy: %d %e\n", mreg, rull);
      TGeoMaterial* material = (gGeoManager->GetCurrentVolume())->GetMaterial();
      // Int_t nmat = material->GetIndex();
      TFlukaCerenkov*  cerenkov = dynamic_cast<TFlukaCerenkov*> (material->GetCerenkovProperties());
      if (cerenkov) {
	  Double_t eff = (cerenkov->GetQuantumEfficiency(rull));
	  if (gRandom->Rndm() > eff) {
	      edep = 0.;
	  }
      }
  }
  if (debug) printf("endraw: Depositing energy for : %d %e icode: %d \n", TRACKR.ispusr[mkbmx2-1], rull, icode);

  if (icode != 21 && icode != 22) {
      fluka->SetRull(edep);
      (TVirtualMCApplication::Instance())->Stepping();
  } else {
  //
  // for icode 21,22 the particle has fallen below thresshold
  // This has to be signalled to the StepManager()
  //
      fluka->SetRull(0.);
      (TVirtualMCApplication::Instance())->Stepping();
      fluka->SetTrackIsNew(kFALSE);
      fluka->SetIcode(icode);
      fluka->SetRull(edep);
      (TVirtualMCApplication::Instance())->Stepping();
  }
} // end of endraw
} // end of extern "C"

<|endoftext|>


<commit_before>#include "QCachingLocale.h"
#include "Parser.h"
#include "Analyst.h"
#include <QObject>
#include <QTime>

int main(int argc, char *argv[]) {
    QCachingLocale cl;

    // Merely instantiating QCachingLocale activates it. Use Q_UNUSED to prevent
    // compiler warnings.
    Q_UNUSED(cl);

    // FIXME: argc and argv are not yet being used.
    Q_UNUSED(argc)
    Q_UNUSED(argv);

    QTextStream cout(stdout);
    QTextStream cerr(stderr);
    QTime timer;

    EpisodesParser::Parser * parser;
    Analytics::Analyst * analyst;

    EpisodesParser::Parser::initParserHelpers("./config/browscap.csv",
                                              "./config/browscap-index.db",
                                              "./config/GeoIPCity.dat",
                                              "./config/GeoIPASNum.dat",
                                              "./config/EpisodesSpeeds.csv"
                                              );

    // Instantiate the EpisodesParser and the Analytics. Then connect them.
    parser = new EpisodesParser::Parser();
    // TODO: when these parameters have been figured out, they should be the defaults
    // and therefor they should be moved to the Analyst constructor.
    analyst = new Analytics::Analyst(0.05, 0.04, 0.2);

    // Set constraints. This defines which associations will be found. By
    // default, only causes for slow episodes will be searched.
    analyst->addFrequentItemsetItemConstraint("episode:*", Analytics::CONSTRAINT_POSITIVE_MATCH_ANY);
    analyst->addRuleConsequentItemConstraint("duration:slow", Analytics::CONSTRAINT_POSITIVE_MATCH_ANY);
    //analyst->addRuleConsequentItemConstraint("duration:acceptable", Analytics::CONSTRAINT_POSITIVE_MATCH_ANY);
    //analyst->addRuleConsequentItemConstraint("duration:fast", Analytics::CONSTRAINT_POSITIVE_MATCH_ANY);

    QObject::connect(parser, SIGNAL(processedChunk(QList<QStringList>, double)), analyst, SLOT(analyzeTransactions(QList<QStringList>, double)));

    timer.start();

    // Execute the EpisodesParser. This will generate transactions. These will
    // automatically be sent to Analytics' Analyst.
    int linesParsed = parser->parse("/Users/wimleers/School/masterthesis/logs/driverpacks.net.episodes.log");
    int timePassed = timer.elapsed();
    cout << QString("Duration: %1 ms. Parsed %2 lines. That's %3 lines/ms or %4 ms/line.")
            .arg(timePassed)
            .arg(linesParsed)
            .arg((float)linesParsed/timePassed)
            .arg((float)timePassed/linesParsed)
            << endl;
    delete parser;

    EpisodesParser::Parser::clearParserHelperCaches();

    // FIXME: remove this eventually. This is merely here to check on memory
    // consumption after everything has been deleted from memory.
    QTextStream in(stdin);
    forever {
        QString line = in.readLine();
        if (!line.isNull())
            break;
    }
}

<commit_msg>Finally, the commit in which all the hard work over the last 4 weeks reaches its ultimate status: it is now all working! In this commit, I enable rule mining over user-configurable time (bucket) ranges in the PatternTree that was generated by the FPStream algorithm implementation I built over these last 4 weeks. The essence of my thesis now *works*! :) Hurray!<commit_after>#include "QCachingLocale.h"
#include "Parser.h"
#include "Analyst.h"
#include <QObject>
#include <QTime>

int main(int argc, char *argv[]) {
    QCachingLocale cl;

    // Merely instantiating QCachingLocale activates it. Use Q_UNUSED to prevent
    // compiler warnings.
    Q_UNUSED(cl);

    // FIXME: argc and argv are not yet being used.
    Q_UNUSED(argc)
    Q_UNUSED(argv);

    QTextStream cout(stdout);
    QTextStream cerr(stderr);
    QTime timer;

    EpisodesParser::Parser * parser;
    Analytics::Analyst * analyst;

    EpisodesParser::Parser::initParserHelpers("./config/browscap.csv",
                                              "./config/browscap-index.db",
                                              "./config/GeoIPCity.dat",
                                              "./config/GeoIPASNum.dat",
                                              "./config/EpisodesSpeeds.csv"
                                              );

    // Instantiate the EpisodesParser and the Analytics. Then connect them.
    parser = new EpisodesParser::Parser();
    // TODO: when these parameters have been figured out, they should be the defaults
    // and therefor they should be moved to the Analyst constructor.
    analyst = new Analytics::Analyst(0.05, 0.04, 0.2);

    // Set constraints. This defines which associations will be found. By
    // default, only causes for slow episodes will be searched.
    analyst->addFrequentItemsetItemConstraint("episode:*", Analytics::CONSTRAINT_POSITIVE_MATCH_ANY);
    analyst->addRuleConsequentItemConstraint("duration:slow", Analytics::CONSTRAINT_POSITIVE_MATCH_ANY);
    //analyst->addRuleConsequentItemConstraint("duration:acceptable", Analytics::CONSTRAINT_POSITIVE_MATCH_ANY);
    //analyst->addRuleConsequentItemConstraint("duration:fast", Analytics::CONSTRAINT_POSITIVE_MATCH_ANY);

    QObject::connect(parser, SIGNAL(processedChunk(QList<QStringList>, double)), analyst, SLOT(analyzeTransactions(QList<QStringList>, double)));

    timer.start();

    // Execute the EpisodesParser. This will generate transactions. These will
    // automatically be sent to Analytics' Analyst.
    int linesParsed = parser->parse("/Users/wimleers/School/masterthesis/logs/driverpacks.net.episodes.log");
    int timePassed = timer.elapsed();
    cout << QString("Duration: %1 ms. Parsed %2 lines. That's %3 lines/ms or %4 ms/line.")
            .arg(timePassed)
            .arg(linesParsed)
            .arg((float)linesParsed/timePassed)
            .arg((float)timePassed/linesParsed)
            << endl;
    delete parser;

    EpisodesParser::Parser::clearParserHelperCaches();

    // Mine association rules.
    analyst->mineRules(0, (uint) TTW_NUM_BUCKETS - 1); // Rules over the entire dataset.
    analyst->mineRules(4, 4); // Rules over the past hour.

    // FIXME: remove this eventually. This is merely here to check on memory
    // consumption after everything has been deleted from memory.
    QTextStream in(stdin);
    forever {
        QString line = in.readLine();
        if (!line.isNull())
            break;
    }
}

<|endoftext|>


<commit_before>// @(#)root/roofitcore:$name:  $:$id$
// Authors: Wouter Verkerke  November 2007

// C/C++ headers
#include <list>
#include <iostream>
#include <iomanip>

// ROOT headers
#include "TWebFile.h"
#include "TSystem.h"
#include "TString.h"
#include "TROOT.h"
#include "TFile.h"
#include "TBenchmark.h"

// RooFit headers
#include "RooNumIntConfig.h"
#include "RooResolutionModel.h"
#include "RooAbsData.h"
#include "RooTrace.h"
#include "RooMath.h"
#include "RooUnitTest.h"

// Tests file
#include "stressHistFactory_tests.cxx"

using namespace std ;
using namespace RooFit ;

//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*//
//                                                                           //
// RooStats Unit Test S.T.R.E.S.S. Suite                                     //
// Authors: Ioan Gabriel Bucur, Lorenzo Moneta, Wouter Verkerke              //
//                                                                           //
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*//

//------------------------------------------------------------------------
void StatusPrint(const Int_t id, const TString &title, const Int_t status, const Int_t lineWidth)
{
   // Print test program number and its title
   TString header = TString::Format("Test %d : %s ", id, title.Data());
   cout << left << setw(lineWidth) << setfill('.') << header << " " << (status > 0 ? "OK" : (status < 0 ? "SKIPPED" : "FAILED")) << endl;
}

//______________________________________________________________________________
Int_t stressHistFactory(const char* refFile, Bool_t writeRef, Int_t verbose, Bool_t allTests, Bool_t oneTest, Int_t testNumber, Bool_t dryRun)
{
   // width of lines when printing test results
   const Int_t lineWidth = 120;

   // Save memory directory location
   RooUnitTest::setMemDir(gDirectory) ;

   std::cout << "using reference file " << refFile << std::endl;
   TFile* fref = 0 ;
   if (!dryRun) {
      if (TString(refFile).Contains("http:")) {
         if (writeRef) {
            cout << "stressHistFactory ERROR: reference file must be local file in writing mode" << endl ;
            return -1 ;
         }
         fref = new TWebFile(refFile) ;
      } else {
         fref = new TFile(refFile, writeRef ? "RECREATE" : "") ;
      }
      if (fref->IsZombie()) {
         cout << "stressHistFactory ERROR: cannot open reference file " << refFile << endl ;
         return -1;
      }
   }

   if (dryRun) {
      // Preload singletons here so they don't show up in trace accounting
      RooNumIntConfig::defaultConfig() ;
      RooResolutionModel::identity() ;

      RooTrace::active(1) ;
   }

   // Add dedicated logging stream for errors that will remain active in silent mode
   RooMsgService::instance().addStream(RooFit::ERROR) ;

   cout << left << setw(lineWidth) << setfill('*') << "" << endl;
   cout << "*" << setw(lineWidth - 2) << setfill(' ') << " Histfactory S.T.R.E.S.S. suite " << "*" << endl;
   cout << setw(lineWidth) << setfill('*') << "" << endl;
   cout << setw(lineWidth) << setfill('*') << "" << endl;

   TStopwatch timer;
   timer.Start();

   list<RooUnitTest*> testList;
   testList.push_back(new PdfComparison(fref, writeRef, verbose));

   TString suiteType = TString::Format(" Starting S.T.R.E.S.S. %s",
                                       allTests ? "full suite" : (oneTest ? TString::Format("test %d", testNumber).Data() : "basic suite")
                                      );

   cout << "*" << setw(lineWidth - 3) << setfill(' ') << suiteType << " *" << endl;
   cout << setw(lineWidth) << setfill('*') << "" << endl;

   gBenchmark->Start("stressHistFactory");

   int nFailed = 0;
   {
      Int_t i;
      list<RooUnitTest*>::iterator iter;

      if (oneTest && (testNumber <= 0 || (UInt_t) testNumber > testList.size())) {
         cout << "Tests are numbered from 1 to " << testList.size() << endl;
         return -1;
      } else {
         for (iter = testList.begin(), i = 1; iter != testList.end(); iter++, i++) {
            if (!oneTest || testNumber == i) {
               int status = (*iter)->isTestAvailable() ? (*iter)->runTest() : -1;
               StatusPrint(i, (*iter)->GetName(), status , lineWidth);
               if (!status) nFailed++;
            }
            delete *iter;
         }
      }
   }

   if (dryRun) {
      RooTrace::dump();
   }

   gBenchmark->Stop("stressHistFactory");

   //Print table with results
   Bool_t UNIX = strcmp(gSystem->GetName(), "Unix") == 0;
   cout << setw(lineWidth) << setfill('*') << "" << endl;
   if (UNIX) {
      TString sp = gSystem->GetFromPipe("uname -a");
      cout << "* SYS: " << sp << endl;
      if (strstr(gSystem->GetBuildNode(), "Linux")) {
         sp = gSystem->GetFromPipe("lsb_release -d -s");
         cout << "* SYS: " << sp << endl;
      }
      if (strstr(gSystem->GetBuildNode(), "Darwin")) {
         sp  = gSystem->GetFromPipe("sw_vers -productVersion");
         sp += " Mac OS X ";
         cout << "* SYS: " << sp << endl;
      }
   } else {
      const Char_t *os = gSystem->Getenv("OS");
      if (!os) cout << "*  SYS: Windows 95" << endl;
      else     cout << "*  SYS: " << os << " " << gSystem->Getenv("PROCESSOR_IDENTIFIER") << endl;
   }

   cout << setw(lineWidth) << setfill('*') << "" << endl;
   gBenchmark->Print("stressHistFactory");
#ifdef __CINT__
   Double_t reftime = 186.34; //pcbrun4 interpreted
#else
   Double_t reftime = 93.59; //pcbrun4 compiled
#endif
   const Double_t rootmarks = 860 * reftime / gBenchmark->GetCpuTime("stressHistFactory");

   cout << setw(lineWidth) << setfill('*') << "" << endl;
   cout << TString::Format("*  ROOTMARKS = %6.1f  *  Root %-8s %d/%d", rootmarks, gROOT->GetVersion(),
                           gROOT->GetVersionDate(), gROOT->GetVersionTime()) << endl;
   cout << setw(lineWidth) << setfill('*') << "" << endl;

   // NOTE: The function TStopwatch::CpuTime() calls Tstopwatch::Stop(), so you do not need to stop the timer separately.
   cout << "Time at the end of job = " << timer.CpuTime() << " seconds" << endl;

   if (fref) {
      fref->Close() ;
      delete fref ;
   }

   delete gBenchmark ;
   gBenchmark = 0 ;

   return nFailed;
}

//_____________________________batch only_____________________
#ifndef __CINT__

int main(int argc, const char *argv[])
{
   Bool_t doWrite     = kFALSE;
   Int_t  verbose     =      0;
   Bool_t allTests    = kFALSE;
   Bool_t oneTest     = kFALSE;
   Int_t testNumber   =      0;
   Bool_t dryRun      = kFALSE;

   string refFileName = "$ROOTSYS/test/stressHistFactory_ref.root" ;

   // Parse command line arguments
   for (Int_t i = 1 ;  i < argc ; i++) {
      string arg = argv[i] ;

      if (arg == "-f") {
         cout << "stressHistFactory: using reference file " << argv[i + 1] << endl ;
         refFileName = argv[++i] ;
      } else if (arg == "-w") {
         cout << "stressHistFactory: running in writing mode to update reference file" << endl ;
         doWrite = kTRUE ;
      } else if (arg == "-mc") {
         cout << "stressHistFactory: running in memcheck mode, no regression tests are performed" << endl;
         dryRun = kTRUE;
      } else if (arg == "-v") {
         cout << "stressHistFactory: running in verbose mode" << endl;
         verbose = 1;
      } else if (arg == "-vv") {
         cout << "stressHistFactory: running in very verbose mode" << endl;
         verbose = 2;
      } else if (arg == "-a") {
         cout << "stressHistFactory: deploying full suite of tests" << endl;
         allTests = kTRUE;
      } else if (arg == "-n") {
         cout << "stressHistFactory: running single test" << endl;
         oneTest = kTRUE;
         testNumber = atoi(argv[++i]);
      } else if (arg == "-d") {
         cout << "stressHistFactory: setting gDebug to " << argv[i + 1] << endl;
         gDebug = atoi(argv[++i]);
      } else if (arg == "-h") {
         cout << "usage: stressHistFactory [ options ] " << endl;
         cout << "" << endl;
         cout << "       -f <file> : use given reference file instead of default (" << refFileName << ")" << endl;
         cout << "       -w        : write reference file, instead of reading file and running comparison tests" << endl;
         cout << "       -n N      : only run test with sequential number N" << endl;
         cout << "       -a        : run full suite of tests (default is basic suite); this overrides the -n single test option" << endl;
         cout << "       -mc       : memory check mode, no regression test are performed. Set this flag when running with valgrind" << endl;
         cout << "       -vs       : use vector-based storage for all datasets (default is tree-based storage)" << endl;
         cout << "       -v/-vv    : set verbose mode (show result of each regression test) or very verbose mode (show all roofit output as well)" << endl;
         cout << "       -d N      : set ROOT gDebug flag to N" << endl ;
         cout << " " << endl ;
         return 0 ;
      }

   }

   // Disable caching of complex error function calculation, as we don't
   // want to write out the cache file as part of the validation procedure
   RooMath::cacheCERF(kFALSE) ;

   gBenchmark = new TBenchmark();
   return stressHistFactory(refFileName.c_str(), doWrite, verbose, allTests, oneTest, testNumber, dryRun);
}

#endif
<commit_msg>cosider skipped tests as failed ones<commit_after>// @(#)root/roofitcore:$name:  $:$id$
// Authors: Wouter Verkerke  November 2007

// C/C++ headers
#include <list>
#include <iostream>
#include <iomanip>

// ROOT headers
#include "TWebFile.h"
#include "TSystem.h"
#include "TString.h"
#include "TROOT.h"
#include "TFile.h"
#include "TBenchmark.h"

// RooFit headers
#include "RooNumIntConfig.h"
#include "RooResolutionModel.h"
#include "RooAbsData.h"
#include "RooTrace.h"
#include "RooMath.h"
#include "RooUnitTest.h"

// Tests file
#include "stressHistFactory_tests.cxx"

using namespace std ;
using namespace RooFit ;

//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*//
//                                                                           //
// RooStats Unit Test S.T.R.E.S.S. Suite                                     //
// Authors: Ioan Gabriel Bucur, Lorenzo Moneta, Wouter Verkerke              //
//                                                                           //
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*//

//------------------------------------------------------------------------
void StatusPrint(const Int_t id, const TString &title, const Int_t status, const Int_t lineWidth)
{
   // Print test program number and its title
   TString header = TString::Format("Test %d : %s ", id, title.Data());
   cout << left << setw(lineWidth) << setfill('.') << header << " " << (status > 0 ? "OK" : (status < 0 ? "SKIPPED" : "FAILED")) << endl;
}

//______________________________________________________________________________
Int_t stressHistFactory(const char* refFile, Bool_t writeRef, Int_t verbose, Bool_t allTests, Bool_t oneTest, Int_t testNumber, Bool_t dryRun)
{
   // width of lines when printing test results
   const Int_t lineWidth = 120;

   // Save memory directory location
   RooUnitTest::setMemDir(gDirectory) ;

   std::cout << "using reference file " << refFile << std::endl;
   TFile* fref = 0 ;
   if (!dryRun) {
      if (TString(refFile).Contains("http:")) {
         if (writeRef) {
            cout << "stressHistFactory ERROR: reference file must be local file in writing mode" << endl ;
            return -1 ;
         }
         fref = new TWebFile(refFile) ;
      } else {
         fref = new TFile(refFile, writeRef ? "RECREATE" : "") ;
      }
      if (fref->IsZombie()) {
         cout << "stressHistFactory ERROR: cannot open reference file " << refFile << endl ;
         return -1;
      }
   }

   if (dryRun) {
      // Preload singletons here so they don't show up in trace accounting
      RooNumIntConfig::defaultConfig() ;
      RooResolutionModel::identity() ;

      RooTrace::active(1) ;
   }

   // Add dedicated logging stream for errors that will remain active in silent mode
   RooMsgService::instance().addStream(RooFit::ERROR) ;

   cout << left << setw(lineWidth) << setfill('*') << "" << endl;
   cout << "*" << setw(lineWidth - 2) << setfill(' ') << " Histfactory S.T.R.E.S.S. suite " << "*" << endl;
   cout << setw(lineWidth) << setfill('*') << "" << endl;
   cout << setw(lineWidth) << setfill('*') << "" << endl;

   TStopwatch timer;
   timer.Start();

   list<RooUnitTest*> testList;
   testList.push_back(new PdfComparison(fref, writeRef, verbose));

   TString suiteType = TString::Format(" Starting S.T.R.E.S.S. %s",
                                       allTests ? "full suite" : (oneTest ? TString::Format("test %d", testNumber).Data() : "basic suite")
                                      );

   cout << "*" << setw(lineWidth - 3) << setfill(' ') << suiteType << " *" << endl;
   cout << setw(lineWidth) << setfill('*') << "" << endl;

   gBenchmark->Start("stressHistFactory");

   int nFailed = 0;
   {
      Int_t i;
      list<RooUnitTest*>::iterator iter;

      if (oneTest && (testNumber <= 0 || (UInt_t) testNumber > testList.size())) {
         cout << "Tests are numbered from 1 to " << testList.size() << endl;
         return -1;
      } else {
         for (iter = testList.begin(), i = 1; iter != testList.end(); iter++, i++) {
            if (!oneTest || testNumber == i) {
               int status = (*iter)->isTestAvailable() ? (*iter)->runTest() : -1;
               StatusPrint(i, (*iter)->GetName(), status , lineWidth);
               if (status <= 0) nFailed++; // successfull tests return a positive number
            }
            delete *iter;
         }
      }
   }

   if (dryRun) {
      RooTrace::dump();
   }

   gBenchmark->Stop("stressHistFactory");

   //Print table with results
   Bool_t UNIX = strcmp(gSystem->GetName(), "Unix") == 0;
   cout << setw(lineWidth) << setfill('*') << "" << endl;
   if (UNIX) {
      TString sp = gSystem->GetFromPipe("uname -a");
      cout << "* SYS: " << sp << endl;
      if (strstr(gSystem->GetBuildNode(), "Linux")) {
         sp = gSystem->GetFromPipe("lsb_release -d -s");
         cout << "* SYS: " << sp << endl;
      }
      if (strstr(gSystem->GetBuildNode(), "Darwin")) {
         sp  = gSystem->GetFromPipe("sw_vers -productVersion");
         sp += " Mac OS X ";
         cout << "* SYS: " << sp << endl;
      }
   } else {
      const Char_t *os = gSystem->Getenv("OS");
      if (!os) cout << "*  SYS: Windows 95" << endl;
      else     cout << "*  SYS: " << os << " " << gSystem->Getenv("PROCESSOR_IDENTIFIER") << endl;
   }

   cout << setw(lineWidth) << setfill('*') << "" << endl;
   gBenchmark->Print("stressHistFactory");

   Double_t reftime = 2.4; //maclm compiled
   const Double_t rootmarks = 860 * reftime / gBenchmark->GetCpuTime("stressHistFactory");

   cout << setw(lineWidth) << setfill('*') << "" << endl;
   cout << TString::Format("*  ROOTMARKS = %6.1f  *  Root %-8s %d/%d", rootmarks, gROOT->GetVersion(),
                           gROOT->GetVersionDate(), gROOT->GetVersionTime()) << endl;
   cout << setw(lineWidth) << setfill('*') << "" << endl;

   // NOTE: The function TStopwatch::CpuTime() calls Tstopwatch::Stop(), so you do not need to stop the timer separately.
   cout << "Time at the end of job = " << timer.CpuTime() << " seconds" << endl;

   if (fref) {
      fref->Close() ;
      delete fref ;
   }

   delete gBenchmark ;
   gBenchmark = 0 ;

   return nFailed;
}

//_____________________________batch only_____________________
#ifndef __CINT__

int main(int argc, const char *argv[])
{
   Bool_t doWrite     = kFALSE;
   Int_t  verbose     =      0;
   Bool_t allTests    = kFALSE;
   Bool_t oneTest     = kFALSE;
   Int_t testNumber   =      0;
   Bool_t dryRun      = kFALSE;

   string refFileName = "$ROOTSYS/test/stressHistFactory_ref.root" ;

   // Parse command line arguments
   for (Int_t i = 1 ;  i < argc ; i++) {
      string arg = argv[i] ;

      if (arg == "-f") {
         cout << "stressHistFactory: using reference file " << argv[i + 1] << endl ;
         refFileName = argv[++i] ;
      } else if (arg == "-w") {
         cout << "stressHistFactory: running in writing mode to update reference file" << endl ;
         doWrite = kTRUE ;
      } else if (arg == "-mc") {
         cout << "stressHistFactory: running in memcheck mode, no regression tests are performed" << endl;
         dryRun = kTRUE;
      } else if (arg == "-v") {
         cout << "stressHistFactory: running in verbose mode" << endl;
         verbose = 1;
      } else if (arg == "-vv") {
         cout << "stressHistFactory: running in very verbose mode" << endl;
         verbose = 2;
      } else if (arg == "-a") {
         cout << "stressHistFactory: deploying full suite of tests" << endl;
         allTests = kTRUE;
      } else if (arg == "-n") {
         cout << "stressHistFactory: running single test" << endl;
         oneTest = kTRUE;
         testNumber = atoi(argv[++i]);
      } else if (arg == "-d") {
         cout << "stressHistFactory: setting gDebug to " << argv[i + 1] << endl;
         gDebug = atoi(argv[++i]);
      } else if (arg == "-h") {
         cout << "usage: stressHistFactory [ options ] " << endl;
         cout << "" << endl;
         cout << "       -f <file> : use given reference file instead of default (" << refFileName << ")" << endl;
         cout << "       -w        : write reference file, instead of reading file and running comparison tests" << endl;
         cout << "       -n N      : only run test with sequential number N" << endl;
         cout << "       -a        : run full suite of tests (default is basic suite); this overrides the -n single test option" << endl;
         cout << "       -mc       : memory check mode, no regression test are performed. Set this flag when running with valgrind" << endl;
         cout << "       -vs       : use vector-based storage for all datasets (default is tree-based storage)" << endl;
         cout << "       -v/-vv    : set verbose mode (show result of each regression test) or very verbose mode (show all roofit output as well)" << endl;
         cout << "       -d N      : set ROOT gDebug flag to N" << endl ;
         cout << " " << endl ;
         return 0 ;
      }

   }

   // Disable caching of complex error function calculation, as we don't
   // want to write out the cache file as part of the validation procedure
   RooMath::cacheCERF(kFALSE) ;

   gBenchmark = new TBenchmark();
   return stressHistFactory(refFileName.c_str(), doWrite, verbose, allTests, oneTest, testNumber, dryRun);
}

#endif
<|endoftext|>


<commit_before>#include <rai/node/utility.hpp>

#include <rai/lib/interface.h>

#include <lmdb/libraries/liblmdb/lmdb.h>

#include <ed25519-donna/ed25519.h>

boost::filesystem::path rai::unique_path ()
{
	auto result (working_path () / boost::filesystem::unique_path ());
	return result;
}

rai::mdb_env::mdb_env (bool & error_a, boost::filesystem::path const & path_a)
{
	boost::system::error_code error;
	if (path_a.has_parent_path ())
	{
		boost::filesystem::create_directories (path_a.parent_path (), error);
		if (!error)
		{
			auto status1 (mdb_env_create (&environment));
			assert (status1 == 0);
			auto status2 (mdb_env_set_maxdbs (environment, 128));
			assert (status2 == 0);
			auto status3 (mdb_env_set_mapsize (environment, 1ULL * 1024 * 1024 * 1024 * 1024)); // 1 Terabyte
			assert (status3 == 0);
			auto status4 (mdb_env_open (environment, path_a.string ().c_str (), MDB_NOSUBDIR, 00600));
			error_a = status4 != 0;
		}
		else
		{
			error_a = true;
			environment = nullptr;
		}
	}
	else
	{
		error_a = true;
		environment = nullptr;
	}
}

rai::mdb_env::~mdb_env ()
{
	if (environment != nullptr)
	{
		mdb_env_close (environment);
	}
}

rai::mdb_env::operator MDB_env * () const
{
	return environment;
}

rai::mdb_val::mdb_val () :
value ({0, nullptr})
{
}

rai::mdb_val::mdb_val (MDB_val const & value_a) :
value (value_a)
{
}

rai::mdb_val::mdb_val (size_t size_a, void * data_a) :
value ({size_a, data_a})
{
}

rai::mdb_val::mdb_val (rai::uint128_union const & val_a) :
mdb_val (sizeof (val_a), const_cast <rai::uint128_union *> (&val_a))
{
}

rai::mdb_val::mdb_val (rai::uint256_union const & val_a) :
mdb_val (sizeof (val_a), const_cast <rai::uint256_union *> (&val_a))
{
}

void * rai::mdb_val::data () const
{
	return value.mv_data;
}

size_t rai::mdb_val::size () const
{
	return value.mv_size;
}

rai::uint256_union rai::mdb_val::uint256 () const
{
	rai::uint256_union result;
	assert (size () == sizeof (result));
	std::copy (reinterpret_cast <uint8_t const *> (data ()), reinterpret_cast <uint8_t const *> (data ()) + sizeof (result), result.bytes.data ());
	return result;
}

rai::mdb_val::operator MDB_val * () const
{
	// Allow passing a temporary to a non-c++ function which doesn't have constness
	return const_cast <MDB_val *> (&value);
};

rai::mdb_val::operator MDB_val const & () const
{
	return value;
}

rai::transaction::transaction (rai::mdb_env & environment_a, MDB_txn * parent_a, bool write) :
environment (environment_a)
{
	auto status (mdb_txn_begin (environment_a, parent_a, write ? 0 : MDB_RDONLY, &handle));
	assert (status == 0);
}

rai::transaction::~transaction ()
{
	auto status (mdb_txn_commit (handle));
	assert (status == 0);
}

rai::transaction::operator MDB_txn * () const
{
	return handle;
}

void rai::open_or_create (std::fstream & stream_a, std::string const & path_a)
{
	stream_a.open (path_a, std::ios_base::in);
	if (stream_a.fail ())
	{
		stream_a.open (path_a, std::ios_base::out);
	}
	stream_a.close ();
	stream_a.open (path_a, std::ios_base::in | std::ios_base::out);
}
<commit_msg>Using safer MDB_NOTLS in case io_threads > mdb_env_set_maxreaders<commit_after>#include <rai/node/utility.hpp>

#include <rai/lib/interface.h>

#include <lmdb/libraries/liblmdb/lmdb.h>

#include <ed25519-donna/ed25519.h>

boost::filesystem::path rai::unique_path ()
{
	auto result (working_path () / boost::filesystem::unique_path ());
	return result;
}

rai::mdb_env::mdb_env (bool & error_a, boost::filesystem::path const & path_a)
{
	boost::system::error_code error;
	if (path_a.has_parent_path ())
	{
		boost::filesystem::create_directories (path_a.parent_path (), error);
		if (!error)
		{
			auto status1 (mdb_env_create (&environment));
			assert (status1 == 0);
			auto status2 (mdb_env_set_maxdbs (environment, 128));
			assert (status2 == 0);
			auto status3 (mdb_env_set_mapsize (environment, 1ULL * 1024 * 1024 * 1024 * 1024)); // 1 Terabyte
			assert (status3 == 0);
			// It seems if there's ever more threads than mdb_env_set_maxreaders has read slots available, we get failures on transaction creation unless MDB_NOTLS is specified
			// This can happen if something like 256 io_threads are specified in the node config
			auto status4 (mdb_env_open (environment, path_a.string ().c_str (), MDB_NOSUBDIR | MDB_NOTLS, 00600));
			error_a = status4 != 0;
		}
		else
		{
			error_a = true;
			environment = nullptr;
		}
	}
	else
	{
		error_a = true;
		environment = nullptr;
	}
}

rai::mdb_env::~mdb_env ()
{
	if (environment != nullptr)
	{
		mdb_env_close (environment);
	}
}

rai::mdb_env::operator MDB_env * () const
{
	return environment;
}

rai::mdb_val::mdb_val () :
value ({0, nullptr})
{
}

rai::mdb_val::mdb_val (MDB_val const & value_a) :
value (value_a)
{
}

rai::mdb_val::mdb_val (size_t size_a, void * data_a) :
value ({size_a, data_a})
{
}

rai::mdb_val::mdb_val (rai::uint128_union const & val_a) :
mdb_val (sizeof (val_a), const_cast <rai::uint128_union *> (&val_a))
{
}

rai::mdb_val::mdb_val (rai::uint256_union const & val_a) :
mdb_val (sizeof (val_a), const_cast <rai::uint256_union *> (&val_a))
{
}

void * rai::mdb_val::data () const
{
	return value.mv_data;
}

size_t rai::mdb_val::size () const
{
	return value.mv_size;
}

rai::uint256_union rai::mdb_val::uint256 () const
{
	rai::uint256_union result;
	assert (size () == sizeof (result));
	std::copy (reinterpret_cast <uint8_t const *> (data ()), reinterpret_cast <uint8_t const *> (data ()) + sizeof (result), result.bytes.data ());
	return result;
}

rai::mdb_val::operator MDB_val * () const
{
	// Allow passing a temporary to a non-c++ function which doesn't have constness
	return const_cast <MDB_val *> (&value);
};

rai::mdb_val::operator MDB_val const & () const
{
	return value;
}

rai::transaction::transaction (rai::mdb_env & environment_a, MDB_txn * parent_a, bool write) :
environment (environment_a)
{
	auto status (mdb_txn_begin (environment_a, parent_a, write ? 0 : MDB_RDONLY, &handle));
	assert (status == 0);
}

rai::transaction::~transaction ()
{
	auto status (mdb_txn_commit (handle));
	assert (status == 0);
}

rai::transaction::operator MDB_txn * () const
{
	return handle;
}

void rai::open_or_create (std::fstream & stream_a, std::string const & path_a)
{
	stream_a.open (path_a, std::ios_base::in);
	if (stream_a.fail ())
	{
		stream_a.open (path_a, std::ios_base::out);
	}
	stream_a.close ();
	stream_a.open (path_a, std::ios_base::in | std::ios_base::out);
}
<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/PoseStamped.h>

#include "ClientSocket.h"
#include "SocketException.h"
#include <iostream>
#include <string>

#include <tf/tf.h>

#define ROS_RATE 10

using namespace std;

ClientSocket client_socket("", 54300);
const int ENC_MAX = 3932159;
const int SPEED_MAX = 32767;
const float DIST_PER_PULSE = 0.552486;  // mm par pulse
const int WHEEL_DIST = 570;             // 533;

// long int ENC_L = 0;
// long int ENC_R = 0;

pthread_t thread_odom;
pthread_t thread_pub;
pthread_mutex_t mutex_socket = PTHREAD_MUTEX_INITIALIZER;

class MachinePose_s {
 private:
 public:
  MachinePose_s() {
    ROS_DEBUG("In Mimamorukun Constructor");
    this->pos_odom.x = 0.0;
    this->pos_odom.y = 0.0;
    this->pos_odom.theta = 0.0;
    this->vel_odom.x = 0.0;
    this->vel_odom.y = 0.0;
    this->vel_odom.theta = 0.0;
  };
  ~MachinePose_s() {};
  void updateOdom();
  bool goPose(/*const geometry_msgs::Pose2D::ConstPtr& cmd_pose*/);
  bool postPose();
  geometry_msgs::Twist tgtTwist;
  geometry_msgs::Pose2D pos_odom;
  // geometry_msgs::PoseStamped pos_odom;
  // geometry_msgs::Twist    vel_odom;
  geometry_msgs::Pose2D vel_odom;

  void setCurrentPosition(geometry_msgs::Pose2D pose);
  geometry_msgs::Pose2D getCurrentPosition();
  // bool goPose2(/*const geometry_msgs::Pose2D::ConstPtr& cmd_pose*/);

} mchn_pose;

int Dist2Pulse(int dist) { return ((float)dist) / DIST_PER_PULSE; }
int Pulse2Dist(int pulse) { return ((float)pulse) * DIST_PER_PULSE; }
double Rad2Deg(double rad) { return rad * (180.0) / M_PI; }
double Deg2Rad(double deg) { return deg * M_PI / 180.0; }
// double Deg2Rad(double deg) { return deg * 3 / 180.0; }
double MM2M(double mm) { return mm * 0.001; }
double M2MM(double M) { return M * 1000; }
double sqr(double val) { return pow(val, 2); }
double Limit(double val, double max, double min) {
  if (val > max)
    return max;
  else if (min > val)
    return min;
  else
    return val;
}
double nomalizeAng(double rad) {
  while (rad > M_PI) {  //角度を-180°~180°(-π~π)の範囲に合わせる
    rad = rad - (2 * M_PI);
  }
  while (rad < -M_PI) {
    rad = rad + (2 * M_PI);
  }
  return rad;
}

void MachinePose_s::updateOdom() {
  // update Encoder value
  long int ENC_L, ENC_R;
  long int tmpENC_L = 0;
  long int tmpENC_R = 0;
  string reply;
  pthread_mutex_lock(&mutex_socket);
  client_socket << "@GP1@GP2"; /*use 250ms for send and get reply*/
  client_socket >> reply;
  pthread_mutex_unlock(&mutex_socket);
  ROS_DEBUG_STREAM("@GP raw:" << reply);
  sscanf(reply.c_str(), "@GP1,%ld@GP2,%ld", &tmpENC_L, &tmpENC_R);
  ROS_DEBUG_STREAM("tmpENC_L:" << tmpENC_L << "    tmpENC_R:" << tmpENC_R);
  if (tmpENC_L > ENC_MAX / 2)
    ENC_L = tmpENC_L - (ENC_MAX + 1);
  else
    ENC_L = tmpENC_L;
  if (tmpENC_R > ENC_MAX / 2)
    ENC_R = tmpENC_R - (ENC_MAX + 1);
  else
    ENC_R = tmpENC_R;

  static long int ENC_R_old = 0;
  static long int ENC_L_old = 0;
  double detLp /*,r , dX ,dY,dL*/;

  if (fabs(ENC_L - ENC_L_old) > ENC_MAX / 2) ENC_L_old -= ENC_MAX + 1;
  if (fabs(ENC_R - ENC_R_old) > ENC_MAX / 2) ENC_R_old -= ENC_MAX + 1;

  //エンコーダーの位置での前回からの移動距離dL_R,dL_Lを算出
  double dL_L = (double)(ENC_L - ENC_L_old) * (-DIST_PER_PULSE);
  double dL_R = (double)(ENC_R - ENC_R_old) * DIST_PER_PULSE;

  //角速度SIGMAを算出
  double POS_SIGMA = (dL_R - dL_L) / WHEEL_DIST;
  double dL = (dL_R + dL_L) * 0.50000;

  double dX = dL * cos(mchn_pose.pos_odom.theta + POS_SIGMA);  // X,Yの前回からの移動量計算
  double dY = dL * sin(mchn_pose.pos_odom.theta + POS_SIGMA);
  ENC_R_old = ENC_R;  //前回のエンコーダーの値を記録
  ENC_L_old = ENC_L;
  mchn_pose.pos_odom.x += dX;
  mchn_pose.pos_odom.y += dY;
  mchn_pose.pos_odom.theta += POS_SIGMA;
  mchn_pose.pos_odom.theta = nomalizeAng(mchn_pose.pos_odom.theta);
  mchn_pose.vel_odom.x = dX;
  mchn_pose.vel_odom.y = dY;
  mchn_pose.vel_odom.theta = POS_SIGMA;
  return;
}

/*------------------------------------
 * send command to mbed in wheel chair
 * argument:
 *   arg_speed: forward speed   [mm/sec]
 *   arg_theta: CCW turn speed  [radian/sec]
 * ----------------------------------*/
void spinWheel(/*double arg_speed, double arg_theta*/) {
  double arg_speed = mchn_pose.tgtTwist.linear.x;
  double arg_theta = mchn_pose.tgtTwist.angular.z;
  // ROS_INFO("X:%4.2f   Theta:%4.2f",arg_speed,arg_theta);
  double val_L = -Dist2Pulse(arg_speed) + Dist2Pulse((WHEEL_DIST / 2) * arg_theta);
  double val_R = Dist2Pulse(arg_speed) + Dist2Pulse((WHEEL_DIST / 2) * arg_theta);
  val_L = (int)Limit(val_L, (double)SPEED_MAX, (double)-SPEED_MAX);
  val_R = (int)Limit(val_R, (double)SPEED_MAX, (double)-SPEED_MAX);
  printf("val_L:%2.f   val_R:%2.f", val_L, val_R);

  string cmd_L = boost::lexical_cast<string>(val_L);
  string cmd_R = boost::lexical_cast<string>(val_R);

  string message;
  message = "@SS1," + cmd_L + "@SS2," + cmd_R;
  pthread_mutex_lock(&mutex_socket);
  client_socket << message;
  // message = "@SS2," + cmd_R;
  // client_socket << message;
  string reply;
  client_socket >> reply;
  pthread_mutex_unlock(&mutex_socket);
  ROS_DEBUG_STREAM("@SS raw: " << reply);
  // cout << "Response:" << reply << "   ";
}

void receiveCmdVel(const geometry_msgs::Twist::ConstPtr &cmd_vel) {
  mchn_pose.tgtTwist = *cmd_vel;
  // spinWheel(/*cmd_vel->linear.x,cmd_vel->angular.z*/);
}

void *odom_update(void *ptr) {
  // ros::Rate r(30);
  ros::Rate r(ROS_RATE);
  while (ros::ok()) {
    mchn_pose.updateOdom();
    r.sleep();
  }
}

void *publish_odom(void *ptr) {
  static ros::NodeHandle nh("~");
  // static ros::Publisher pub = nh.advertise<geometry_msgs::PoseStamped>("kalman_pose", 1);
  static ros::Publisher pub = nh.advertise<nav_msgs::Odometry>("odom", 1);
  nav_msgs::Odometry odom;
  odom.header.frame_id = "/odom";
  odom.child_frame_id = "/base_link_footprint";
  odom.pose.pose.position.z = 0.0;
  ros::Rate r(ROS_RATE);
  while (ros::ok()) {
    odom.pose.pose.position.x = mchn_pose.pos_odom.x;
    odom.pose.pose.position.y = mchn_pose.pos_odom.y;
    odom.header.stamp = ros::Time::now();
    odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(mchn_pose.pos_odom.theta);
    odom.twist.twist.linear.x = 0.0;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.angular.z = 0.0;
    pub.publish(odom);
    r.sleep();
  }
}

int main(int argc, char **argv) {
  ROS_INFO("wc_controller");
  ros::init(argc, argv, "wc_controller");
  ros::NodeHandle n;

  int Kp_, Ki_, Kd_;
  string s_Kp_, s_Ki_, s_Kd_;
  ros::NodeHandle nh_param("~");
  string tmp_ip;
  nh_param.param<string>("IP_ADDR", tmp_ip, "192.168.11.99");
  nh_param.param<int>("spin_Kp", Kp_, 4800);
  nh_param.param<int>("spin_Ki", Ki_, /*30*/ 100);
  nh_param.param<int>("spin_Kd", Kd_, 40000);
  // acces like "mimamorukun_controller/spd_Kp"
  client_socket.init(tmp_ip, 54300);
  s_Kp_ = boost::lexical_cast<string>(Kp_);
  s_Ki_ = boost::lexical_cast<string>(Ki_);
  s_Kd_ = boost::lexical_cast<string>(Kd_);

  try {
    string reply;
    try {
      // koyuusinndou 3Hz at Kp = 8000
      // client_socket << "@CR1@CR2@SM1,1@SM2,1@PP1,50@PP2,50@PI1,100@PI2,100@PD1,10@PD2,10";
      client_socket << "@CR1@CR2@SM1,1@SM2,1@PP1," + s_Kp_ + "@PP2," + s_Kp_ + "@PI1," + s_Ki_ +
                           "@PI2," + s_Ki_ + "@PD1," + s_Kd_ + "@PD2," + s_Kd_;
      client_socket >> reply;
    }
    catch (SocketException &) {
    }
    cout << "Response:" << reply << "\n";
    ;
  }
  catch (SocketException &e) {
    cout << "Exception was caught:" << e.description() << "\n";
  }

  // mchn_pose.updateVicon();
  printf("initial val  x:%4.2lf y:%4.2lf th:%4.2lf\n\r", mchn_pose.pos_odom.x, mchn_pose.pos_odom.y,
         Rad2Deg(mchn_pose.pos_odom.theta));
  // mchn_pose.setCurrentPosition(mchn_pose.pos_vicon);

  if (pthread_create(&thread_odom, NULL, odom_update, NULL)) {
    cout << "error creating thread." << endl;
    abort();
  }

  if (pthread_create(&thread_pub, NULL, publish_odom, NULL)) {
    cout << "error creating thread." << endl;
    abort();
  }

  ros::Rate r(ROS_RATE);
  while (n.ok()) {
    spinWheel();
    ros::spinOnce();
    r.sleep();
  }
  return (0);
}

/*****************************************************************************************************/

void MachinePose_s::setCurrentPosition(geometry_msgs::Pose2D pose) { pos_odom = pose; }
<commit_msg>オドメトリと速度のクラスをROSのメッセージに変更<commit_after>#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/PoseStamped.h>

#include "ClientSocket.h"
#include "SocketException.h"
#include <iostream>
#include <string>

#include <tf/tf.h>

#define ROS_RATE 10

using namespace std;

ClientSocket client_socket("", 54300);
const int ENC_MAX = 3932159;
const int SPEED_MAX = 32767;
const float DIST_PER_PULSE = 0.552486;  // mm par pulse
const int WHEEL_DIST = 570;             // 533;

// long int ENC_L = 0;
// long int ENC_R = 0;

pthread_t thread_odom;
pthread_t thread_pub;
pthread_mutex_t mutex_socket = PTHREAD_MUTEX_INITIALIZER;

class MachinePose_s {
 private:
 public:
  MachinePose_s() {
    ROS_DEBUG("In Mimamorukun Constructor");
    m_Odom.header.frame_id = "/odom";
    m_Odom.child_frame_id = "/base_link_footprint";
    m_Odom.pose.pose.position.z = 0.0;
    // this->pos_odom.x = 0.0;
    // this->pos_odom.y = 0.0;
    // this->pos_odom.theta = 0.0;
    // this->vel_odom.x = 0.0;
    // this->vel_odom.y = 0.0;
    // this->vel_odom.theta = 0.0;
  };
  ~MachinePose_s() {};
  void updateOdom();
  bool goPose(/*const geometry_msgs::Pose2D::ConstPtr& cmd_pose*/);
  bool postPose();
  nav_msgs::Odometry m_Odom;
  geometry_msgs::Twist tgtTwist;
  // geometry_msgs::Pose2D pos_odom;
  // geometry_msgs::PoseStamped pos_odom;
  // geometry_msgs::Twist    vel_odom;
  // geometry_msgs::Pose2D vel_odom;

  // void setCurrentPosition(geometry_msgs::Pose2D pose);
  void setCurrentPosition(geometry_msgs::Pose pose);
  geometry_msgs::Pose2D getCurrentPosition();
  // bool goPose2(/*const geometry_msgs::Pose2D::ConstPtr& cmd_pose*/);

} mchn_pose;

int Dist2Pulse(int dist) { return ((float)dist) / DIST_PER_PULSE; }
int Pulse2Dist(int pulse) { return ((float)pulse) * DIST_PER_PULSE; }
double Rad2Deg(double rad) { return rad * (180.0) / M_PI; }
double Deg2Rad(double deg) { return deg * M_PI / 180.0; }
// double Deg2Rad(double deg) { return deg * 3 / 180.0; }
double MM2M(double mm) { return mm * 0.001; }
double M2MM(double M) { return M * 1000; }
double sqr(double val) { return pow(val, 2); }
double Limit(double val, double max, double min) {
  if (val > max)
    return max;
  else if (min > val)
    return min;
  else
    return val;
}
double nomalizeAng(double rad) {
  while (rad > M_PI) {  //角度を-180°~180°(-π~π)の範囲に合わせる
    rad = rad - (2 * M_PI);
  }
  while (rad < -M_PI) {
    rad = rad + (2 * M_PI);
  }
  return rad;
}

void MachinePose_s::updateOdom() {
  // update Encoder value
  long int ENC_L, ENC_R;
  long int tmpENC_L = 0;
  long int tmpENC_R = 0;
  string reply;
  pthread_mutex_lock(&mutex_socket);
  client_socket << "@GP1@GP2"; /*use 250ms for send and get reply*/
  client_socket >> reply;
  pthread_mutex_unlock(&mutex_socket);
  ROS_DEBUG_STREAM("@GP raw:" << reply);
  sscanf(reply.c_str(), "@GP1,%ld@GP2,%ld", &tmpENC_L, &tmpENC_R);
  ROS_DEBUG_STREAM("tmpENC_L:" << tmpENC_L << "    tmpENC_R:" << tmpENC_R);
  if (tmpENC_L > ENC_MAX / 2)
    ENC_L = tmpENC_L - (ENC_MAX + 1);
  else
    ENC_L = tmpENC_L;
  if (tmpENC_R > ENC_MAX / 2)
    ENC_R = tmpENC_R - (ENC_MAX + 1);
  else
    ENC_R = tmpENC_R;

  static long int ENC_R_old = 0;
  static long int ENC_L_old = 0;
  double detLp /*,r , dX ,dY,dL*/;

  if (fabs(ENC_L - ENC_L_old) > ENC_MAX / 2) ENC_L_old -= ENC_MAX + 1;
  if (fabs(ENC_R - ENC_R_old) > ENC_MAX / 2) ENC_R_old -= ENC_MAX + 1;

  //エンコーダーの位置での前回からの移動距離dL_R,dL_Lを算出
  double dL_L = (double)(ENC_L - ENC_L_old) * (-DIST_PER_PULSE);
  double dL_R = (double)(ENC_R - ENC_R_old) * DIST_PER_PULSE;

  //角速度SIGMAを算出
  double POS_SIGMA = (dL_R - dL_L) / WHEEL_DIST;
  double dL = (dL_R + dL_L) * 0.50000;

  // double dX = dL * cos(mchn_pose.pos_odom.theta + POS_SIGMA);  // X,Yの前回からの移動量計算
  // double dY = dL * sin(mchn_pose.pos_odom.theta + POS_SIGMA);
  double dX =
      dL * cos(tf::getYaw(m_Odom.pose.pose.orientation) + POS_SIGMA);  // X,Yの前回からの移動量計算
  double dY = dL * sin(tf::getYaw(m_Odom.pose.pose.orientation) + POS_SIGMA);
  ENC_R_old = ENC_R;  //前回のエンコーダーの値を記録
  ENC_L_old = ENC_L;
  // mchn_pose.pos_odom.x += dX;
  // mchn_pose.pos_odom.y += dY;
  // mchn_pose.pos_odom.theta += POS_SIGMA;
  // mchn_pose.pos_odom.theta = nomalizeAng(mchn_pose.pos_odom.theta);
  // mchn_pose.vel_odom.x = dX;
  // mchn_pose.vel_odom.y = dY;
  // mchn_pose.vel_odom.theta = POS_SIGMA;
  m_Odom.pose.pose.position.x += MM2M(dX);
  m_Odom.pose.pose.position.y += MM2M(dY);
  // m_Odom.pose.pose.orientation += POS_SIGMA;
  // m_Odom.pos_odom.theta = nomalizeAng(mchn_pose.pos_odom.theta);
  tf::Quaternion q1;
  tf::quaternionMsgToTF(m_Odom.pose.pose.orientation, q1);
  tf::Quaternion q2;
  tf::quaternionMsgToTF(tf::createQuaternionMsgFromYaw(POS_SIGMA), q2);
  q1 += q2;
  tf::quaternionTFToMsg(q1, m_Odom.pose.pose.orientation);
  m_Odom.twist.twist.linear.x = MM2M(dL) / (double)ROS_RATE;
  m_Odom.twist.twist.linear.y = 0.0;
  m_Odom.twist.twist.angular.z = POS_SIGMA;
  return;
}

/*------------------------------------
 * send command to mbed in wheel chair
 * argument:
 *   arg_speed: forward speed   [mm/sec]
 *   arg_theta: CCW turn speed  [radian/sec]
 * ----------------------------------*/
void spinWheel(/*double arg_speed, double arg_theta*/) {
  double arg_speed = M2MM(mchn_pose.tgtTwist.linear.x);
  double arg_theta = mchn_pose.tgtTwist.angular.z;
  // ROS_INFO("X:%4.2f   Theta:%4.2f",arg_speed,arg_theta);
  double val_L = -Dist2Pulse(arg_speed) + Dist2Pulse((WHEEL_DIST / 2) * arg_theta);
  double val_R = Dist2Pulse(arg_speed) + Dist2Pulse((WHEEL_DIST / 2) * arg_theta);
  val_L = (int)Limit(val_L, (double)SPEED_MAX, (double)-SPEED_MAX);
  val_R = (int)Limit(val_R, (double)SPEED_MAX, (double)-SPEED_MAX);
  printf("val_L:%2.f   val_R:%2.f", val_L, val_R);

  string cmd_L = boost::lexical_cast<string>(val_L);
  string cmd_R = boost::lexical_cast<string>(val_R);

  string message;
  message = "@SS1," + cmd_L + "@SS2," + cmd_R;
  pthread_mutex_lock(&mutex_socket);
  client_socket << message;
  // message = "@SS2," + cmd_R;
  // client_socket << message;
  string reply;
  client_socket >> reply;
  pthread_mutex_unlock(&mutex_socket);
  ROS_DEBUG_STREAM("@SS raw: " << reply);
  // cout << "Response:" << reply << "   ";
}

void receiveCmdVel(const geometry_msgs::Twist::ConstPtr &cmd_vel) {
  mchn_pose.tgtTwist = *cmd_vel;
  // spinWheel(/*cmd_vel->linear.x,cmd_vel->angular.z*/);
}

void *odom_update(void *ptr) {
  // ros::Rate r(30);
  ros::Rate r(ROS_RATE);
  while (ros::ok()) {
    mchn_pose.updateOdom();
    r.sleep();
  }
}

void *publish_odom(void *ptr) {
  static ros::NodeHandle nh("~");
  // static ros::Publisher pub = nh.advertise<geometry_msgs::PoseStamped>("kalman_pose", 1);
  static ros::Publisher pub = nh.advertise<nav_msgs::Odometry>("odom", 1);
  // nav_msgs::Odometry tmp_odom;
  // tmp_odom.header.frame_id = "/odom";
  // tmp_odom.child_frame_id = "/base_link_footprint";
  // tmp_odom.pose.pose.position.z = 0.0;
  ros::Rate r(ROS_RATE);
  while (ros::ok()) {
    // tmp_odom.pose.pose.position.x = mchn_pose.pos_odom.x;
    // tmp_odom.pose.pose.position.y = mchn_pose.pos_odom.y;
    // tmp_odom.header.stamp = ros::Time::now();
    // tmp_odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(mchn_pose.pos_odom.theta);
    // tmp_odom.twist.twist.linear.x = 0.0;
    // tmp_odom.twist.twist.linear.y = 0.0;
    // tmp_odom.twist.twist.angular.z = 0.0;
    // pub.publish(tmp_odom);
    pub.publish(mchn_pose.m_Odom);
    r.sleep();
  }
}

int main(int argc, char **argv) {
  ROS_INFO("wc_controller");
  ros::init(argc, argv, "wc_controller");
  ros::NodeHandle n;

  int Kp_, Ki_, Kd_;
  string s_Kp_, s_Ki_, s_Kd_;
  ros::NodeHandle nh_param("~");
  string tmp_ip;
  nh_param.param<string>("IP_ADDR", tmp_ip, "192.168.11.99");
  nh_param.param<int>("spin_Kp", Kp_, 4800);
  nh_param.param<int>("spin_Ki", Ki_, /*30*/ 100);
  nh_param.param<int>("spin_Kd", Kd_, 40000);
  // acces like "mimamorukun_controller/spd_Kp"
  client_socket.init(tmp_ip, 54300);
  s_Kp_ = boost::lexical_cast<string>(Kp_);
  s_Ki_ = boost::lexical_cast<string>(Ki_);
  s_Kd_ = boost::lexical_cast<string>(Kd_);

  try {
    string reply;
    try {
      // koyuusinndou 3Hz at Kp = 8000
      // client_socket << "@CR1@CR2@SM1,1@SM2,1@PP1,50@PP2,50@PI1,100@PI2,100@PD1,10@PD2,10";
      client_socket << "@CR1@CR2@SM1,1@SM2,1@PP1," + s_Kp_ + "@PP2," + s_Kp_ + "@PI1," + s_Ki_ +
                           "@PI2," + s_Ki_ + "@PD1," + s_Kd_ + "@PD2," + s_Kd_;
      client_socket >> reply;
    }
    catch (SocketException &) {
    }
    cout << "Response:" << reply << "\n";
    ;
  }
  catch (SocketException &e) {
    cout << "Exception was caught:" << e.description() << "\n";
  }

  // mchn_pose.updateVicon();
  // printf("initial val  x:%4.2lf y:%4.2lf th:%4.2lf\n\r", mchn_pose.pos_odom.x,
  // mchn_pose.pos_odom.y,
  //        Rad2Deg(mchn_pose.pos_odom.theta));
  printf("initial val  x:%4.2lf y:%4.2lf th:%4.2lf\n\r", mchn_pose.m_Odom.pose.pose.position.x,
         mchn_pose.m_Odom.pose.pose.position.y,
         Rad2Deg(tf::getYaw(mchn_pose.m_Odom.pose.pose.orientation)));
  // mchn_pose.setCurrentPosition(mchn_pose.pos_vicon);

  if (pthread_create(&thread_odom, NULL, odom_update, NULL)) {
    cout << "error creating thread." << endl;
    abort();
  }

  if (pthread_create(&thread_pub, NULL, publish_odom, NULL)) {
    cout << "error creating thread." << endl;
    abort();
  }

  ros::Rate r(ROS_RATE);
  while (n.ok()) {
    spinWheel();
    ros::spinOnce();
    r.sleep();
  }
  return (0);
}

/*****************************************************************************************************/

// void MachinePose_s::setCurrentPosition(geometry_msgs::Pose2D pose) { pos_odom = pose; }
void MachinePose_s::setCurrentPosition(geometry_msgs::Pose pose) {
  m_Odom.pose.pose = pose;
  // pos_odom = pose;
}
<|endoftext|>


<commit_before>#include <iostream>
#include <unordered_map>

#include <drc_utils/LcmWrapper.hpp>
#include <ConciseArgs>
#include <lcm/lcm-cpp.hpp>

#include <bot_lcmgl_client/lcmgl.h>

#include <maps/BotWrapper.hpp>
#include <maps/Collector.hpp>
#include <maps/Utils.hpp>
#include <maps/MapManager.hpp>
#include <maps/LocalMap.hpp>
#include <maps/PointCloudView.hpp>

#include <lcmtypes/bot_core/rigid_transform_t.hpp>

#include <pcl/registration/icp.h>

struct State : public maps::Collector::DataListener {
  std::shared_ptr<drc::LcmWrapper> mLcmWrapper;
  std::shared_ptr<lcm::LCM> mLcm;
  std::shared_ptr<maps::BotWrapper> mBotWrapper;
  std::shared_ptr<maps::Collector> mCollector;
  std::string mLaserChannel;
  std::string mUpdateChannel;
  bot_lcmgl_t* mLcmGl;
  int64_t mActiveMapId;

  int64_t mTimeMin;
  int64_t mTimeMax;
  maps::PointCloud::Ptr mRefCloud;
  maps::PointCloud::Ptr mCurCloud;
  Eigen::Isometry3f mCurToRef;

  State() {
    // initialize some variables
    mLcmWrapper.reset(new drc::LcmWrapper());
    mLcm = mLcmWrapper->get();
    mBotWrapper.reset(new maps::BotWrapper(mLcm));
    mCollector.reset(new maps::Collector());
    mCollector->setBotWrapper(mBotWrapper);
    mLaserChannel = "SCAN_FREE";
    mUpdateChannel = "MAPS_LOCAL_CORRECTION";
    mActiveMapId = 1;
    mTimeMin = mTimeMax = 0;
    mLcmGl = bot_lcmgl_init(mBotWrapper->getLcm()->getUnderlyingLCM(),
                            "maps-registrar");
    mCurToRef = Eigen::Isometry3f::Identity();
  }

  ~State() {
    bot_lcmgl_destroy(mLcmGl);
  }

  void start() {
    // create new submap
    maps::LocalMap::Spec mapSpec;
    mapSpec.mId = mActiveMapId;
    mapSpec.mPointBufferSize = 5000;
    mapSpec.mActive = true;
    mapSpec.mBoundMin = Eigen::Vector3f(-1,-1,-1)*1e10;
    mapSpec.mBoundMax = Eigen::Vector3f(1,1,1)*1e10;
    mapSpec.mResolution = 0.01;
    mCollector->getMapManager()->createMap(mapSpec);

    // start running wrapper
    mCollector->getDataReceiver()->addChannel
      (mLaserChannel, maps::SensorDataReceiver::SensorTypePlanarLidar,
       mLaserChannel, "local");
    mCollector->addListener(*this);
    mCollector->start();

    // start lcm thread running
    mLcmWrapper->startHandleThread(true);
  }

  void notify(const maps::SensorDataReceiver::SensorData& iData) {
    const float kPi = 4*atan(1);
    const float kDegToRad = kPi/180;

    // find time range of swath and check if it overlaps previous one
    int64_t timeMin(0), timeMax(0);
    if (!mCollector->getLatestSwath(10*kDegToRad, 190*kDegToRad,
                                    timeMin, timeMax)) return;
    if ((timeMin == mTimeMin) && (timeMax == mTimeMax)) return;
    mTimeMin = timeMin;
    mTimeMax = timeMax;
    std::cout << "time range: " << timeMin << " " << timeMax << std::endl;

    // create space-time bounds from desired time range and around head
    Eigen::Isometry3f headToLocal;
    mBotWrapper->getTransform("head","local",headToLocal);
    Eigen::Vector3f headPos = headToLocal.translation();
    std::cout << "HEAD POS " << headPos.transpose() << std::endl;
    maps::LocalMap::SpaceTimeBounds bounds;
    bounds.mTimeMin = timeMin;
    bounds.mTimeMax = timeMax;
    Eigen::Vector3f minPoint(-2,-2,-3), maxPoint(2,2,0.5);
    bounds.mPlanes =
      maps::Utils::planesFromBox(minPoint+headPos, maxPoint+headPos);

    // get point cloud corresponding to this time range
    maps::LocalMap::Ptr localMap =
      mCollector->getMapManager()->getMap(mActiveMapId);
    maps::PointCloudView::Ptr cloudView =
      localMap->getAsPointCloud(0.01, bounds);
    mCurCloud = cloudView->getAsPointCloud();

    if (mRefCloud == NULL) {
      mRefCloud = mCurCloud;
    }

    Eigen::Isometry3f curToRef;
    if (registerToReference(mCurCloud, curToRef)) {
      std::cout << "SUCCESS" << std::endl;
      mCurToRef = curToRef;
      publishMessage(mCurToRef, timeMax);
    }
  }

  void publishMessage(const Eigen::Isometry3f& iTransform,
                      const int64_t iTime) {
    bot_core::rigid_transform_t msg;
    msg.utime = iTime;
    for (int k = 0; k < 3; ++k) msg.trans[k] = iTransform(k,3);
    Eigen::Quaternionf q(iTransform.linear());
    msg.quat[0] = q.w();
    msg.quat[1] = q.x();
    msg.quat[2] = q.y();
    msg.quat[3] = q.z();
    mLcm->publish(mUpdateChannel, &msg);
  }

  bool registerToReference(const maps::PointCloud::Ptr& iCurCloud,
                           Eigen::Isometry3f& oCurToRef) {
    typedef maps::PointCloud::PointType PointType;
    if (mRefCloud == NULL) return false;
    pcl::IterativeClosestPoint<PointType, PointType> icp;
    icp.setInputCloud(iCurCloud);
    icp.setInputTarget(mRefCloud);
    icp.setMaxCorrespondenceDistance(0.05);
    pcl::PointCloud<PointType> finalCloud;
    icp.align(finalCloud, mCurToRef.matrix());
    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
      icp.getFitnessScore() << std::endl;
    std::cout << "matching points:" << finalCloud.size() << std::endl;
    std::cout << icp.getFinalTransformation() << std::endl;
    oCurToRef.matrix() = icp.getFinalTransformation();

    // debug
    bot_lcmgl_t* lcmgl = mLcmGl;
    bot_lcmgl_color4f(lcmgl, 0.5, 0, 0, 0.3);
    bot_lcmgl_point_size(lcmgl, 3);
    for (int i = 0; i < iCurCloud->size(); ++i) {
      maps::PointCloud::PointType point = (*iCurCloud)[i];
      bot_lcmgl_begin(lcmgl, LCMGL_POINTS);
      bot_lcmgl_vertex3f(lcmgl, point.x, point.y, point.z);
      bot_lcmgl_end(lcmgl);
    }
    bot_lcmgl_color4f(lcmgl, 0, 0.5, 0, 0.3);
    bot_lcmgl_point_size(lcmgl, 3);
    for (int i = 0; i < mRefCloud->size(); ++i) {
      maps::PointCloud::PointType point = (*mRefCloud)[i];
      bot_lcmgl_begin(lcmgl, LCMGL_POINTS);
      bot_lcmgl_vertex3f(lcmgl, point.x, point.y, point.z);
      bot_lcmgl_end(lcmgl);
    }
    bot_lcmgl_color4f(lcmgl, 0, 0, 0.5,0.3);
    bot_lcmgl_point_size(lcmgl, 3);
    for (int i = 0; i < finalCloud.size(); ++i) {
      maps::PointCloud::PointType point = finalCloud[i];
      bot_lcmgl_begin(lcmgl, LCMGL_POINTS);
      bot_lcmgl_vertex3f(lcmgl, point.x, point.y, point.z);
      bot_lcmgl_end(lcmgl);
    }
    bot_lcmgl_switch_buffer(lcmgl);

    return icp.hasConverged();
  }
};

int main(const int iArgc, const char** iArgv) {

  // instantiate state object
  State state;

  // parse arguments
  ConciseArgs opt(iArgc, (char**)iArgv);
  opt.add(state.mUpdateChannel, "u", "update_channel",
          "channel on which to publish pose updates");
  opt.parse();

  state.start();

  return 0;
}
<commit_msg>added registration control message to reset module<commit_after>#include <iostream>
#include <unordered_map>
#include <thread>

#include <drc_utils/LcmWrapper.hpp>
#include <ConciseArgs>
#include <lcm/lcm-cpp.hpp>

#include <bot_lcmgl_client/lcmgl.h>

#include <maps/BotWrapper.hpp>
#include <maps/Collector.hpp>
#include <maps/Utils.hpp>
#include <maps/MapManager.hpp>
#include <maps/LocalMap.hpp>
#include <maps/PointCloudView.hpp>

#include <lcmtypes/bot_core/rigid_transform_t.hpp>
#include <lcmtypes/drc/map_registration_command_t.hpp>

#include <pcl/registration/icp.h>

struct State : public maps::Collector::DataListener {
  std::shared_ptr<drc::LcmWrapper> mLcmWrapper;
  std::shared_ptr<lcm::LCM> mLcm;
  std::shared_ptr<maps::BotWrapper> mBotWrapper;
  std::shared_ptr<maps::Collector> mCollector;
  std::string mLaserChannel;
  std::string mUpdateChannel;
  bot_lcmgl_t* mLcmGl;
  int64_t mActiveMapId;

  int64_t mTimeMin;
  int64_t mTimeMax;
  maps::PointCloud::Ptr mRefCloud;
  maps::PointCloud::Ptr mCurCloud;
  Eigen::Isometry3f mCurToRef;
  std::mutex mMutex;

  State() {
    // initialize some variables
    mLcmWrapper.reset(new drc::LcmWrapper());
    mLcm = mLcmWrapper->get();
    mBotWrapper.reset(new maps::BotWrapper(mLcm));
    mCollector.reset(new maps::Collector());
    mCollector->setBotWrapper(mBotWrapper);
    mLaserChannel = "SCAN_FREE";
    mUpdateChannel = "MAP_LOCAL_CORRECTION";
    mActiveMapId = 1;
    mTimeMin = mTimeMax = 0;
    mLcmGl = bot_lcmgl_init(mBotWrapper->getLcm()->getUnderlyingLCM(),
                            "maps-registrar");
    mCurToRef = Eigen::Isometry3f::Identity();

    mLcm->subscribe("MAP_REGISTRATION_COMMAND", &State::onCommand, this);
  }

  ~State() {
    bot_lcmgl_destroy(mLcmGl);
  }

  void onCommand(const lcm::ReceiveBuffer* iBuf,
                 const std::string& iChannel,
                 const drc::map_registration_command_t* iMessage) {
    if (iMessage->command == drc::map_registration_command_t::RESET_REFERENCE) {
      std::unique_lock<std::mutex> lock(mMutex);
      mRefCloud.reset();
      mCurToRef = Eigen::Isometry3f::Identity();
    }
  }

  void start() {
    // create new submap
    maps::LocalMap::Spec mapSpec;
    mapSpec.mId = mActiveMapId;
    mapSpec.mPointBufferSize = 5000;
    mapSpec.mActive = true;
    mapSpec.mBoundMin = Eigen::Vector3f(-1,-1,-1)*1e10;
    mapSpec.mBoundMax = Eigen::Vector3f(1,1,1)*1e10;
    mapSpec.mResolution = 0.01;
    mCollector->getMapManager()->createMap(mapSpec);

    // start running wrapper
    mCollector->getDataReceiver()->addChannel
      (mLaserChannel, maps::SensorDataReceiver::SensorTypePlanarLidar,
       mLaserChannel, "local");
    mCollector->addListener(*this);
    mCollector->start();

    // start lcm thread running
    mLcmWrapper->startHandleThread(true);
  }

  void notify(const maps::SensorDataReceiver::SensorData& iData) {
    const float kPi = 4*atan(1);
    const float kDegToRad = kPi/180;

    // find time range of swath and check if it overlaps previous one
    int64_t timeMin(0), timeMax(0);
    if (!mCollector->getLatestSwath(10*kDegToRad, 190*kDegToRad,
                                    timeMin, timeMax)) return;
    if ((timeMin == mTimeMin) && (timeMax == mTimeMax)) return;
    mTimeMin = timeMin;
    mTimeMax = timeMax;
    std::cout << "time range: " << timeMin << " " << timeMax << std::endl;

    // create space-time bounds from desired time range and around head
    Eigen::Isometry3f headToLocal;
    mBotWrapper->getTransform("head","local",headToLocal);
    Eigen::Vector3f headPos = headToLocal.translation();
    std::cout << "HEAD POS " << headPos.transpose() << std::endl;
    maps::LocalMap::SpaceTimeBounds bounds;
    bounds.mTimeMin = timeMin;
    bounds.mTimeMax = timeMax;
    Eigen::Vector3f minPoint(-2,-2,-3), maxPoint(2,2,0.5);
    bounds.mPlanes =
      maps::Utils::planesFromBox(minPoint+headPos, maxPoint+headPos);

    // get point cloud corresponding to this time range
    maps::LocalMap::Ptr localMap =
      mCollector->getMapManager()->getMap(mActiveMapId);
    maps::PointCloudView::Ptr cloudView =
      localMap->getAsPointCloud(0.01, bounds);
    mCurCloud = cloudView->getAsPointCloud();

    {
      // align clouds
      std::unique_lock<std::mutex> lock(mMutex);

      if (mRefCloud == NULL) {
        mRefCloud = mCurCloud;
      }

      Eigen::Isometry3f curToRef;
      if (registerToReference(mCurCloud, curToRef)) {
        std::cout << "SUCCESS" << std::endl;
        mCurToRef = curToRef;
        publishMessage(mCurToRef, timeMax);
      }
    }
  }

  void publishMessage(const Eigen::Isometry3f& iTransform,
                      const int64_t iTime) {
    bot_core::rigid_transform_t msg;
    msg.utime = iTime;
    for (int k = 0; k < 3; ++k) msg.trans[k] = iTransform(k,3);
    Eigen::Quaternionf q(iTransform.linear());
    msg.quat[0] = q.w();
    msg.quat[1] = q.x();
    msg.quat[2] = q.y();
    msg.quat[3] = q.z();
    mLcm->publish(mUpdateChannel, &msg);
  }

  bool registerToReference(const maps::PointCloud::Ptr& iCurCloud,
                           Eigen::Isometry3f& oCurToRef) {
    typedef maps::PointCloud::PointType PointType;
    if (mRefCloud == NULL) return false;
    pcl::IterativeClosestPoint<PointType, PointType> icp;
    icp.setInputCloud(iCurCloud);
    icp.setInputTarget(mRefCloud);
    icp.setMaxCorrespondenceDistance(0.05);
    pcl::PointCloud<PointType> finalCloud;
    icp.align(finalCloud, mCurToRef.matrix());
    std::cout << "converged?:" << icp.hasConverged() << " score: " <<
      icp.getFitnessScore() << std::endl;
    std::cout << "matching points:" << finalCloud.size() << std::endl;
    std::cout << icp.getFinalTransformation() << std::endl;
    oCurToRef.matrix() = icp.getFinalTransformation();

    // debug
    bot_lcmgl_t* lcmgl = mLcmGl;
    bot_lcmgl_color4f(lcmgl, 0.5, 0, 0, 0.3);
    bot_lcmgl_point_size(lcmgl, 3);
    for (int i = 0; i < iCurCloud->size(); ++i) {
      maps::PointCloud::PointType point = (*iCurCloud)[i];
      bot_lcmgl_begin(lcmgl, LCMGL_POINTS);
      bot_lcmgl_vertex3f(lcmgl, point.x, point.y, point.z);
      bot_lcmgl_end(lcmgl);
    }
    bot_lcmgl_color4f(lcmgl, 0, 0.5, 0, 0.3);
    bot_lcmgl_point_size(lcmgl, 3);
    for (int i = 0; i < mRefCloud->size(); ++i) {
      maps::PointCloud::PointType point = (*mRefCloud)[i];
      bot_lcmgl_begin(lcmgl, LCMGL_POINTS);
      bot_lcmgl_vertex3f(lcmgl, point.x, point.y, point.z);
      bot_lcmgl_end(lcmgl);
    }
    bot_lcmgl_color4f(lcmgl, 0, 0, 0.5,0.3);
    bot_lcmgl_point_size(lcmgl, 3);
    for (int i = 0; i < finalCloud.size(); ++i) {
      maps::PointCloud::PointType point = finalCloud[i];
      bot_lcmgl_begin(lcmgl, LCMGL_POINTS);
      bot_lcmgl_vertex3f(lcmgl, point.x, point.y, point.z);
      bot_lcmgl_end(lcmgl);
    }
    bot_lcmgl_switch_buffer(lcmgl);

    return icp.hasConverged();
  }
};

int main(const int iArgc, const char** iArgv) {

  // instantiate state object
  State state;

  // parse arguments
  ConciseArgs opt(iArgc, (char**)iArgv);
  opt.add(state.mUpdateChannel, "u", "update_channel",
          "channel on which to publish pose updates");
  opt.parse();

  state.start();

  return 0;
}
<|endoftext|>


<commit_before>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2020.  All rights reserved
 */
//  TEST    Foundation::IO::Other
#include "Stroika/Foundation/StroikaPreComp.h"

#include "Stroika/Foundation/Characters/ToString.h"
#include "Stroika/Foundation/Containers/Set.h"
#include "Stroika/Foundation/Debug/Assertions.h"
#include "Stroika/Foundation/Debug/Trace.h"
#include "Stroika/Foundation/Execution/Process.h"
#include "Stroika/Foundation/IO/FileSystem/DirectoryIterable.h"
#include "Stroika/Foundation/IO/FileSystem/DirectoryIterator.h"
#include "Stroika/Foundation/IO/FileSystem/FileOutputStream.h"
#include "Stroika/Foundation/IO/FileSystem/FileSystem.h"
#include "Stroika/Foundation/IO/FileSystem/MountedFilesystem.h"
#include "Stroika/Foundation/IO/FileSystem/PathName.h"
#include "Stroika/Foundation/IO/FileSystem/WellKnownLocations.h"

#include "../TestHarness/SimpleClass.h"
#include "../TestHarness/TestHarness.h"

using namespace Stroika;
using namespace Stroika::Foundation;
using namespace Stroika::Foundation::IO::FileSystem;

namespace {
    void Test1_DirectoryIterator_ ()
    {
        Debug::TraceContextBumper ctx ("Test1_DirectoryIterator_");
        {
            Debug::TraceContextBumper ctx1 ("simple test");
            for (DirectoryIterator i{WellKnownLocations::GetTemporary ()}; not i.Done (); ++i) {
                DbgTrace (L"filename = %s", i->c_str ());
            }
        }
        {
            Debug::TraceContextBumper ctx1 ("t2");
            DirectoryIterator         i{WellKnownLocations::GetTemporary ()};
            for (DirectoryIterator i2 = i; not i2.Done (); ++i2) {
                DbgTrace (L"filename = %s", i2->c_str ());
            }
        }
    }
}

namespace {
    void Test2_DirectoryIterable_ ()
    {
        Debug::TraceContextBumper ctx ("Test2_DirectoryIterable_");
        for (String filename : DirectoryIterable (WellKnownLocations::GetTemporary ())) {
            DbgTrace (L"filename = %s", filename.c_str ());
        }
        {
            Debug::TraceContextBumper                      ctx1 ("test-known-dir");
            static const Containers::Set<filesystem::path> kFileNamesForDir_{L"foo.txt", L"bar.png", L"t3.txt", L"blag.nope"};
            static const filesystem::path                  kTestSubDir_ = WellKnownLocations::GetTemporary () / ToPath (L"Regtest-write-files-" + Characters::ToString (Execution::GetCurrentProcessID ()));
            IO::FileSystem::Default ().RemoveDirectoryIf (kTestSubDir_, IO::FileSystem::eRemoveAnyContainedFiles);
            [[maybe_unused]] auto&& cleanup = Execution::Finally ([]() noexcept {
                IgnoreExceptionsForCall (IO::FileSystem::Default ().RemoveDirectoryIf (kTestSubDir_, IO::FileSystem::eRemoveAnyContainedFiles));
            });
            IO::FileSystem::Directory (kTestSubDir_).AssureExists ();
            kFileNamesForDir_.Apply ([](filesystem::path i) { IO::FileSystem::FileOutputStream::New (kTestSubDir_ / i); });
            //DbgTrace (L"kTestSubDir_=%s", kTestSubDir_.c_str ());
            //DbgTrace (L"kFileNamesForDir_=%s", Characters::ToString (kFileNamesForDir_).c_str ());
            //DbgTrace (L"DirectoryIterable (kTestSubDir_)=%s", Characters::ToString (DirectoryIterable (kTestSubDir_)).c_str ());
            VerifyTestResult (Containers::Set<filesystem::path>::EqualsComparer{}(kFileNamesForDir_, DirectoryIterable (kTestSubDir_)));
            {
                Containers::Set<filesystem::path>     answers1;
                Containers::Set<filesystem::path>     answers2;
                DirectoryIterable                     tmp (kTestSubDir_);
                Traversal::Iterator<filesystem::path> i2 = tmp.end (); // we had a bug with copying iterator - when refcnt != 1 - hangs - never advances... Windows only
                for (Traversal::Iterator<filesystem::path> i = tmp.begin (); i != tmp.end (); ++i) {
                    answers1 += *i;
                    i2 = i;
                    answers2 += *i;
                }
                VerifyTestResult (kFileNamesForDir_ == answers1);
                VerifyTestResult (kFileNamesForDir_ == answers2);
            }
        }
    }
}

namespace {
    namespace Test3_Pathnames_ {
        void Test_ExtractDirAndBaseName_ ()
        {
// Tests from DOCS line in ExtractDirAndBaseName
#if qPlatform_POSIX
            VerifyTestResult ((ExtractDirAndBaseName (L"/usr/lib") == pair<String, String>{L"/usr/", L"lib"}));
            VerifyTestResult ((ExtractDirAndBaseName (L"/usr/") == pair<String, String>{L"/", L"usr/"}));
            VerifyTestResult ((ExtractDirAndBaseName (L"usr") == pair<String, String>{L"./", L"usr"}));
            VerifyTestResult ((ExtractDirAndBaseName (L"/") == pair<String, String>{L"/", L""}));
            VerifyTestResult ((ExtractDirAndBaseName (L".") == pair<String, String>{L"./", L"."}));
            VerifyTestResult ((ExtractDirAndBaseName (L"..") == pair<String, String>{L"./", L".."}));
#elif qPlatform_Windows
            VerifyTestResult ((ExtractDirAndBaseName (L"\\usr\\lib") == pair<String, String>{L"\\usr\\", L"lib"}));
            VerifyTestResult ((ExtractDirAndBaseName (L"\\usr\\") == pair<String, String>{L"\\", L"usr\\"}));
            VerifyTestResult ((ExtractDirAndBaseName (L"usr") == pair<String, String>{L".\\", L"usr"}));
            VerifyTestResult ((ExtractDirAndBaseName (L"\\") == pair<String, String>{L"\\", L""}));
            VerifyTestResult ((ExtractDirAndBaseName (L".") == pair<String, String>{L".\\", L"."}));
            VerifyTestResult ((ExtractDirAndBaseName (L"..") == pair<String, String>{L".\\", L".."}));
            VerifyTestResult ((ExtractDirAndBaseName (L"c:\\h\\m.t") == pair<String, String>{L"c:\\h\\", L"m.t"}));
#endif
        }
        void Test_GetFileBaseName_ ()
        {
            VerifyTestResult (GetFileBaseName (L"foo") == L"foo");
            VerifyTestResult (GetFileBaseName (L"foo.cpp") == L"foo");
            VerifyTestResult (GetFileBaseName (L"foo.exe") == L"foo");
            VerifyTestResult (GetFileBaseName (L".exe") == L".exe");
#if qPlatform_POSIX
            VerifyTestResult (GetFileBaseName (L"/tmp/.CPUBurner") == L".CPUBurner");
#elif qPlatform_Windows
            VerifyTestResult (GetFileBaseName (L"c:\\tmp\\.CPUBurner") == L".CPUBurner");
#endif
        }

        void DoTest ()
        {
            Debug::TraceContextBumper ctx ("Test3_Pathnames_");
            Test_ExtractDirAndBaseName_ ();
            Test_GetFileBaseName_ ();
        }
    }
}

namespace {
    namespace Test4_MountedFilesystems_ {
        void DoTest ()
        {
            Debug::TraceContextBumper ctx ("Test4_MountedFilesystems_");
            for (auto i : IO::FileSystem::GetMountedFilesystems ()) {
                DbgTrace (L"fs=%s", Characters::ToString (i).c_str ());
            }
        }
    }
}

namespace {
    namespace Test5_DisksPresent_ {
        void DoTest ()
        {
            Debug::TraceContextBumper ctx ("Test5_DisksPresent_");
            for (auto i : IO::FileSystem::GetAvailableDisks ()) {
                DbgTrace (L"d=%s", Characters::ToString (i).c_str ());
            }
        }
    }
}

namespace {
    void DoRegressionTests_ ()
    {
        Test1_DirectoryIterator_ ();
        Test2_DirectoryIterable_ ();
        Test3_Pathnames_::DoTest ();
        Test4_MountedFilesystems_::DoTest ();
        Test5_DisksPresent_::DoTest ();
    }
}

int main ([[maybe_unused]] int argc, [[maybe_unused]] const char* argv[])
{
    Stroika::TestHarness::Setup ();
    return Stroika::TestHarness::PrintPassOrFail (DoRegressionTests_);
}
<commit_msg>fix regression due to filesystem change<commit_after>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2020.  All rights reserved
 */
//  TEST    Foundation::IO::Other
#include "Stroika/Foundation/StroikaPreComp.h"

#include "Stroika/Foundation/Characters/ToString.h"
#include "Stroika/Foundation/Containers/Set.h"
#include "Stroika/Foundation/Debug/Assertions.h"
#include "Stroika/Foundation/Debug/Trace.h"
#include "Stroika/Foundation/Execution/Process.h"
#include "Stroika/Foundation/IO/FileSystem/DirectoryIterable.h"
#include "Stroika/Foundation/IO/FileSystem/DirectoryIterator.h"
#include "Stroika/Foundation/IO/FileSystem/FileOutputStream.h"
#include "Stroika/Foundation/IO/FileSystem/FileSystem.h"
#include "Stroika/Foundation/IO/FileSystem/MountedFilesystem.h"
#include "Stroika/Foundation/IO/FileSystem/PathName.h"
#include "Stroika/Foundation/IO/FileSystem/WellKnownLocations.h"

#include "../TestHarness/SimpleClass.h"
#include "../TestHarness/TestHarness.h"

using namespace Stroika;
using namespace Stroika::Foundation;
using namespace Stroika::Foundation::IO::FileSystem;

namespace {
    void Test1_DirectoryIterator_ ()
    {
        Debug::TraceContextBumper ctx ("Test1_DirectoryIterator_");
        {
            Debug::TraceContextBumper ctx1 ("simple test");
            for (DirectoryIterator i{WellKnownLocations::GetTemporary ()}; not i.Done (); ++i) {
                DbgTrace (L"filename = %s", i->c_str ());
            }
        }
        {
            Debug::TraceContextBumper ctx1 ("t2");
            DirectoryIterator         i{WellKnownLocations::GetTemporary ()};
            for (DirectoryIterator i2 = i; not i2.Done (); ++i2) {
                DbgTrace (L"filename = %s", i2->c_str ());
            }
        }
    }
}

namespace {
    void Test2_DirectoryIterable_ ()
    {
        Debug::TraceContextBumper ctx ("Test2_DirectoryIterable_");
        for (filesystem::path filename : DirectoryIterable (WellKnownLocations::GetTemporary ())) {
            DbgTrace (L"filename = %s", Characters::ToString (filename).c_str ());
        }
        {
            Debug::TraceContextBumper                      ctx1 ("test-known-dir");
            static const Containers::Set<filesystem::path> kFileNamesForDir_{L"foo.txt", L"bar.png", L"t3.txt", L"blag.nope"};
            static const filesystem::path                  kTestSubDir_ = WellKnownLocations::GetTemporary () / ToPath (L"Regtest-write-files-" + Characters::ToString (Execution::GetCurrentProcessID ()));
            IO::FileSystem::Default ().RemoveDirectoryIf (kTestSubDir_, IO::FileSystem::eRemoveAnyContainedFiles);
            [[maybe_unused]] auto&& cleanup = Execution::Finally ([]() noexcept {
                IgnoreExceptionsForCall (IO::FileSystem::Default ().RemoveDirectoryIf (kTestSubDir_, IO::FileSystem::eRemoveAnyContainedFiles));
            });
            IO::FileSystem::Directory (kTestSubDir_).AssureExists ();
            kFileNamesForDir_.Apply ([](filesystem::path i) { IO::FileSystem::FileOutputStream::New (kTestSubDir_ / i); });
            //DbgTrace (L"kTestSubDir_=%s", kTestSubDir_.c_str ());
            //DbgTrace (L"kFileNamesForDir_=%s", Characters::ToString (kFileNamesForDir_).c_str ());
            //DbgTrace (L"DirectoryIterable (kTestSubDir_)=%s", Characters::ToString (DirectoryIterable (kTestSubDir_)).c_str ());
            VerifyTestResult (Containers::Set<filesystem::path>::EqualsComparer{}(kFileNamesForDir_, DirectoryIterable (kTestSubDir_)));
            {
                Containers::Set<filesystem::path>     answers1;
                Containers::Set<filesystem::path>     answers2;
                DirectoryIterable                     tmp (kTestSubDir_);
                Traversal::Iterator<filesystem::path> i2 = tmp.end (); // we had a bug with copying iterator - when refcnt != 1 - hangs - never advances... Windows only
                for (Traversal::Iterator<filesystem::path> i = tmp.begin (); i != tmp.end (); ++i) {
                    answers1 += *i;
                    i2 = i;
                    answers2 += *i;
                }
                VerifyTestResult (kFileNamesForDir_ == answers1);
                VerifyTestResult (kFileNamesForDir_ == answers2);
            }
        }
    }
}

namespace {
    namespace Test3_Pathnames_ {
        void Test_ExtractDirAndBaseName_ ()
        {
// Tests from DOCS line in ExtractDirAndBaseName
#if qPlatform_POSIX
            VerifyTestResult ((ExtractDirAndBaseName (L"/usr/lib") == pair<String, String>{L"/usr/", L"lib"}));
            VerifyTestResult ((ExtractDirAndBaseName (L"/usr/") == pair<String, String>{L"/", L"usr/"}));
            VerifyTestResult ((ExtractDirAndBaseName (L"usr") == pair<String, String>{L"./", L"usr"}));
            VerifyTestResult ((ExtractDirAndBaseName (L"/") == pair<String, String>{L"/", L""}));
            VerifyTestResult ((ExtractDirAndBaseName (L".") == pair<String, String>{L"./", L"."}));
            VerifyTestResult ((ExtractDirAndBaseName (L"..") == pair<String, String>{L"./", L".."}));
#elif qPlatform_Windows
            VerifyTestResult ((ExtractDirAndBaseName (L"\\usr\\lib") == pair<String, String>{L"\\usr\\", L"lib"}));
            VerifyTestResult ((ExtractDirAndBaseName (L"\\usr\\") == pair<String, String>{L"\\", L"usr\\"}));
            VerifyTestResult ((ExtractDirAndBaseName (L"usr") == pair<String, String>{L".\\", L"usr"}));
            VerifyTestResult ((ExtractDirAndBaseName (L"\\") == pair<String, String>{L"\\", L""}));
            VerifyTestResult ((ExtractDirAndBaseName (L".") == pair<String, String>{L".\\", L"."}));
            VerifyTestResult ((ExtractDirAndBaseName (L"..") == pair<String, String>{L".\\", L".."}));
            VerifyTestResult ((ExtractDirAndBaseName (L"c:\\h\\m.t") == pair<String, String>{L"c:\\h\\", L"m.t"}));
#endif
        }
        void Test_GetFileBaseName_ ()
        {
            VerifyTestResult (GetFileBaseName (L"foo") == L"foo");
            VerifyTestResult (GetFileBaseName (L"foo.cpp") == L"foo");
            VerifyTestResult (GetFileBaseName (L"foo.exe") == L"foo");
            VerifyTestResult (GetFileBaseName (L".exe") == L".exe");
#if qPlatform_POSIX
            VerifyTestResult (GetFileBaseName (L"/tmp/.CPUBurner") == L".CPUBurner");
#elif qPlatform_Windows
            VerifyTestResult (GetFileBaseName (L"c:\\tmp\\.CPUBurner") == L".CPUBurner");
#endif
        }

        void DoTest ()
        {
            Debug::TraceContextBumper ctx ("Test3_Pathnames_");
            Test_ExtractDirAndBaseName_ ();
            Test_GetFileBaseName_ ();
        }
    }
}

namespace {
    namespace Test4_MountedFilesystems_ {
        void DoTest ()
        {
            Debug::TraceContextBumper ctx ("Test4_MountedFilesystems_");
            for (auto i : IO::FileSystem::GetMountedFilesystems ()) {
                DbgTrace (L"fs=%s", Characters::ToString (i).c_str ());
            }
        }
    }
}

namespace {
    namespace Test5_DisksPresent_ {
        void DoTest ()
        {
            Debug::TraceContextBumper ctx ("Test5_DisksPresent_");
            for (auto i : IO::FileSystem::GetAvailableDisks ()) {
                DbgTrace (L"d=%s", Characters::ToString (i).c_str ());
            }
        }
    }
}

namespace {
    void DoRegressionTests_ ()
    {
        Test1_DirectoryIterator_ ();
        Test2_DirectoryIterable_ ();
        Test3_Pathnames_::DoTest ();
        Test4_MountedFilesystems_::DoTest ();
        Test5_DisksPresent_::DoTest ();
    }
}

int main ([[maybe_unused]] int argc, [[maybe_unused]] const char* argv[])
{
    Stroika::TestHarness::Setup ();
    return Stroika::TestHarness::PrintPassOrFail (DoRegressionTests_);
}
<|endoftext|>


<commit_before>#include <string>
#include <vector>
#include <list>
#include <map>
#include <set>
#include <deque>
#include <stack>
#include <bitset>
#include <algorithm>
#include <functional>
#include <numeric>
#include <utility>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <cstdio>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <iterator>
#include <tuple>
#include <regex>
#include <array>
#include <valarray>
#define all(v)begin(v),end(v)
#define dump(v)copy(all(v),ostream_iterator<decltype(*v.begin())>(cout,"\n"))
#define rg(i,a,b)for(int i=a,i##e=b;i<i##e;++i)
#define fr(i,n)for(int i=0,i##e=n;i<i##e;++i)
#define rf(i,n)for(int i=n-1;i>=0;--i)
#define ei(a,m)for(auto&a:m)if(int a##i=&a-&*begin(m)+1)if(--a##i,1)
#define sz(v)int(v.size())
#define sr(v)sort(all(v))
#define rs(v)sort(all(v),greater<int>())
#define rev(v)reverse(all(v))
#define eb emplace_back
#define stst stringstream
#define big numeric_limits<int>::max()
#define g(t,i)get<i>(t)
#define cb(v,w)copy(all(v),back_inserter(w))
#define uni(v)sort(all(v));v.erase(unique(all(v)),end(v))
#define vt(...)vector<tuple<__VA_ARGS__>>
#define smx(a,b)a=max(a,b)
#define smn(a,b)a=min(a,b)
#define words(w,q)vector<string>w;[&w](string&&s){stringstream u(s);string r;while(u>>r)w.eb(r);}(q);
#define digits(d,n,s)vector<int>d;[&d](int m){while(m)d.eb(m%s),m/=s;}(n);
typedef long long ll;
using namespace std;

struct TournamentsAmbiguityNumber {
	int scrutinizeTable(vector <string> t) {
		const int q = sz(t);
		int c = 0;
		fr(i, q) for (int j = i+1; j < q; ++j) for (int k = j+1; k < q; ++k) {
			if (t[i][j] == '1' && t[j][k] == '1' && t[k][i] == '1') {
				++c;
			}
		}
		return c;
	}
};

// BEGIN KAWIGIEDIT TESTING
// Generated by KawigiEdit-pf 2.3.0
#include <iostream>
#include <string>
#include <vector>
#include <ctime>
#include <cmath>
using namespace std;
bool KawigiEdit_RunTest(int testNum, vector <string> p0, bool hasAnswer, int p1) {
	cout << "Test " << testNum << ": [" << "{";
	for (int i = 0; int(p0.size()) > i; ++i) {
		if (i > 0) {
			cout << ",";
		}
		cout << "\"" << p0[i] << "\"";
	}
	cout << "}";
	cout << "]" << endl;
	TournamentsAmbiguityNumber *obj;
	int answer;
	obj = new TournamentsAmbiguityNumber();
	clock_t startTime = clock();
	answer = obj->scrutinizeTable(p0);
	clock_t endTime = clock();
	delete obj;
	bool res;
	res = true;
	cout << "Time: " << double(endTime - startTime) / CLOCKS_PER_SEC << " seconds" << endl;
	if (hasAnswer) {
		cout << "Desired answer:" << endl;
		cout << "\t" << p1 << endl;
	}
	cout << "Your answer:" << endl;
	cout << "\t" << answer << endl;
	if (hasAnswer) {
		res = answer == p1;
	}
	if (!res) {
		cout << "DOESN'T MATCH!!!!" << endl;
	} else if (double(endTime - startTime) / CLOCKS_PER_SEC >= 2) {
		cout << "FAIL the timeout" << endl;
		res = false;
	} else if (hasAnswer) {
		cout << "Match :-)" << endl;
	} else {
		cout << "OK, but is it right?" << endl;
	}
	cout << "" << endl;
	return res;
}
int main() {
	bool all_right;
	bool disabled;
	bool tests_disabled;
	all_right = true;
	tests_disabled = false;

	vector <string> p0;
	int p1;

	// ----- test 0 -----
	disabled = false;
	p0 = {"-10","0-1","10-"};
	p1 = 3;
	all_right = (disabled || KawigiEdit_RunTest(0, p0, true, p1) ) && all_right;
	tests_disabled = tests_disabled || disabled;
	// ------------------

	// ----- test 1 -----
	disabled = false;
	p0 = {"----","----","----","----"};
	p1 = 0;
	all_right = (disabled || KawigiEdit_RunTest(1, p0, true, p1) ) && all_right;
	tests_disabled = tests_disabled || disabled;
	// ------------------

	// ----- test 2 -----
	disabled = false;
	p0 = {"-1","0-"};
	p1 = 0;
	all_right = (disabled || KawigiEdit_RunTest(2, p0, true, p1) ) && all_right;
	tests_disabled = tests_disabled || disabled;
	// ------------------

	// ----- test 3 -----
	disabled = false;
	p0 = {"--1-10-1---1--1-00","--0110000--0---10-","01--00000100-00011","-0---0010-11110100","001--01-00-0001-1-","11111--100--1-1-01","-1110--00110-11-01","0110-01--100110-10","-111111---01--0-01","--0-1100----10011-","--10--011--1--101-","01101-110-0--1-0-1","---010-0-0---00-11","--101-00-1-01-0-0-","0-110001110-11-110","-010-----011--0--0","11010110100-010--0","1-01-0010--00-111-"};
	p1 = 198;
	all_right = (disabled || KawigiEdit_RunTest(3, p0, true, p1) ) && all_right;
	tests_disabled = tests_disabled || disabled;
	// ------------------

	if (all_right) {
		if (tests_disabled) {
			cout << "You're a stud (but some test cases were disabled)!" << endl;
		} else {
			cout << "You're a stud (at least on given cases)!" << endl;
		}
	} else {
		cout << "Some of the test cases had errors." << endl;
	}
	return 0;
}
// END KAWIGIEDIT TESTING
<commit_msg>TournamentsAmbiguityNumber<commit_after>#include <string>
#include <vector>
#include <list>
#include <map>
#include <set>
#include <deque>
#include <stack>
#include <bitset>
#include <algorithm>
#include <functional>
#include <numeric>
#include <utility>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <cstdio>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <iterator>
#include <tuple>
#include <regex>
#include <array>
#include <valarray>
#define all(v)begin(v),end(v)
#define dump(v)copy(all(v),ostream_iterator<decltype(*v.begin())>(cout,"\n"))
#define rg(i,a,b)for(int i=a,i##e=b;i<i##e;++i)
#define fr(i,n)for(int i=0,i##e=n;i<i##e;++i)
#define rf(i,n)for(int i=n-1;i>=0;--i)
#define ei(a,m)for(auto&a:m)if(int a##i=&a-&*begin(m)+1)if(--a##i,1)
#define sz(v)int(v.size())
#define sr(v)sort(all(v))
#define rs(v)sort(all(v),greater<int>())
#define rev(v)reverse(all(v))
#define eb emplace_back
#define stst stringstream
#define big numeric_limits<int>::max()
#define g(t,i)get<i>(t)
#define cb(v,w)copy(all(v),back_inserter(w))
#define uni(v)sort(all(v));v.erase(unique(all(v)),end(v))
#define vt(...)vector<tuple<__VA_ARGS__>>
#define smx(a,b)a=max(a,b)
#define smn(a,b)a=min(a,b)
#define words(w,q)vector<string>w;[&w](string&&s){stringstream u(s);string r;while(u>>r)w.eb(r);}(q);
#define digits(d,n,s)vector<int>d;[&d](int m){while(m)d.eb(m%s),m/=s;}(n);
typedef long long ll;
using namespace std;

struct TournamentsAmbiguityNumber {
	int scrutinizeTable(vector <string> t) {
		const int q = sz(t);
		int c = 0;
		fr(i, q) fr(j, q) fr(k, q) {
			if (t[i][j] == '1' && t[j][k] == '1' && t[k][i] == '1') {
				++c;
			}
		}
		return c;
	}
};

// BEGIN KAWIGIEDIT TESTING
// Generated by KawigiEdit-pf 2.3.0
#include <iostream>
#include <string>
#include <vector>
#include <ctime>
#include <cmath>
using namespace std;
bool KawigiEdit_RunTest(int testNum, vector <string> p0, bool hasAnswer, int p1) {
	cout << "Test " << testNum << ": [" << "{";
	for (int i = 0; int(p0.size()) > i; ++i) {
		if (i > 0) {
			cout << ",";
		}
		cout << "\"" << p0[i] << "\"";
	}
	cout << "}";
	cout << "]" << endl;
	TournamentsAmbiguityNumber *obj;
	int answer;
	obj = new TournamentsAmbiguityNumber();
	clock_t startTime = clock();
	answer = obj->scrutinizeTable(p0);
	clock_t endTime = clock();
	delete obj;
	bool res;
	res = true;
	cout << "Time: " << double(endTime - startTime) / CLOCKS_PER_SEC << " seconds" << endl;
	if (hasAnswer) {
		cout << "Desired answer:" << endl;
		cout << "\t" << p1 << endl;
	}
	cout << "Your answer:" << endl;
	cout << "\t" << answer << endl;
	if (hasAnswer) {
		res = answer == p1;
	}
	if (!res) {
		cout << "DOESN'T MATCH!!!!" << endl;
	} else if (double(endTime - startTime) / CLOCKS_PER_SEC >= 2) {
		cout << "FAIL the timeout" << endl;
		res = false;
	} else if (hasAnswer) {
		cout << "Match :-)" << endl;
	} else {
		cout << "OK, but is it right?" << endl;
	}
	cout << "" << endl;
	return res;
}
int main() {
	bool all_right;
	bool disabled;
	bool tests_disabled;
	all_right = true;
	tests_disabled = false;

	vector <string> p0;
	int p1;

	// ----- test 0 -----
	disabled = false;
	p0 = {"-10","0-1","10-"};
	p1 = 3;
	all_right = (disabled || KawigiEdit_RunTest(0, p0, true, p1) ) && all_right;
	tests_disabled = tests_disabled || disabled;
	// ------------------

	// ----- test 1 -----
	disabled = false;
	p0 = {"----","----","----","----"};
	p1 = 0;
	all_right = (disabled || KawigiEdit_RunTest(1, p0, true, p1) ) && all_right;
	tests_disabled = tests_disabled || disabled;
	// ------------------

	// ----- test 2 -----
	disabled = false;
	p0 = {"-1","0-"};
	p1 = 0;
	all_right = (disabled || KawigiEdit_RunTest(2, p0, true, p1) ) && all_right;
	tests_disabled = tests_disabled || disabled;
	// ------------------

	// ----- test 3 -----
	disabled = false;
	p0 = {"--1-10-1---1--1-00","--0110000--0---10-","01--00000100-00011","-0---0010-11110100","001--01-00-0001-1-","11111--100--1-1-01","-1110--00110-11-01","0110-01--100110-10","-111111---01--0-01","--0-1100----10011-","--10--011--1--101-","01101-110-0--1-0-1","---010-0-0---00-11","--101-00-1-01-0-0-","0-110001110-11-110","-010-----011--0--0","11010110100-010--0","1-01-0010--00-111-"};
	p1 = 198;
	all_right = (disabled || KawigiEdit_RunTest(3, p0, true, p1) ) && all_right;
	tests_disabled = tests_disabled || disabled;
	// ------------------

	if (all_right) {
		if (tests_disabled) {
			cout << "You're a stud (but some test cases were disabled)!" << endl;
		} else {
			cout << "You're a stud (at least on given cases)!" << endl;
		}
	} else {
		cout << "Some of the test cases had errors." << endl;
	}
	return 0;
}
// END KAWIGIEDIT TESTING
<|endoftext|>