<commit_before>// @(#)root/pythia8:$Name$:$Id$
// Author: Andreas Morsch   27/10/2007

/*************************************************************************
 * Copyright (C) 1995-2008, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

////////////////////////////////////////////////////////////////////////////////
//                                                                            //
// TPythia8                                                                   //
//                                                                            //
// TPythia is an interface class to C++ version of Pythia 8.1                 //
// event generators, written by T.Sjostrand.                                  //
////////////////////////////////////////////////////////////////////////////////
/*
*------------------------------------------------------------------------------------*
 |                                                                                |
 |  *------------------------------------------------------------------------------*  |
 |  |                                                                              |  |
 |  |                                                                              |  |
 |  |   PPP   Y   Y  TTTTT  H   H  III    A      Welcome to the Lund Monte Carlo!  |  |
 |  |   P  P   Y Y     T    H   H   I    A A     This is PYTHIA version 8.100      |  |
 |  |   PPP     Y      T    HHHHH   I   AAAAA    Last date of change: 20 Oct 2007  |  |
 |  |   P       Y      T    H   H   I   A   A                                      |  |
 |  |   P       Y      T    H   H  III  A   A    Now is 27 Oct 2007 at 18:26:53    |  |
 |  |                                                                              |  |
 |  |   Main author: Torbjorn Sjostrand; CERN/PH, CH-1211 Geneva, Switzerland,     |  |
 |  |     and Department of Theoretical Physics, Lund University, Lund, Sweden;    |  |
 |  |     phone: + 41 - 22 - 767 82 27; e-mail: torbjorn@thep.lu.se                |  |
 |  |   Author: Stephen Mrenna; Computing Division, Simulations Group,             |  |
 |  |     Fermi National Accelerator Laboratory, MS 234, Batavia, IL 60510, USA;   |  |
 |  |     phone: + 1 - 630 - 840 - 2556; e-mail: mrenna@fnal.gov                   |  |
 |  |   Author: Peter Skands; CERN/PH, CH-1211 Geneva, Switzerland,                |  |
 |  |     and Theoretical Physics Department,                                      |  |
 |  |     Fermi National Accelerator Laboratory, MS 106, Batavia, IL 60510, USA;   |  |
 |  |     phone: + 41 - 22 - 767 24 59; e-mail: skands@fnal.gov                    |  |
 |  |                                                                              |  |
 |  |   The main program reference is the 'Brief Introduction to PYTHIA 8.1',      |  |
 |  |   T. Sjostrand, S. Mrenna and P. Skands, arXiv:0710.3820                     |  |
 |  |                                                                              |  |
 |  |   The main physics reference is the 'PYTHIA 6.4 Physics and Manual',         |  |
 |  |   T. Sjostrand, S. Mrenna and P. Skands, JHEP05 (2006) 026 [hep-ph/0603175]. |  |
 |  |                                                                              |  |
 |  |   An archive of program versions and documentation is found on the web:      |  |
 |  |   http://www.thep.lu.se/~torbjorn/Pythia.html                                |  |
 |  |                                                                              |  |
 |  |   This program is released under the GNU General Public Licence version 2.   |  |
 |  |   Please respect the MCnet Guidelines for Event Generator Authors and Users. |  |
 |  |                                                                              |  |
 |  |   Disclaimer: this program comes without any guarantees.                     |  |
 |  |   Beware of errors and use common sense when interpreting results.           |  |
 |  |                                                                              |  |
 |  |   Copyright (C) 2007 Torbjorn Sjostrand                                      |  |
 |  |                                                                              |  |
 |  |                                                                              |  |
 |  *------------------------------------------------------------------------------*  |
 |                                                                                |
 *------------------------------------------------------------------------------------*
*/

#include "TPythia8.h"

#include "TClonesArray.h"
#include <TParticle.h>

ClassImp(TPythia8)

TPythia8*  TPythia8::fgInstance = 0;

//___________________________________________________________________________
TPythia8::TPythia8():
    TGenerator("TPythia8", "TPythia8"),
    fPythia(0),
    fNumberOfParticles(0)
{
   // Constructor
   if (fgInstance)
      Fatal("TPythia8", "There's already an instance of TPythia8");

   delete fParticles; // was allocated as TObjArray in TGenerator

   fParticles = new TClonesArray("TParticle",50);
   fPythia    = new Pythia8::Pythia();
}

//___________________________________________________________________________
TPythia8::~TPythia8()
{
   // Destructor
   if (fParticles) {
      fParticles->Delete();
      delete fParticles;
      fParticles = 0;
   }
   delete fPythia;
}

//___________________________________________________________________________
TPythia8* TPythia8::Instance() {
   // Return an instance of TPythia8
   return fgInstance ? fgInstance : (fgInstance = new TPythia8()) ;
}

//___________________________________________________________________________
Bool_t TPythia8::Initialize(Int_t  idAin, Int_t idBin, Double_t ecms)
{
   // Initialization
   AddParticlesToPdgDataBase();
   return fPythia->init(idAin, idBin, ecms);
}

//___________________________________________________________________________
void TPythia8::GenerateEvent()
{
   // Generate the next event
   fPythia->next();
   fNumberOfParticles  = fPythia->event.size();
   ImportParticles();
}

//___________________________________________________________________________
Int_t TPythia8::ImportParticles(TClonesArray *particles, Option_t *option)
{
   // Import particles from Pythia stack
   if (particles == 0) return 0;
   TClonesArray &clonesParticles = *particles;
   clonesParticles.Clear();
   Int_t nparts=0;
   Int_t i;
   if (!strcmp(option,"") || !strcmp(option,"Final")) {
      for (i = 1; i <= fNumberOfParticles; i++) {
         if (fPythia->event[i].isFinal()) {
            new(clonesParticles[nparts]) TParticle(
                fPythia->event[i].id(),
                fPythia->event[i].isFinal(),
                fPythia->event[i].mother1(),
                fPythia->event[i].mother2(),
                fPythia->event[i].daughter1(),
                fPythia->event[i].daughter2(),
                fPythia->event[i].px(),     // [GeV/c]
                fPythia->event[i].py(),     // [GeV/c]
                fPythia->event[i].pz(),     // [GeV/c]
                fPythia->event[i].e(),      // [GeV]
                fPythia->event[i].xProd(),  // [mm]
                fPythia->event[i].yProd(),  // [mm]
                fPythia->event[i].zProd(),  // [mm]
                fPythia->event[i].tProd()); // [mm/c]
		nparts++;
	    } // final state partice
	} // particle loop
    } else if (!strcmp(option,"All")) {
	for (i = 1; i <= fNumberOfParticles; i++) {
	    new(clonesParticles[nparts]) TParticle(
		fPythia->event[i].id(),
		fPythia->event[i].isFinal(),
		fPythia->event[i].mother1(),
		fPythia->event[i].mother2(),
		fPythia->event[i].daughter1(),
		fPythia->event[i].daughter2(),
		fPythia->event[i].px(),       // [GeV/c]
		fPythia->event[i].py(),       // [GeV/c]
		fPythia->event[i].pz(),       // [GeV/c]
		fPythia->event[i].e(),        // [GeV]
		fPythia->event[i].xProd(),    // [mm]
		fPythia->event[i].yProd(),    // [mm]
		fPythia->event[i].zProd(),    // [mm]
		fPythia->event[i].tProd());   // [mm/c]
	    nparts++;
	} // particle loop
    }
    return nparts;
}

//___________________________________________________________________________
TObjArray* TPythia8::ImportParticles(Option_t* /* option */)
{
   // Import particles from Pythia stack
   fParticles->Clear();
   Int_t numpart   = fPythia->event.size();
   TClonesArray &a = *((TClonesArray*)fParticles);
   for (Int_t i = 0; i<numpart; i++) {
      new(a[i]) TParticle(
         fPythia->event[i].id(),
         fPythia->event[i].isFinal(),
         fPythia->event[i].mother1(),
         fPythia->event[i].mother2(),
         fPythia->event[i].daughter1(),
         fPythia->event[i].daughter2(),
         fPythia->event[i].px(),       // [GeV/c]
         fPythia->event[i].py(),       // [GeV/c]
         fPythia->event[i].pz(),       // [GeV/c]
         fPythia->event[i].e(),        // [GeV]
         fPythia->event[i].xProd(),    // [mm]
         fPythia->event[i].yProd(),    // [mm]
         fPythia->event[i].zProd(),    // [mm]
         fPythia->event[i].tProd());   // [mm/c]
   }
   return fParticles;
}

//___________________________________________________________________________
Int_t TPythia8::GetN() const
{
   // Initialization
   return (fPythia->event.size());
}

//___________________________________________________________________________
void TPythia8::ReadString(char* string) const
{
   // Configuration
   fPythia->readString(string);
}

//___________________________________________________________________________
void TPythia8::PrintStatistics() const
{
   // Print end of run statistics
   fPythia->statistics();
}

//___________________________________________________________________________
void TPythia8::EventListing() const
{
   // Event listing
   fPythia->event.list();
}

//___________________________________________________________________________
void TPythia8::AddParticlesToPdgDataBase()
{
   // Add some pythia specific particle code to the data base

   TDatabasePDG *pdgDB = TDatabasePDG::Instance();
   pdgDB->AddParticle("string","string", 0, kTRUE,
                      0, 0, "QCD string", 90);
   pdgDB->AddParticle("rho_diff0", "rho_diff0", 0, kTRUE,
                      0, 0, "QCD diffr. state", 9900110);
   pdgDB->AddParticle("pi_diffr+", "pi_diffr+", 0, kTRUE,
                      0, 1, "QCD diffr. state", 9900210);
   pdgDB->AddParticle("omega_di", "omega_di", 0, kTRUE,
                      0, 0, "QCD diffr. state", 9900220);
   pdgDB->AddParticle("phi_diff","phi_diff", 0, kTRUE,
                      0, 0, "QCD diffr. state", 9900330);
   pdgDB->AddParticle("J/psi_di", "J/psi_di", 0, kTRUE,
                      0, 0, "QCD diffr. state", 9900440);
   pdgDB->AddParticle("n_diffr0","n_diffr0",0,kTRUE,
                      0, 0, "QCD diffr. state", 9902110);
   pdgDB->AddParticle("p_diffr+","p_diffr+", 0, kTRUE,
                      0, 1, "QCD diffr. state", 9902210);
}
<commit_msg>From Andreas Morsch: Correct number of particles and references.<commit_after>// @(#)root/pythia8:$Name$:$Id$
// Author: Andreas Morsch   27/10/2007

/*************************************************************************
 * Copyright (C) 1995-2008, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

////////////////////////////////////////////////////////////////////////////////
//                                                                            //
// TPythia8                                                                   //
//                                                                            //
// TPythia is an interface class to C++ version of Pythia 8.1                 //
// event generators, written by T.Sjostrand.                                  //
////////////////////////////////////////////////////////////////////////////////
/*
*------------------------------------------------------------------------------------*
 |                                                                                |
 |  *------------------------------------------------------------------------------*  |
 |  |                                                                              |  |
 |  |                                                                              |  |
 |  |   PPP   Y   Y  TTTTT  H   H  III    A      Welcome to the Lund Monte Carlo!  |  |
 |  |   P  P   Y Y     T    H   H   I    A A     This is PYTHIA version 8.100      |  |
 |  |   PPP     Y      T    HHHHH   I   AAAAA    Last date of change: 20 Oct 2007  |  |
 |  |   P       Y      T    H   H   I   A   A                                      |  |
 |  |   P       Y      T    H   H  III  A   A    Now is 27 Oct 2007 at 18:26:53    |  |
 |  |                                                                              |  |
 |  |   Main author: Torbjorn Sjostrand; CERN/PH, CH-1211 Geneva, Switzerland,     |  |
 |  |     and Department of Theoretical Physics, Lund University, Lund, Sweden;    |  |
 |  |     phone: + 41 - 22 - 767 82 27; e-mail: torbjorn@thep.lu.se                |  |
 |  |   Author: Stephen Mrenna; Computing Division, Simulations Group,             |  |
 |  |     Fermi National Accelerator Laboratory, MS 234, Batavia, IL 60510, USA;   |  |
 |  |     phone: + 1 - 630 - 840 - 2556; e-mail: mrenna@fnal.gov                   |  |
 |  |   Author: Peter Skands; CERN/PH, CH-1211 Geneva, Switzerland,                |  |
 |  |     and Theoretical Physics Department,                                      |  |
 |  |     Fermi National Accelerator Laboratory, MS 106, Batavia, IL 60510, USA;   |  |
 |  |     phone: + 41 - 22 - 767 24 59; e-mail: skands@fnal.gov                    |  |
 |  |                                                                              |  |
 |  |   The main program reference is the 'Brief Introduction to PYTHIA 8.1',      |  |
 |  |   T. Sjostrand, S. Mrenna and P. Skands, arXiv:0710.3820                     |  |
 |  |                                                                              |  |
 |  |   The main physics reference is the 'PYTHIA 6.4 Physics and Manual',         |  |
 |  |   T. Sjostrand, S. Mrenna and P. Skands, JHEP05 (2006) 026 [hep-ph/0603175]. |  |
 |  |                                                                              |  |
 |  |   An archive of program versions and documentation is found on the web:      |  |
 |  |   http://www.thep.lu.se/~torbjorn/Pythia.html                                |  |
 |  |                                                                              |  |
 |  |   This program is released under the GNU General Public Licence version 2.   |  |
 |  |   Please respect the MCnet Guidelines for Event Generator Authors and Users. |  |
 |  |                                                                              |  |
 |  |   Disclaimer: this program comes without any guarantees.                     |  |
 |  |   Beware of errors and use common sense when interpreting results.           |  |
 |  |                                                                              |  |
 |  |   Copyright (C) 2007 Torbjorn Sjostrand                                      |  |
 |  |                                                                              |  |
 |  |                                                                              |  |
 |  *------------------------------------------------------------------------------*  |
 |                                                                                |
 *------------------------------------------------------------------------------------*
*/

#include "TPythia8.h"

#include "TClonesArray.h"
#include <TParticle.h>

ClassImp(TPythia8)

TPythia8*  TPythia8::fgInstance = 0;

//___________________________________________________________________________
TPythia8::TPythia8():
    TGenerator("TPythia8", "TPythia8"),
    fPythia(0),
    fNumberOfParticles(0)
{
   // Constructor
   if (fgInstance)
      Fatal("TPythia8", "There's already an instance of TPythia8");

   delete fParticles; // was allocated as TObjArray in TGenerator

   fParticles = new TClonesArray("TParticle",50);
   fPythia    = new Pythia8::Pythia();
}

//___________________________________________________________________________
TPythia8::~TPythia8()
{
   // Destructor
   if (fParticles) {
      fParticles->Delete();
      delete fParticles;
      fParticles = 0;
   }
   delete fPythia;
}

//___________________________________________________________________________
TPythia8* TPythia8::Instance() {
   // Return an instance of TPythia8
   return fgInstance ? fgInstance : (fgInstance = new TPythia8()) ;
}

//___________________________________________________________________________
Bool_t TPythia8::Initialize(Int_t  idAin, Int_t idBin, Double_t ecms)
{
   // Initialization
   AddParticlesToPdgDataBase();
   return fPythia->init(idAin, idBin, ecms);
}

//___________________________________________________________________________
void TPythia8::GenerateEvent()
{
   // Generate the next event
   fPythia->next();
   fNumberOfParticles  = fPythia->event.size() - 1;
   ImportParticles();
}

//___________________________________________________________________________
Int_t TPythia8::ImportParticles(TClonesArray *particles, Option_t *option)
{
   // Import particles from Pythia stack
   if (particles == 0) return 0;
   TClonesArray &clonesParticles = *particles;
   clonesParticles.Clear();
   Int_t nparts=0;
   Int_t i;
   if (!strcmp(option,"") || !strcmp(option,"Final")) {
      for (i = 1; i <= fNumberOfParticles; i++) {
         if (fPythia->event[i].isFinal()) {
            new(clonesParticles[nparts]) TParticle(
                fPythia->event[i].id(),
                fPythia->event[i].isFinal(),
                fPythia->event[i].mother1() - 1,
                fPythia->event[i].mother2() - 1,
                fPythia->event[i].daughter1() - 1,
                fPythia->event[i].daughter2() - 1,
                fPythia->event[i].px(),     // [GeV/c]
                fPythia->event[i].py(),     // [GeV/c]
                fPythia->event[i].pz(),     // [GeV/c]
                fPythia->event[i].e(),      // [GeV]
                fPythia->event[i].xProd(),  // [mm]
                fPythia->event[i].yProd(),  // [mm]
                fPythia->event[i].zProd(),  // [mm]
                fPythia->event[i].tProd()); // [mm/c]
		nparts++;
	    } // final state partice
	} // particle loop
    } else if (!strcmp(option,"All")) {
	for (i = 1; i <= fNumberOfParticles; i++) {
	    new(clonesParticles[nparts]) TParticle(
		fPythia->event[i].id(),
		fPythia->event[i].isFinal(),
		fPythia->event[i].mother1() - 1,
		fPythia->event[i].mother2() - 1,
		fPythia->event[i].daughter1() - 1,
		fPythia->event[i].daughter2() - 1,
		fPythia->event[i].px(),       // [GeV/c]
		fPythia->event[i].py(),       // [GeV/c]
		fPythia->event[i].pz(),       // [GeV/c]
		fPythia->event[i].e(),        // [GeV]
		fPythia->event[i].xProd(),    // [mm]
		fPythia->event[i].yProd(),    // [mm]
		fPythia->event[i].zProd(),    // [mm]
		fPythia->event[i].tProd());   // [mm/c]
	    nparts++;
	} // particle loop
    }
    return nparts;
}

//___________________________________________________________________________
TObjArray* TPythia8::ImportParticles(Option_t* /* option */)
{
   // Import particles from Pythia stack
   fParticles->Clear();
   Int_t numpart   = fPythia->event.size() - 1;
   TClonesArray &a = *((TClonesArray*)fParticles);
   for (Int_t i = 1; i <= numpart; i++) {
      new(a[i]) TParticle(
         fPythia->event[i].id(),
         fPythia->event[i].isFinal(),
         fPythia->event[i].mother1()  - 1,
         fPythia->event[i].mother2()  - 1,
         fPythia->event[i].daughter1() - 1,
         fPythia->event[i].daughter2() - 1,
         fPythia->event[i].px(),       // [GeV/c]
         fPythia->event[i].py(),       // [GeV/c]
         fPythia->event[i].pz(),       // [GeV/c]
         fPythia->event[i].e(),        // [GeV]
         fPythia->event[i].xProd(),    // [mm]
         fPythia->event[i].yProd(),    // [mm]
         fPythia->event[i].zProd(),    // [mm]
         fPythia->event[i].tProd());   // [mm/c]
   }
   return fParticles;
}

//___________________________________________________________________________
Int_t TPythia8::GetN() const
{
   // Initialization
   return (fPythia->event.size() - 1);
}

//___________________________________________________________________________
void TPythia8::ReadString(char* string) const
{
   // Configuration
   fPythia->readString(string);
}

//___________________________________________________________________________
void TPythia8::PrintStatistics() const
{
   // Print end of run statistics
   fPythia->statistics();
}

//___________________________________________________________________________
void TPythia8::EventListing() const
{
   // Event listing
   fPythia->event.list();
}

//___________________________________________________________________________
void TPythia8::AddParticlesToPdgDataBase()
{
   // Add some pythia specific particle code to the data base

   TDatabasePDG *pdgDB = TDatabasePDG::Instance();
   pdgDB->AddParticle("string","string", 0, kTRUE,
                      0, 0, "QCD string", 90);
   pdgDB->AddParticle("rho_diff0", "rho_diff0", 0, kTRUE,
                      0, 0, "QCD diffr. state", 9900110);
   pdgDB->AddParticle("pi_diffr+", "pi_diffr+", 0, kTRUE,
                      0, 1, "QCD diffr. state", 9900210);
   pdgDB->AddParticle("omega_di", "omega_di", 0, kTRUE,
                      0, 0, "QCD diffr. state", 9900220);
   pdgDB->AddParticle("phi_diff","phi_diff", 0, kTRUE,
                      0, 0, "QCD diffr. state", 9900330);
   pdgDB->AddParticle("J/psi_di", "J/psi_di", 0, kTRUE,
                      0, 0, "QCD diffr. state", 9900440);
   pdgDB->AddParticle("n_diffr0","n_diffr0",0,kTRUE,
                      0, 0, "QCD diffr. state", 9902110);
   pdgDB->AddParticle("p_diffr+","p_diffr+", 0, kTRUE,
                      0, 1, "QCD diffr. state", 9902210);
}
<|endoftext|>


<commit_before>/*
**
** Author(s):
**  - Cedric GESTES <gestes@aldebaran-robotics.com>
**
** Copyright (C) 2013 Aldebaran Robotics
*/
#include "pysession.hpp"
#include <qimessaging/session.hpp>
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <qitype/dynamicobjectbuilder.hpp>
#include "pyfuture.hpp"
#include "gil.hpp"
#include "pysignal.hpp"

qiLogCategory("qi.py");

namespace qi { namespace py {

qi::AnyReference triggerBouncer(qi::SignalBase *sig, const std::vector<qi::AnyReference>& args) {
  sig->trigger(args);
  return qi::AnyReference();
}

    class PySession {
    public:
      PySession()
        : _ses(new qi::Session)
        , nSigConnected(0)
        , nSigDisconnected(0)
        , connected(makePySignal("[m]"))
        , disconnected(makePySignal("[m]"))
      {
        // Get SignalBase from our PySignals
        qi::SignalBase *conn = getSignal(connected);
        qi::SignalBase *disconn = getSignal(disconnected);
        // Connect our PySignals with qi::Session signals, have to use a dynamic generic function to trigger
        nSigConnected = _ses->connected.connect(qi::AnyFunction::fromDynamicFunction(boost::bind(&triggerBouncer, conn, _1)));
        nSigDisconnected = _ses->disconnected.connect(qi::AnyFunction::fromDynamicFunction(boost::bind(&triggerBouncer, disconn, _1)));
      }

      ~PySession() {
        _ses->connected.disconnect(nSigConnected);
        _ses->disconnected.disconnect(nSigDisconnected);
      }

      //return a future, or None (and throw in case of error)
      boost::python::object connect(const std::string &url, bool _async=false) {
        GILScopedUnlock _unlock;
        return toPyFutureAsync(_ses->connect(url), _async);
      }

      //return a future, or None (and throw in case of error)
      boost::python::object close(bool _async=false) {
        GILScopedUnlock _unlock;
        return toPyFutureAsync(_ses->close(), _async);
      }

      boost::python::object service(const std::string &name, bool _async=false) {
        GILScopedUnlock _unlock;
        return toPyFutureAsync(_ses->service(name), _async);
      }

      boost::python::object services(bool _async=false) {
        GILScopedUnlock _unlock;
        return toPyFutureAsync(_ses->services(), _async);
      }

      boost::python::object registerService(const std::string &name, boost::python::object obj, bool _async=false) {
        GILScopedUnlock _unlock;
        return toPyFutureAsync(_ses->registerService(name, qi::AnyReference(obj).toObject()), _async);
      }

    private:
      boost::shared_ptr<qi::Session> _ses;
      int nSigConnected;
      int nSigDisconnected;

    public:
      boost::python::object connected;
      boost::python::object disconnected;
    };

    void export_pysession() {
      boost::python::class_<PySession>("Session")
          .def("connect", &PySession::connect, (boost::python::arg("url"), boost::python::arg("_async") = false))
          .def("close", &PySession::close, (boost::python::arg("_async") = false))
          .def("service", &PySession::service, (boost::python::arg("service"), boost::python::arg("_async") = false))
          .def("services", &PySession::services, (boost::python::arg("_async") = false))
          .def("registerService", &PySession::registerService, (boost::python::arg("name"), boost::python::arg("object"), boost::python::arg("_async") = false))
          .def_readonly("connected", &PySession::connected)
          .def_readonly("disconnected", &PySession::disconnected)
          ;
    }

  }
}
<commit_msg>python: session add unregisterService<commit_after>/*
**
** Author(s):
**  - Cedric GESTES <gestes@aldebaran-robotics.com>
**
** Copyright (C) 2013 Aldebaran Robotics
*/
#include "pysession.hpp"
#include <qimessaging/session.hpp>
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <qitype/dynamicobjectbuilder.hpp>
#include "pyfuture.hpp"
#include "gil.hpp"
#include "pysignal.hpp"

qiLogCategory("qi.py");

namespace qi { namespace py {

qi::AnyReference triggerBouncer(qi::SignalBase *sig, const std::vector<qi::AnyReference>& args) {
  sig->trigger(args);
  return qi::AnyReference();
}

    class PySession {
    public:
      PySession()
        : _ses(new qi::Session)
        , nSigConnected(0)
        , nSigDisconnected(0)
        , connected(makePySignal("[m]"))
        , disconnected(makePySignal("[m]"))
      {
        // Get SignalBase from our PySignals
        qi::SignalBase *conn = getSignal(connected);
        qi::SignalBase *disconn = getSignal(disconnected);
        // Connect our PySignals with qi::Session signals, have to use a dynamic generic function to trigger
        nSigConnected = _ses->connected.connect(qi::AnyFunction::fromDynamicFunction(boost::bind(&triggerBouncer, conn, _1)));
        nSigDisconnected = _ses->disconnected.connect(qi::AnyFunction::fromDynamicFunction(boost::bind(&triggerBouncer, disconn, _1)));
      }

      ~PySession() {
        _ses->connected.disconnect(nSigConnected);
        _ses->disconnected.disconnect(nSigDisconnected);
      }

      //return a future, or None (and throw in case of error)
      boost::python::object connect(const std::string &url, bool _async=false) {
        GILScopedUnlock _unlock;
        return toPyFutureAsync(_ses->connect(url), _async);
      }

      //return a future, or None (and throw in case of error)
      boost::python::object close(bool _async=false) {
        GILScopedUnlock _unlock;
        return toPyFutureAsync(_ses->close(), _async);
      }

      boost::python::object service(const std::string &name, bool _async=false) {
        GILScopedUnlock _unlock;
        return toPyFutureAsync(_ses->service(name), _async);
      }

      boost::python::object services(bool _async=false) {
        GILScopedUnlock _unlock;
        return toPyFutureAsync(_ses->services(), _async);
      }

      boost::python::object registerService(const std::string &name, boost::python::object obj, bool _async=false) {
        GILScopedUnlock _unlock;
        return toPyFutureAsync(_ses->registerService(name, qi::AnyReference(obj).toObject()), _async);
      }

      boost::python::object unregisterService(const unsigned int &id, bool _async=false) {
        GILScopedUnlock _unlock;
        return toPyFutureAsync(_ses->unregisterService(id), _async);
      }

    private:
      boost::shared_ptr<qi::Session> _ses;
      int nSigConnected;
      int nSigDisconnected;

    public:
      boost::python::object connected;
      boost::python::object disconnected;
    };

    void export_pysession() {
      boost::python::class_<PySession>("Session")
          .def("connect", &PySession::connect, (boost::python::arg("url"), boost::python::arg("_async") = false))
          .def("close", &PySession::close, (boost::python::arg("_async") = false))
          .def("service", &PySession::service, (boost::python::arg("service"), boost::python::arg("_async") = false))
          .def("services", &PySession::services, (boost::python::arg("_async") = false))
          .def("registerService", &PySession::registerService, (boost::python::arg("name"), boost::python::arg("object"), boost::python::arg("_async") = false))
          .def("unregisterService", &PySession::unregisterService, (boost::python::arg("id"), boost::python::arg("_async") = false))
          .def_readonly("connected", &PySession::connected)
          .def_readonly("disconnected", &PySession::disconnected)
          ;
    }

  }
}
<|endoftext|>


<commit_before>// Copyright 2017 Intermodalics All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>

#include <jni.h>

#include <glog/logging.h>
#include <image_transport/publisher_plugin.h>
#include <nodelet/loader.h>
#include <pluginlib/class_loader.h>
#include <ros/ros.h>

#include "tango_helper.h"

#ifdef __cplusplus
extern "C" {
#endif

JNIEXPORT jboolean JNICALL
Java_eu_intermodalics_nodelet_1manager_TangoInitializationHelper_setBinderTangoService(
    JNIEnv* env, jclass /*class*/, jobject binder) {
  return tango_helper::SetBinder(env, binder);
}

JNIEXPORT jboolean JNICALL
Java_eu_intermodalics_nodelet_1manager_TangoInitializationHelper_isTangoVersionOk(
    JNIEnv* env, jclass /*class*/, jobject activity) {
  return tango_helper::IsTangoVersionOk(env, activity);
}

JNIEXPORT jint JNICALL
Java_eu_intermodalics_nodelet_1manager_NodeletManager_execute(
        JNIEnv* env, jobject /*obj*/, jstring master_uri_value,
        jstring host_ip_value, jstring node_name_value,
        /* unused */ jobjectArray remapping_objects_value) {
    const char* master_uri = env->GetStringUTFChars(master_uri_value, NULL);
    const char* host_ip = env->GetStringUTFChars(host_ip_value, NULL);
    const char* node_name = env->GetStringUTFChars(node_name_value, NULL);
    int argc = 3;
    std::string master_uri_string("__master:=" + std::string(master_uri));
    std::string host_ip_string("__ip:=" + std::string(host_ip));
    const std::string node_name_string(node_name);
    char* argv[] = {"/", &master_uri_string[0], &host_ip_string[0]};

    env->ReleaseStringUTFChars(master_uri_value, master_uri);
    env->ReleaseStringUTFChars(host_ip_value, host_ip);
    env->ReleaseStringUTFChars(node_name_value, node_name);

    ros::init(argc, argv, node_name_string.c_str());
    nodelet::Loader loader;
    std::map<std::string, std::string> remappings;
    std::vector<std::string> nodelet_argv;

    LOG(INFO) << "Start loading nodelets.";
    const bool result = loader.load("/tango", "tango_ros_native/TangoRosNode", remappings, nodelet_argv);
    if (!result) {
        LOG(ERROR) << "Problem loading Tango ROS nodelet!";
        return 1;
    }
    LOG(INFO) << "Finished loading nodelets.";

    // Check that all necessary plugins are available.
    pluginlib::ClassLoader<image_transport::PublisherPlugin> image_transport_pub_loader("image_transport", "image_transport::PublisherPlugin");
    if (!image_transport_pub_loader.isClassAvailable("image_transport/raw_pub")) {
      LOG(ERROR) << "Plugin image_transport/raw_pub is not available.";
      return 1;
    }
    if (!image_transport_pub_loader.isClassAvailable("image_transport/compressed_pub")) {
      LOG(ERROR) << "Plugin image_transport/compressed_pub is not available.";
      return 1;
    }

    ros::AsyncSpinner spinner(4);
    spinner.start();
    ros::waitForShutdown();
    return 0;
}

JNIEXPORT jint JNICALL
Java_eu_intermodalics_nodelet_1manager_NodeletManager_shutdown(
        JNIEnv* /*env*/, jobject /*obj*/) {
    return 0;
}

#ifdef __cplusplus
}
#endif
<commit_msg>Fix error when renaming to TangoNodeletManager<commit_after>// Copyright 2017 Intermodalics All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>

#include <jni.h>

#include <glog/logging.h>
#include <image_transport/publisher_plugin.h>
#include <nodelet/loader.h>
#include <pluginlib/class_loader.h>
#include <ros/ros.h>

#include "tango_helper.h"

#ifdef __cplusplus
extern "C" {
#endif

JNIEXPORT jboolean JNICALL
Java_eu_intermodalics_nodelet_1manager_TangoInitializationHelper_setBinderTangoService(
    JNIEnv* env, jclass /*class*/, jobject binder) {
  return tango_helper::SetBinder(env, binder);
}

JNIEXPORT jboolean JNICALL
Java_eu_intermodalics_nodelet_1manager_TangoInitializationHelper_isTangoVersionOk(
    JNIEnv* env, jclass /*class*/, jobject activity) {
  return tango_helper::IsTangoVersionOk(env, activity);
}

JNIEXPORT jint JNICALL
Java_eu_intermodalics_nodelet_1manager_TangoNodeletManager_execute(
        JNIEnv* env, jobject /*obj*/, jstring master_uri_value,
        jstring host_ip_value, jstring node_name_value,
        /* unused */ jobjectArray remapping_objects_value) {
    const char* master_uri = env->GetStringUTFChars(master_uri_value, NULL);
    const char* host_ip = env->GetStringUTFChars(host_ip_value, NULL);
    const char* node_name = env->GetStringUTFChars(node_name_value, NULL);
    int argc = 3;
    std::string master_uri_string("__master:=" + std::string(master_uri));
    std::string host_ip_string("__ip:=" + std::string(host_ip));
    const std::string node_name_string(node_name);
    char* argv[] = {"/", &master_uri_string[0], &host_ip_string[0]};

    env->ReleaseStringUTFChars(master_uri_value, master_uri);
    env->ReleaseStringUTFChars(host_ip_value, host_ip);
    env->ReleaseStringUTFChars(node_name_value, node_name);

    ros::init(argc, argv, node_name_string.c_str());
    nodelet::Loader loader;
    std::map<std::string, std::string> remappings;
    std::vector<std::string> nodelet_argv;

    LOG(INFO) << "Start loading nodelets.";
    const bool result = loader.load("/tango", "tango_ros_native/TangoRosNode", remappings, nodelet_argv);
    if (!result) {
        LOG(ERROR) << "Problem loading Tango ROS nodelet!";
        return 1;
    }
    LOG(INFO) << "Finished loading nodelets.";

    // Check that all necessary plugins are available.
    pluginlib::ClassLoader<image_transport::PublisherPlugin> image_transport_pub_loader("image_transport", "image_transport::PublisherPlugin");
    if (!image_transport_pub_loader.isClassAvailable("image_transport/raw_pub")) {
      LOG(ERROR) << "Plugin image_transport/raw_pub is not available.";
      return 1;
    }
    if (!image_transport_pub_loader.isClassAvailable("image_transport/compressed_pub")) {
      LOG(ERROR) << "Plugin image_transport/compressed_pub is not available.";
      return 1;
    }

    ros::AsyncSpinner spinner(4);
    spinner.start();
    ros::waitForShutdown();
    return 0;
}

JNIEXPORT jint JNICALL
Java_eu_intermodalics_nodelet_1manager_TangoNodeletManager_shutdown(
        JNIEnv* /*env*/, jobject /*obj*/) {
    return 0;
}

#ifdef __cplusplus
}
#endif
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   ORFEO Toolbox
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
  See OTBCopyright.txt for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#include "itkExceptionObject.h"

#include "itkPolyLineParametricPath.h"
#include "otbImage.h"
#include "otbVectorImage.h"
#include "otbRoadExtractionFilter.h"
#include "otbDrawPathListFilter.h"
#include "otbImageFileReader.h"
#include "otbImageFileWriter.h"
#include "otbPolyLineParametricPathWithValue.h"
#include "otbMath.h"

int otbRoadExtractionFilter(int argc, char * argv[])
{
  try
    {
      const unsigned int Dimension = 2;
      typedef otb::VectorImage<double,Dimension> InputImageType;
      typedef otb::Image<double,Dimension> OutputImageType;
      typedef otb::PolyLineParametricPathWithValue<double,Dimension> PathType;

      typedef otb::ImageFileReader<InputImageType> ReaderType;
      typedef otb::ImageFileWriter<OutputImageType> WriterType;
      typedef otb::RoadExtractionFilter<InputImageType,PathType> RoadExtractionFilterType;
      typedef RoadExtractionFilterType::OutputPathListType OutputPathListType;
      typedef RoadExtractionFilterType::InputPixelType InputPixelType;
      typedef otb::DrawPathListFilter<OutputImageType, PathType, OutputImageType> DrawPathFilterType;

	  //Parameters
      const char * inputFileName(argv[1]);
      const char * outputFileName(argv[2]);
      InputPixelType ReferencePixel;
      ReferencePixel.Reserve(4);
      ReferencePixel.SetElement(0,::atof(argv[3]));
      ReferencePixel.SetElement(1,::atof(argv[4]));
      ReferencePixel.SetElement(2,::atof(argv[5]));
      ReferencePixel.SetElement(3,::atof(argv[6]));
      const double Alpha = ::atof(argv[7]);
      const double AmplitudeThreshold = ::atof(argv[8]);
      const double Tolerance = ::atof(argv[9]);
      const double MaxAngle = ((M_PI*::atof(argv[10]))/180.);
      const double FirstMeanDistanceThreshold  = ::atof(argv[11]);
      const double SecondMeanDistanceThreshold  = ::atof(argv[12]);
      const double LinkAngularThreshold = ((M_PI*::atof(argv[13]))/180.);
      const double LinkDistanceThreshold  = ::atof(argv[14]);

      // Instantiating object
      ReaderType::Pointer reader = ReaderType::New();
      RoadExtractionFilterType::Pointer roadExtraction = RoadExtractionFilterType::New();
      DrawPathFilterType::Pointer draw = DrawPathFilterType::New();
      WriterType::Pointer writer = WriterType::New();

      //Initialisation parameters
      reader->SetFileName(inputFileName);

      roadExtraction->SetInput(reader->GetOutput());
      roadExtraction->SetReferencePixel(ReferencePixel);
      roadExtraction->SetAlpha(Alpha);
      roadExtraction->SetAmplitudeThreshold(AmplitudeThreshold);
      roadExtraction->SetTolerance(Tolerance);
      roadExtraction->SetMaxAngle(MaxAngle);
      roadExtraction->SetFirstMeanDistanceThreshold(FirstMeanDistanceThreshold);
      roadExtraction->SetSecondMeanDistanceThreshold(SecondMeanDistanceThreshold);
      roadExtraction->SetAngularThreshold(LinkAngularThreshold);
      roadExtraction->SetDistanceThreshold(LinkDistanceThreshold);

      reader->GenerateOutputInformation();
      OutputImageType::Pointer image = OutputImageType::New();
      image->SetRegions(reader->GetOutput()->GetLargestPossibleRegion());
      image->Allocate();
      image->FillBuffer(0);

      OutputPathListType * listPath = roadExtraction->GetOutput();

      draw->SetInput(image);
      //Use internal value of path
      draw->UseInternalPathValueOn();
      draw->SetInputPath(roadExtraction->GetOutput());

      writer->SetFileName(outputFileName);
      writer->SetInput(draw->GetOutput());
      writer->Update();
    }

  catch( itk::ExceptionObject & err )
    {
    std::cout << "Exception itk::ExceptionObject thrown !" << std::endl;
    std::cout << err << std::endl;
    return EXIT_FAILURE;
    }
  catch( std::bad_alloc & err )
    {
    std::cout << "Exception bad_alloc : "<<(char*)err.what()<< std::endl;
    return EXIT_FAILURE;
    }
  catch( ... )
    {
    std::cout << "Unknown exception thrown !" << std::endl;
    return EXIT_FAILURE;
    }
  return EXIT_SUCCESS;
}
<commit_msg>Variable inutilisée<commit_after>/*=========================================================================

  Program:   ORFEO Toolbox
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
  See OTBCopyright.txt for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#include "itkExceptionObject.h"

#include "itkPolyLineParametricPath.h"
#include "otbImage.h"
#include "otbVectorImage.h"
#include "otbRoadExtractionFilter.h"
#include "otbDrawPathListFilter.h"
#include "otbImageFileReader.h"
#include "otbImageFileWriter.h"
#include "otbPolyLineParametricPathWithValue.h"
#include "otbMath.h"

int otbRoadExtractionFilter(int argc, char * argv[])
{
  try
    {
      const unsigned int Dimension = 2;
      typedef otb::VectorImage<double,Dimension> InputImageType;
      typedef otb::Image<double,Dimension> OutputImageType;
      typedef otb::PolyLineParametricPathWithValue<double,Dimension> PathType;

      typedef otb::ImageFileReader<InputImageType> ReaderType;
      typedef otb::ImageFileWriter<OutputImageType> WriterType;
      typedef otb::RoadExtractionFilter<InputImageType,PathType> RoadExtractionFilterType;
      typedef RoadExtractionFilterType::OutputPathListType OutputPathListType;
      typedef RoadExtractionFilterType::InputPixelType InputPixelType;
      typedef otb::DrawPathListFilter<OutputImageType, PathType, OutputImageType> DrawPathFilterType;

	  //Parameters
      const char * inputFileName(argv[1]);
      const char * outputFileName(argv[2]);
      InputPixelType ReferencePixel;
      ReferencePixel.Reserve(4);
      ReferencePixel.SetElement(0,::atof(argv[3]));
      ReferencePixel.SetElement(1,::atof(argv[4]));
      ReferencePixel.SetElement(2,::atof(argv[5]));
      ReferencePixel.SetElement(3,::atof(argv[6]));
      const double Alpha = ::atof(argv[7]);
      const double AmplitudeThreshold = ::atof(argv[8]);
      const double Tolerance = ::atof(argv[9]);
      const double MaxAngle = ((M_PI*::atof(argv[10]))/180.);
      const double FirstMeanDistanceThreshold  = ::atof(argv[11]);
      const double SecondMeanDistanceThreshold  = ::atof(argv[12]);
      const double LinkAngularThreshold = ((M_PI*::atof(argv[13]))/180.);
      const double LinkDistanceThreshold  = ::atof(argv[14]);

      // Instantiating object
      ReaderType::Pointer reader = ReaderType::New();
      RoadExtractionFilterType::Pointer roadExtraction = RoadExtractionFilterType::New();
      DrawPathFilterType::Pointer draw = DrawPathFilterType::New();
      WriterType::Pointer writer = WriterType::New();

      //Initialisation parameters
      reader->SetFileName(inputFileName);

      roadExtraction->SetInput(reader->GetOutput());
      roadExtraction->SetReferencePixel(ReferencePixel);
      roadExtraction->SetAlpha(Alpha);
      roadExtraction->SetAmplitudeThreshold(AmplitudeThreshold);
      roadExtraction->SetTolerance(Tolerance);
      roadExtraction->SetMaxAngle(MaxAngle);
      roadExtraction->SetFirstMeanDistanceThreshold(FirstMeanDistanceThreshold);
      roadExtraction->SetSecondMeanDistanceThreshold(SecondMeanDistanceThreshold);
      roadExtraction->SetAngularThreshold(LinkAngularThreshold);
      roadExtraction->SetDistanceThreshold(LinkDistanceThreshold);

      reader->GenerateOutputInformation();
      OutputImageType::Pointer image = OutputImageType::New();
      image->SetRegions(reader->GetOutput()->GetLargestPossibleRegion());
      image->Allocate();
      image->FillBuffer(0);

      draw->SetInput(image);
      //Use internal value of path
      draw->UseInternalPathValueOn();
      draw->SetInputPath(roadExtraction->GetOutput());

      writer->SetFileName(outputFileName);
      writer->SetInput(draw->GetOutput());
      writer->Update();
    }

  catch( itk::ExceptionObject & err )
    {
    std::cout << "Exception itk::ExceptionObject thrown !" << std::endl;
    std::cout << err << std::endl;
    return EXIT_FAILURE;
    }
  catch( std::bad_alloc & err )
    {
    std::cout << "Exception bad_alloc : "<<(char*)err.what()<< std::endl;
    return EXIT_FAILURE;
    }
  catch( ... )
    {
    std::cout << "Unknown exception thrown !" << std::endl;
    return EXIT_FAILURE;
    }
  return EXIT_SUCCESS;
}
<|endoftext|>


<commit_before>// (C) 2014 Arek Olek

#include <algorithm>
#include <functional>
#include <iostream>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_matrix.hpp>

#include <omp.h>

#include "bfs.hpp"
#include "dfs.hpp"
#include "fifodfs.hpp"
#include "fivethree.hpp"
#include "lost.hpp"
#include "rdfs.hpp"
#include "random.hpp"
#include "ilst.hpp"
#include "greedy.hpp"

#include "test_suite.hpp"

#include "debug.hpp"
#include "graph.hpp"
#include "options.hpp"
#include "range.hpp"
#include "timing.hpp"

boost::adjacency_matrix<boost::undirectedS> typedef amatrix;
boost::adjacency_list<boost::hash_setS, boost::vecS, boost::undirectedS> typedef ahash;
boost::adjacency_list<boost::listS, boost::vecS, boost::undirectedS> typedef alist;
boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS> typedef aset;
boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> typedef avec;

template<class Graph, class Tree>
std::function<Tree(Graph&)> make_construction(std::string name) {
  if (name == "bfs")
    return bfs_tree<Graph, Tree> ;
  if (name == "dfs")
    return dfs_tree<Graph, Tree> ;
  if (name == "rdfs")
    return rdfs_tree<Graph, Tree> ;
  if (name == "rdfs50")
    return rdfs_best_tree<Graph, Tree> ;
  if (name == "fifo")
    return fifo_dfs_tree<Graph, Tree> ;
  if (name == "random")
    return random_tree<Graph, Tree> ;
  if (name == "wilson")
    return wilson_tree<Graph, Tree> ;
  if (name == "greedy")
    return greedy_tree<Graph, Tree> ;
  if (name == "ilst")
    return ilst<Graph, Tree> ;
  if (name == "5/3")
    return five_three_tree<Graph, Tree> ;
  throw std::invalid_argument("Unknown construction method: " + name);
}

template<class Graph>
std::function<unsigned(Graph&)> make_upper(std::string name) {
  if (name == "5/3") {
    return [](const Graph& G) {
      return 5*(num_vertices(G)-3)/6;
    };
  }
  return upper_bound<Graph> ;
}

template <class Graph, class Tree, class Suite, class Strings>
void run(Suite& suite, Strings const & constructions, Strings const & improvements, bool scratch) {
  timing total(CLOCK_REALTIME);
  total.start();
  std::cerr << "Suite size: " << suite.size() << std::endl;
  #pragma omp parallel
  {
    auto id = omp_get_thread_num();
    double elapsed_c, elapsed_i;
    timing timer;
    #pragma omp for schedule(dynamic) nowait
    for(uint i = 0; i < suite.size(); ++i) {
      std::stringstream buffer;
      auto trial = suite.get(i);
      auto G = std::get<0>(trial);
      if(!is_connected(G)) continue;

      for(auto cname : constructions) {
        auto construct = make_construction<Graph, Tree>(cname);
        auto upper = make_upper<Graph>(cname);
        timer.start();
        auto T = construct(G);
        elapsed_c = timer.stop();

        assert(num_edges(T) == num_vertices(T)-1);

        const auto tree(T);

        for(auto iname : improvements) {
          if(scratch) T = tree;

          auto improve = make_improvement<Graph, Tree>(iname);
          timer.start();
          auto rules = improve(G, T);
          elapsed_i = timer.stop();

//  1    2       3      4       5      6         7      8      9             10           11        12     13     14     15
//  run  thread  model  degree  param  vertices  edges  upper  construction  improvement  internal  ctime  itime  steps  rules
          buffer
            << i << '\t'
            << id << '\t'
            << suite.type() << '\t'
            << std::get<1>(trial) << '\t'
            << std::get<2>(trial) << '\t'
            << num_vertices(G) << '\t'
            << num_edges(G) << '\t'
            << upper(G) << '\t'
            << cname << '\t'
            << iname << '\t'
            << num_internal(T) << '\t'
            << elapsed_c << '\t'
            << elapsed_i << '\t'
            << sum(rules) << '\t'
            << join(rules, "-") << '\t'
            << std::endl;
            ;
          //show("graph" + std::to_string(i) + ".dot", G, T);

          if(!scratch) elapsed_c += elapsed_i;
        }
      }
      #pragma omp critical
      std::cout << buffer.str() << std::flush;

      if(id == 0) {
        std::cerr << "\t" << 100*i/suite.size() << "%\t";
        std::cerr << readable(total.stop()) << " elapsed    \r";
      }
    }
  }
  std::cerr << "\t100%\n";
}

template <class Graph, class Tree, class Sizes, class Params, class Strings>
void run(std::string t, unsigned z, Sizes sizes, Params params,
         Strings const & constructions, Strings const & improvements, bool scratch) {
  if (t.find(".xml") != std::string::npos) {
    real_suite<Graph> suite(t, z);
    run<Graph, Tree>(suite, constructions, improvements, scratch);
  } else if (t.find('.') != std::string::npos) {
    file_suite<Graph> suite(t);
    run<Graph, Tree>(suite, constructions, improvements, scratch);
  } else {
    test_suite<Graph> suite(t, z, sizes, params);
    run<Graph, Tree>(suite, constructions, improvements, scratch);
  }
}

int main(int argc, char** argv){
  using std::string;
  using std::vector;

  std::ios_base::sync_with_stdio(0);

  options opt(argc, argv);
  auto z = opt.get<int>("-z", 1);
  auto sizes = opt.getList<unsigned>("-n", {100});
  auto tests = opt.getList<string>("-t", {"gnp+mst"});
  auto degrees = opt.getList<double>("-d", { 3. });
  auto constructions = opt.getList<string>("-c", {"bfs", "dfs", "rdfs", "fifo", "rdfs50", "ilst", "random"});
  auto improvements = opt.getList<string>("-i", {"none", "prieto", "lost-light", "lost", "lost-ex"});
  auto scratch = opt.has("--scratch");

  for(auto t : tests)
    run<alist, alist>(t, z, sizes, degrees, constructions, improvements, scratch);

  return 0;
}
<commit_msg>use vector list for tree cause its faster<commit_after>// (C) 2014 Arek Olek

#include <algorithm>
#include <functional>
#include <iostream>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_matrix.hpp>

#include <omp.h>

#include "bfs.hpp"
#include "dfs.hpp"
#include "fifodfs.hpp"
#include "fivethree.hpp"
#include "lost.hpp"
#include "rdfs.hpp"
#include "random.hpp"
#include "ilst.hpp"
#include "greedy.hpp"

#include "test_suite.hpp"

#include "debug.hpp"
#include "graph.hpp"
#include "options.hpp"
#include "range.hpp"
#include "timing.hpp"

boost::adjacency_matrix<boost::undirectedS> typedef amatrix;
boost::adjacency_list<boost::hash_setS, boost::vecS, boost::undirectedS> typedef ahash;
boost::adjacency_list<boost::listS, boost::vecS, boost::undirectedS> typedef alist;
boost::adjacency_list<boost::slistS, boost::vecS, boost::undirectedS> typedef aslist;
boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS> typedef aset;
boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> typedef avec;

template<class Graph, class Tree>
std::function<Tree(Graph&)> make_construction(std::string name) {
  if (name == "bfs")
    return bfs_tree<Graph, Tree> ;
  if (name == "dfs")
    return dfs_tree<Graph, Tree> ;
  if (name == "rdfs")
    return rdfs_tree<Graph, Tree> ;
  if (name == "rdfs50")
    return rdfs_best_tree<Graph, Tree> ;
  if (name == "fifo")
    return fifo_dfs_tree<Graph, Tree> ;
  if (name == "random")
    return random_tree<Graph, Tree> ;
  if (name == "wilson")
    return wilson_tree<Graph, Tree> ;
  if (name == "greedy")
    return greedy_tree<Graph, Tree> ;
  if (name == "ilst")
    return ilst<Graph, Tree> ;
  if (name == "5/3")
    return five_three_tree<Graph, Tree> ;
  throw std::invalid_argument("Unknown construction method: " + name);
}

template<class Graph>
std::function<unsigned(Graph&)> make_upper(std::string name) {
  if (name == "5/3") {
    return [](const Graph& G) {
      return 5*(num_vertices(G)-3)/6;
    };
  }
  return upper_bound<Graph> ;
}

template <class Graph, class Tree, class Suite, class Strings>
void run(Suite& suite, Strings const & constructions, Strings const & improvements, bool scratch) {
  timing total(CLOCK_REALTIME);
  total.start();
  std::cerr << "Suite size: " << suite.size() << std::endl;
  #pragma omp parallel
  {
    auto id = omp_get_thread_num();
    double elapsed_c, elapsed_i;
    timing timer;
    #pragma omp for schedule(dynamic) nowait
    for(uint i = 0; i < suite.size(); ++i) {
      std::stringstream buffer;
      auto trial = suite.get(i);
      auto G = std::get<0>(trial);
      if(!is_connected(G)) continue;

      for(auto cname : constructions) {
        auto construct = make_construction<Graph, Tree>(cname);
        auto upper = make_upper<Graph>(cname);
        timer.start();
        auto T = construct(G);
        elapsed_c = timer.stop();

        assert(num_edges(T) == num_vertices(T)-1);

        const auto tree(T);

        for(auto iname : improvements) {
          if(scratch) T = tree;

          auto improve = make_improvement<Graph, Tree>(iname);
          timer.start();
          auto rules = improve(G, T);
          elapsed_i = timer.stop();

//  1    2       3      4       5      6         7      8      9             10           11        12     13     14     15
//  run  thread  model  degree  param  vertices  edges  upper  construction  improvement  internal  ctime  itime  steps  rules
          buffer
            << i << '\t'
            << id << '\t'
            << suite.type() << '\t'
            << std::get<1>(trial) << '\t'
            << std::get<2>(trial) << '\t'
            << num_vertices(G) << '\t'
            << num_edges(G) << '\t'
            << upper(G) << '\t'
            << cname << '\t'
            << iname << '\t'
            << num_internal(T) << '\t'
            << elapsed_c << '\t'
            << elapsed_i << '\t'
            << sum(rules) << '\t'
            << join(rules, "-") << '\t'
            << std::endl;
            ;
          //show("graph" + std::to_string(i) + ".dot", G, T);

          if(!scratch) elapsed_c += elapsed_i;
        }
      }
      #pragma omp critical
      std::cout << buffer.str() << std::flush;

      if(id == 0) {
        std::cerr << "\t" << 100*i/suite.size() << "%\t";
        std::cerr << readable(total.stop()) << " elapsed    \r";
      }
    }
  }
  std::cerr << "\t100%\n";
}

template <class Graph, class Tree, class Sizes, class Params, class Strings>
void run(std::string t, unsigned z, Sizes sizes, Params params,
         Strings const & constructions, Strings const & improvements, bool scratch) {
  if (t.find(".xml") != std::string::npos) {
    real_suite<Graph> suite(t, z);
    run<Graph, Tree>(suite, constructions, improvements, scratch);
  } else if (t.find('.') != std::string::npos) {
    file_suite<Graph> suite(t);
    run<Graph, Tree>(suite, constructions, improvements, scratch);
  } else {
    test_suite<Graph> suite(t, z, sizes, params);
    run<Graph, Tree>(suite, constructions, improvements, scratch);
  }
}

int main(int argc, char** argv){
  using std::string;
  using std::vector;

  std::ios_base::sync_with_stdio(0);

  options opt(argc, argv);
  auto z = opt.get<int>("-z", 1);
  auto sizes = opt.getList<unsigned>("-n", {100});
  auto tests = opt.getList<string>("-t", {"gnp+mst"});
  auto degrees = opt.getList<double>("-d", { 3. });
  auto constructions = opt.getList<string>("-c", {"bfs", "dfs", "rdfs", "fifo", "rdfs50", "ilst", "random"});
  auto improvements = opt.getList<string>("-i", {"none", "prieto", "lost-light", "lost", "lost-ex"});
  auto scratch = opt.has("--scratch");

  for(auto t : tests) {
    run<alist, avec>(t, z, sizes, degrees, constructions, improvements, scratch);
  }

  return 0;
}
<|endoftext|>


<commit_before>#include "Engine.h"
#include "Game.h"
#include "ObjectDummy.h"
#include "BodyRigid.h"
#include "BodyDummy.h"

#include "Physics.h"
#include "ShapeCapsule.h"
#include "ActorBase.h"
#include "Visualizer.h"
#include "sys/SysControl.h"

#ifdef MEMORY_INFO
#define new new(__FILE__, __LINE__)
#endif // MEMORY_INFO

using namespace MathLib;

/*
*/
#define ACTOR_BASE_IFPS             (1.0f / 60.0f)
#define ACTOR_BASE_CLAMP            89.9f
#define ACTOR_BASE_COLLISIONS       4

/*
*/

CActorBase::CActorBase()
{
	m_vUp = vec3(0.0f, 0.0f, 1.0f);
	m_pObject = new CObjectDummy();
	m_pDummy = new CBodyDummy();
	m_pShape = new CShapeCapsule(1.0f, 1.0f);

	m_nFlush = 0;
	m_vPosition = Vec3_zero;
	m_vVelocity = Vec3_zero;
	m_fPhiAngle = 0.0f;			//б		άƽϵУֱYķX֮ļн
	m_fThetaAngle = 0.0f;			//λǣ뵱ǰ˳ʱ߹ĽǶ

	for (int i = 0; i < NUM_STATES; i++)
	{
		m_pStates[i] = 0;
		m_pTimes[i] = 0.0f;
	}

	m_pDummy->SetEnabled(1);
	m_pObject->SetBody(NULL);
	m_pObject->SetBody(m_pDummy);
	m_pShape->SetBody(NULL);
	m_pShape->SetBody(m_pDummy);

	m_pObject->SetWorldTransform(Get_Body_Transform());
	m_pShape->SetRestitution(0.0f);
	m_pShape->SetCollisionMask(2);

	SetEnabled(1);
	SetViewDirection(vec3(0.0f,1.0f,0.0f));
	SetCollision(1);
	SetCollisionRadius(0.3f);
	SetCollisionHeight(1.0f);
	SetFriction(2.0f);
	SetMinVelocity(2.0f);
	SetMaxVelocity(4.0f);
	SetAcceleration(8.0f);
	SetDamping(8.0f);
	SetJumping(1.5f);

	SetGround(0);
	SetCeiling(0);

}
CActorBase::~CActorBase()
{
	m_pDummy->SetObject(NULL);
	delete m_pObject;
	delete m_pDummy;
}

void CActorBase::SetEnabled(int enable)
{
	m_nEnable = enable;
	m_pDummy->SetEnabled(m_nEnable);
}
int CActorBase::IsEnabled() const
{
	return m_nEnable;
}
void CActorBase::Update(float ifps)
{
	if (!m_nEnable)
	{
		return;
	}
	// impulse
	vec3 impulse = vec3_zero;

	// ortho basis
	vec3 tangent, binormal;
	OrthoBasis(m_vUp, tangent, binormal);
	// current basis
	vec3 x = quat(m_vUp, -m_fPhiAngle) * binormal;
	vec3 y = Normalize(Cross(m_vUp, x));
	vec3 z = Normalize(Cross(x, y));
	// handle states
	Update_States(1, ifps);

	// old velocity
	float x_velocity = Dot(x, m_vVelocity);
	float y_velocity = Dot(y, m_vVelocity);
	float z_velocity = Dot(z, m_vVelocity);

	// movement
	if (m_pStates[STATE_FORWARD]) impulse += x;
	if (m_pStates[STATE_BACKWARD]) impulse -= x;
	if (m_pStates[STATE_MOVE_LEFT]) impulse += y;
	if (m_pStates[STATE_MOVE_RIGHT]) impulse -= y;
	impulse.normalize();
	// velocity
	if (m_pStates[STATE_RUN]) impulse *= m_fMaxVelocity;
	else impulse *= m_fMinVelocity;

	// jump
	if (m_pStates[STATE_JUMP] == STATE_BEGIN)
	{
		impulse += z * CMathCore::Sqrt(2.0f * 9.8f * m_fJumping) / (m_fAcceleration * ifps);
	}

	// rotate velocity
	if (GetGround())
	{
		m_vVelocity = x * x_velocity + y * y_velocity + z * z_velocity;
	}
	// time
	float time = ifps * g_Engine.pPhysics->GetScale();
	// target velocity
	float target_velocity = Length(vec2(Dot(x, impulse), Dot(y, impulse)));

	// penetration tolerance
	float penetration = g_Engine.pPhysics->GetPenetrationTolerance();
	float penetration_2 = penetration * 2.0f;

}<commit_msg>Signed-off-by: mrlitong <litongtongxue@gmail.com><commit_after>#include "Engine.h"
#include "Game.h"
#include "ObjectDummy.h"
#include "BodyRigid.h"
#include "BodyDummy.h"

#include "Physics.h"
#include "ShapeCapsule.h"
#include "ActorBase.h"
#include "Visualizer.h"
#include "sys/SysControl.h"

#ifdef MEMORY_INFO
#define new new(__FILE__, __LINE__)
#endif // MEMORY_INFO

using namespace MathLib;

/*
*/
#define ACTOR_BASE_IFPS             (1.0f / 60.0f)
#define ACTOR_BASE_CLAMP            89.9f
#define ACTOR_BASE_COLLISIONS       4

/*
*/

CActorBase::CActorBase()
{
	m_vUp = vec3(0.0f, 0.0f, 1.0f);
	m_pObject = new CObjectDummy();
	m_pDummy = new CBodyDummy();
	m_pShape = new CShapeCapsule(1.0f, 1.0f);

	m_nFlush = 0;
	m_vPosition = Vec3_zero;
	m_vVelocity = Vec3_zero;
	m_fPhiAngle = 0.0f;			//б		άƽϵУֱYķX֮ļн
	m_fThetaAngle = 0.0f;			//λǣ뵱ǰ˳ʱ߹ĽǶ

	for (int i = 0; i < NUM_STATES; i++)
	{
		m_pStates[i] = 0;
		m_pTimes[i] = 0.0f;
	}

	m_pDummy->SetEnabled(1);
	m_pObject->SetBody(NULL);
	m_pObject->SetBody(m_pDummy);
	m_pShape->SetBody(NULL);
	m_pShape->SetBody(m_pDummy);

	m_pObject->SetWorldTransform(Get_Body_Transform());
	m_pShape->SetRestitution(0.0f);
	m_pShape->SetCollisionMask(2);

	SetEnabled(1);
	SetViewDirection(vec3(0.0f,1.0f,0.0f));
	SetCollision(1);
	SetCollisionRadius(0.3f);
	SetCollisionHeight(1.0f);
	SetFriction(2.0f);
	SetMinVelocity(2.0f);
	SetMaxVelocity(4.0f);
	SetAcceleration(8.0f);
	SetDamping(8.0f);
	SetJumping(1.5f);

	SetGround(0);
	SetCeiling(0);

}
CActorBase::~CActorBase()
{
	m_pDummy->SetObject(NULL);
	delete m_pObject;
	delete m_pDummy;
}

void CActorBase::SetEnabled(int enable)
{
	m_nEnable = enable;
	m_pDummy->SetEnabled(m_nEnable);
}
int CActorBase::IsEnabled() const
{
	return m_nEnable;
}
void CActorBase::Update(float ifps)
{
	if (!m_nEnable)
	{
		return;
	}
	// impulse
	vec3 impulse = vec3_zero;

	// ortho basis
	vec3 tangent, binormal;
	OrthoBasis(m_vUp, tangent, binormal);
	// current basis
	vec3 x = quat(m_vUp, -m_fPhiAngle) * binormal;
	vec3 y = Normalize(Cross(m_vUp, x));
	vec3 z = Normalize(Cross(x, y));
	// handle states
	Update_States(1, ifps);

	// old velocity
	float x_velocity = Dot(x, m_vVelocity);
	float y_velocity = Dot(y, m_vVelocity);
	float z_velocity = Dot(z, m_vVelocity);

	// movement
	if (m_pStates[STATE_FORWARD]) impulse += x;
	if (m_pStates[STATE_BACKWARD]) impulse -= x;
	if (m_pStates[STATE_MOVE_LEFT]) impulse += y;
	if (m_pStates[STATE_MOVE_RIGHT]) impulse -= y;
	impulse.normalize();
	// velocity
	if (m_pStates[STATE_RUN]) impulse *= m_fMaxVelocity;
	else impulse *= m_fMinVelocity;

	// jump
	if (m_pStates[STATE_JUMP] == STATE_BEGIN)
	{
		impulse += z * CMathCore::Sqrt(2.0f * 9.8f * m_fJumping) / (m_fAcceleration * ifps);
	}

	// rotate velocity
	if (GetGround())
	{
		m_vVelocity = x * x_velocity + y * y_velocity + z * z_velocity;
	}
	// time
	float time = ifps * g_Engine.pPhysics->GetScale();
	// target velocity
	float target_velocity = Length(vec2(Dot(x, impulse), Dot(y, impulse)));

	// penetration tolerance
	float penetration = g_Engine.pPhysics->GetPenetrationTolerance();
	float penetration_2 = penetration * 2.0f;

	// frozen linear velocity
	float frozen_velocity = g_Engine.pPhysics->GetFrozenLinearVelocity();

}<|endoftext|>


<commit_before><commit_msg>Remove debug prints<commit_after><|endoftext|>


<commit_before>#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "sensor_msgs/LaserScan.h"
#include <nav_msgs/Odometry.h>
#include "std_msgs/String.h"

#include <cstdlib> // Needed for rand()
#include <ctime> // Needed to seed random number generator with a time value

float prevclosestRange = 0;
double linear_x;
double angular_z;

//pose of the robot
double px;
double py;
double theta;
double prevpx;
double prevpy;
bool isRotate;

int checkcount=0;

class RandomWalk {
		public:
		// Construst a new RandomWalk object and hook up this ROS node
		// to the simulated robot's velocity control and laser topics
		RandomWalk(ros::NodeHandle& nh) :
		fsm(FSM_MOVE_FORWARD),
		rotateStartTime(ros::Time::now()),
		rotateDuration(0.f) {
		// Initialize random time generator
		srand(time(NULL));
		// Advertise a new publisher for the simulated robot's velocity command topic
		// (the second argument indicates that if multiple command messages are in
		// the queue to be sent, only the last command will be sent)
		ros::NodeHandle n;
		commandPub = nh.advertise<geometry_msgs::Twist>("robot_0/cmd_vel",1000);
		sheepPosPub = nh.advertise<std_msgs::String>("sheep_position",1000);
		// Subscribe to the simulated robot's laser scan topic and tell ROS to call
		// this->commandCallback() whenever a new message is published on that topic
		laserSub = nh.subscribe<sensor_msgs::LaserScan>("robot_0/base_scan", 1000, &RandomWalk::commandCallback, this);
		StageOdo_sub = nh.subscribe<nav_msgs::Odometry>("robot_0/odom",1000, &RandomWalk::StageOdom_callback,this);
		//ros::Subscriber StageOdo_sub = n.subscribe<nav_msgs::Odometry>("robot_0/odom",1000, StageOdom_callback);

	};

	void StageOdom_callback(nav_msgs::Odometry msg)
	{
		//This is the call back function to process odometry messages coming from Stage.

		px = 5 + msg.pose.pose.position.x;
		py =10 + msg.pose.pose.position.y;
		//printf("%f",px);

		// If the current position is the same the previous position, then the robot is stuck and needs to move around
		if ((px == prevpx) && (py == prevpy)) {
			//msg.pose.pose.position.x = 5;
			//ROS_INFO("Prevpx: %f",prevpx);

			// Note the negative linear_x
			//linear_x=-0.2;
			//angular_z=1;

			//theta=10;
			//px = 5;
			//py= 5;
			//printf("Robot stuck");
			if (!isRotate) {
				ROS_INFO("Robot stuck");
				double r2 = (double)rand()/((double)RAND_MAX/(M_PI/2));
				double m2 = (double)rand()/((double)RAND_MAX/0.5);
				//ROS_INFO("r2" << r2);
				move(-m2, r2);

			}
		} else {
			// One the robot becomes unstuck, then it moves around again normally
			//linear_x = 0.2;
			//angular_z = 0.2;
			//ROS_INFO("Robot unstuck");
			//checkcount=0;
		}
		ROS_INFO("Robot 0 -- Current x position is: %f", px);
		ROS_INFO("Robot 0 -- Current y position is: %f", py);
		prevpx = px;
		prevpy = py;
	};

	// Send a velocity command
	void move(double linearVelMPS, double angularVelRadPS) {
		geometry_msgs::Twist msg; // The default constructor will set all commands to 0
		msg.linear.x = linearVelMPS;
		msg.angular.z = angularVelRadPS;
		commandPub.publish(msg);
	};

	// Process the incoming laser scan message
	void commandCallback(const sensor_msgs::LaserScan::ConstPtr& msg) {
		if (fsm == FSM_MOVE_FORWARD) {
			// Compute the average range value between MIN_SCAN_ANGLE and MAX_SCAN_ANGLE
			//
			// NOTE: ideally, the following loop should have additional checks to ensure
			// that indices are not out of bounds, by computing:
			//
			//- currAngle = msg->angle_min + msg->angle_increment*currIndex
			//
			// and then ensuring that currAngle <= msg->angle_max
			unsigned int minIndex = ceil((MIN_SCAN_ANGLE_RAD - msg->angle_min) / msg->angle_increment);
			unsigned int maxIndex = ceil((MAX_SCAN_ANGLE_RAD - msg->angle_min) / msg->angle_increment);
			float closestRange = msg->ranges[minIndex];

			for (unsigned int currIndex = minIndex + 1; currIndex < maxIndex; currIndex++) {
				if (msg->ranges[currIndex] < closestRange) {
					closestRange = msg->ranges[currIndex];
				}
			}
			//if (closestRange == prevclosestRange) {
			//	ROS_INFO_STREAM("STUCK");
			//	move(-FORWARD_SPEED_MPS, ROTATE_SPEED_RADPS);
			//	//move(0, ROTATE_SPEED_RADPS);
			//} else {

				//ROS_INFO_STREAM("Range: " << closestRange);
				prevclosestRange = closestRange;

				if (closestRange < PROXIMITY_RANGE_M) {
					fsm=FSM_ROTATE;
					rotateStartTime=ros::Time::now();
					float r2 = (float)rand()/((float)RAND_MAX/100);
					rotateDuration=ros::Duration(rand() % 2);
					//fsm= FSM_MOVE_FORWARD;
				}
			//}

		}
	};
	// Main FSM loop for ensuring that ROS messages are
	// processed in a timely manner, and also for sending
	// velocity controls to the simulated robot based on the FSM state
	void spin() {
		ros::Rate rate(10); // Specify the FSM loop rate in Hz
		while (ros::ok()) { // Keep spinning loop until user presses Ctrl+C
			std_msgs::String msg;
			std::stringstream ss;
			ss << "robot_0 -- px:" << px << " py:" << py << " theta:" << theta << " isRotate:" << isRotate;
    			msg.data = ss.str();

    			//ROS_INFO("%s", msg.data.c_str());

			if (fsm == FSM_MOVE_FORWARD) {
				//ROS_INFO_STREAM("Start forward");
				move(FORWARD_SPEED_MPS, 0);
				checkcount++;
				if (checkcount > 3) {
					isRotate=false;
				}
			}
			if (fsm == FSM_ROTATE) {
				//ROS_INFO_STREAM("Start rotate");
				move(0, ROTATE_SPEED_RADPS);
				rotateEndTime=ros::Time::now();
				isRotate=true;
				//ROS_INFO_STREAM("Time: " << rotateEndTime);

				if ((rotateEndTime - rotateStartTime) > rotateDuration) {
					fsm=FSM_MOVE_FORWARD;
					//ROS_INFO_STREAM("End rotate");
					checkcount=0;
				}
			}

		sheepPosPub.publish(msg);

		ros::spinOnce(); // Need to call this function often to allow ROS to process incoming messages
		rate.sleep(); // Sleep for the rest of the cycle, to enforce the FSM loop rate
		}
	};
	enum FSM {FSM_MOVE_FORWARD, FSM_ROTATE};
	// Tunable parameters
	// TODO: tune parameters as you see fit
	const static double MIN_SCAN_ANGLE_RAD = -10.0/180*M_PI;
	const static double MAX_SCAN_ANGLE_RAD = +10.0/180*M_PI;
	const static float PROXIMITY_RANGE_M = 1; // Should be smaller than sensor_msgs::LaserScan::range_max
	const static double FORWARD_SPEED_MPS = 0.2;
	const static double ROTATE_SPEED_RADPS = M_PI/2;

	protected:
	ros::Publisher commandPub; // Publisher to the simulated robot's velocity command topic
	ros::Publisher sheepPosPub;
	ros::Subscriber laserSub; // Subscriber to the simulated robot's laser scan topic
	enum FSM fsm; // Finite state machine for the random walk algorithm
	ros::Time rotateStartTime; // Start time of the rotation
	ros::Time rotateEndTime;
	ros::Duration rotateDuration; // Duration of the rotation
	ros::Subscriber StageOdo_sub;

};
int main(int argc, char **argv) {
	ros::init(argc, argv, "RobotNode0"); // Initiate new ROS node named "random_walk"
	ros::NodeHandle n;
	prevpx = 0;
	prevpx= 0;
	RandomWalk walker(n); // Create new random walk object
	walker.spin(); // Execute FSM loop
	return 0;
};

<commit_msg>Added line that prints when a new terminal is opened for the rosrun<commit_after>#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "sensor_msgs/LaserScan.h"
#include <nav_msgs/Odometry.h>
#include "std_msgs/String.h"

#include <cstdlib> // Needed for rand()
#include <ctime> // Needed to seed random number generator with a time value

float prevclosestRange = 0;
double linear_x;
double angular_z;

//pose of the robot
double px;
double py;
double theta;
double prevpx;
double prevpy;
bool isRotate;

int checkcount=0;

class RandomWalk {
		public:
		// Construst a new RandomWalk object and hook up this ROS node
		// to the simulated robot's velocity control and laser topics
		RandomWalk(ros::NodeHandle& nh) :
		fsm(FSM_MOVE_FORWARD),
		rotateStartTime(ros::Time::now()),
		rotateDuration(0.f) {
		// Initialize random time generator
		srand(time(NULL));
		// Advertise a new publisher for the simulated robot's velocity command topic
		// (the second argument indicates that if multiple command messages are in
		// the queue to be sent, only the last command will be sent)
		ros::NodeHandle n;
		commandPub = nh.advertise<geometry_msgs::Twist>("robot_0/cmd_vel",1000);
		sheepPosPub = nh.advertise<std_msgs::String>("sheep_position",1000);
		// Subscribe to the simulated robot's laser scan topic and tell ROS to call
		// this->commandCallback() whenever a new message is published on that topic
		laserSub = nh.subscribe<sensor_msgs::LaserScan>("robot_0/base_scan", 1000, &RandomWalk::commandCallback, this);
		StageOdo_sub = nh.subscribe<nav_msgs::Odometry>("robot_0/odom",1000, &RandomWalk::StageOdom_callback,this);
		//ros::Subscriber StageOdo_sub = n.subscribe<nav_msgs::Odometry>("robot_0/odom",1000, StageOdom_callback);

	};

	void StageOdom_callback(nav_msgs::Odometry msg)
	{
		//This is the call back function to process odometry messages coming from Stage.

		px = 5 + msg.pose.pose.position.x;
		py =10 + msg.pose.pose.position.y;
		//printf("%f",px);

		// If the current position is the same the previous position, then the robot is stuck and needs to move around
		if ((px == prevpx) && (py == prevpy)) {
			//msg.pose.pose.position.x = 5;
			//ROS_INFO("Prevpx: %f",prevpx);

			// Note the negative linear_x
			//linear_x=-0.2;
			//angular_z=1;

			//theta=10;
			//px = 5;
			//py= 5;
			//printf("Robot stuck");
			if (!isRotate) {
				ROS_INFO("Robot stuck");
				double r2 = (double)rand()/((double)RAND_MAX/(M_PI/2));
				double m2 = (double)rand()/((double)RAND_MAX/0.5);
				//ROS_INFO("r2" << r2);
				move(-m2, r2);

			}
		} else {
			// One the robot becomes unstuck, then it moves around again normally
			//linear_x = 0.2;
			//angular_z = 0.2;
			//ROS_INFO("Robot unstuck");
			//checkcount=0;
		}
		ROS_INFO("Robot 0 -- Current x position is: %f", px);
		ROS_INFO("Robot 0 -- Current y position is: %f", py);
		prevpx = px;
		prevpy = py;
	};

	// Send a velocity command
	void move(double linearVelMPS, double angularVelRadPS) {
		geometry_msgs::Twist msg; // The default constructor will set all commands to 0
		msg.linear.x = linearVelMPS;
		msg.angular.z = angularVelRadPS;
		commandPub.publish(msg);
	};

	// Process the incoming laser scan message
	void commandCallback(const sensor_msgs::LaserScan::ConstPtr& msg) {
		if (fsm == FSM_MOVE_FORWARD) {
			// Compute the average range value between MIN_SCAN_ANGLE and MAX_SCAN_ANGLE
			//
			// NOTE: ideally, the following loop should have additional checks to ensure
			// that indices are not out of bounds, by computing:
			//
			//- currAngle = msg->angle_min + msg->angle_increment*currIndex
			//
			// and then ensuring that currAngle <= msg->angle_max
			unsigned int minIndex = ceil((MIN_SCAN_ANGLE_RAD - msg->angle_min) / msg->angle_increment);
			unsigned int maxIndex = ceil((MAX_SCAN_ANGLE_RAD - msg->angle_min) / msg->angle_increment);
			float closestRange = msg->ranges[minIndex];

			for (unsigned int currIndex = minIndex + 1; currIndex < maxIndex; currIndex++) {
				if (msg->ranges[currIndex] < closestRange) {
					closestRange = msg->ranges[currIndex];
				}
			}
			//if (closestRange == prevclosestRange) {
			//	ROS_INFO_STREAM("STUCK");
			//	move(-FORWARD_SPEED_MPS, ROTATE_SPEED_RADPS);
			//	//move(0, ROTATE_SPEED_RADPS);
			//} else {

				//ROS_INFO_STREAM("Range: " << closestRange);
				prevclosestRange = closestRange;

				if (closestRange < PROXIMITY_RANGE_M) {
					fsm=FSM_ROTATE;
					rotateStartTime=ros::Time::now();
					float r2 = (float)rand()/((float)RAND_MAX/100);
					rotateDuration=ros::Duration(rand() % 2);
					//fsm= FSM_MOVE_FORWARD;
				}
			//}

		}
	};
	// Main FSM loop for ensuring that ROS messages are
	// processed in a timely manner, and also for sending
	// velocity controls to the simulated robot based on the FSM state
	void spin() {
		ros::Rate rate(10); // Specify the FSM loop rate in Hz
		while (ros::ok()) { // Keep spinning loop until user presses Ctrl+C
			std_msgs::String msg;
			std::stringstream ss;
			ss << "robot_0 -- px:" << px << " py:" << py << " theta:" << theta << " isRotate:" << isRotate;
    			msg.data = ss.str();

    			//ROS_INFO("%s", msg.data.c_str());

			if (fsm == FSM_MOVE_FORWARD) {
				//ROS_INFO_STREAM("Start forward");
				move(FORWARD_SPEED_MPS, 0);
				checkcount++;
				if (checkcount > 3) {
					isRotate=false;
				}
			}
			if (fsm == FSM_ROTATE) {
				//ROS_INFO_STREAM("Start rotate");
				move(0, ROTATE_SPEED_RADPS);
				rotateEndTime=ros::Time::now();
				isRotate=true;
				//ROS_INFO_STREAM("Time: " << rotateEndTime);

				if ((rotateEndTime - rotateStartTime) > rotateDuration) {
					fsm=FSM_MOVE_FORWARD;
					//ROS_INFO_STREAM("End rotate");
					checkcount=0;
				}
			}

		sheepPosPub.publish(msg);

		ros::spinOnce(); // Need to call this function often to allow ROS to process incoming messages
		rate.sleep(); // Sleep for the rest of the cycle, to enforce the FSM loop rate
		}
	};
	enum FSM {FSM_MOVE_FORWARD, FSM_ROTATE};
	// Tunable parameters
	// TODO: tune parameters as you see fit
	const static double MIN_SCAN_ANGLE_RAD = -10.0/180*M_PI;
	const static double MAX_SCAN_ANGLE_RAD = +10.0/180*M_PI;
	const static float PROXIMITY_RANGE_M = 1; // Should be smaller than sensor_msgs::LaserScan::range_max
	const static double FORWARD_SPEED_MPS = 0.2;
	const static double ROTATE_SPEED_RADPS = M_PI/2;

	protected:
	ros::Publisher commandPub; // Publisher to the simulated robot's velocity command topic
	ros::Publisher sheepPosPub;
	ros::Subscriber laserSub; // Subscriber to the simulated robot's laser scan topic
	enum FSM fsm; // Finite state machine for the random walk algorithm
	ros::Time rotateStartTime; // Start time of the rotation
	ros::Time rotateEndTime;
	ros::Duration rotateDuration; // Duration of the rotation
	ros::Subscriber StageOdo_sub;

};
int main(int argc, char **argv) {
	ros::init(argc, argv, "RobotNode0"); // Initiate new ROS node named "RobotNode0"
	ROS_INFO("This node is: RobotNode0");
	ros::NodeHandle n;
	prevpx = 0;
	prevpx= 0;
	RandomWalk walker(n); // Create new random walk object
	walker.spin(); // Execute FSM loop
	return 0;
};

<|endoftext|>


<commit_before>/* -*- c-basic-offset: 2 -*- */
/*
  Copyright(C) 2012 Kouhei Sutou <kou@clear-code.com>

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

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

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

#include "mrn_auto_increment_value_lock.hpp"

namespace mrn {
  AutoIncrementValueLock::AutoIncrementValueLock(TABLE_SHARE *table_share)
    : table_share_(table_share),
      need_lock_(table_share_->tmp_table == NO_TMP_TABLE) {
    if (need_lock_) {
      mysql_mutex_lock(&table_share_->LOCK_ha_data);
    }
  }

  AutoIncrementValueLock::~AutoIncrementValueLock() {
    if (need_lock_) {
      mysql_mutex_unlock(&table_share_->LOCK_ha_data);
    }
  }
}
<commit_msg>mysql5.1: make buildable but alter_table_disable_enable_keys still fails<commit_after>/* -*- c-basic-offset: 2 -*- */
/*
  Copyright(C) 2012 Kouhei Sutou <kou@clear-code.com>

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

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

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

#include "mrn_auto_increment_value_lock.hpp"

#if MYSQL_VERSION_ID >= 50500
#  define AUTO_INCREMENT_VALUE_MUTEX(table_share) (&(table_share->LOCK_ha_data))
#else
#  define AUTO_INCREMENT_VALUE_MUTEX(table_share) (&(table_share->mutex))
#endif

namespace mrn {
  AutoIncrementValueLock::AutoIncrementValueLock(TABLE_SHARE *table_share)
    : table_share_(table_share),
      need_lock_(table_share_->tmp_table == NO_TMP_TABLE) {
    if (need_lock_) {
      mysql_mutex_lock(AUTO_INCREMENT_VALUE_MUTEX(table_share_));
    }
  }

  AutoIncrementValueLock::~AutoIncrementValueLock() {
    if (need_lock_) {
      mysql_mutex_unlock(AUTO_INCREMENT_VALUE_MUTEX(table_share_));
    }
  }
}
<|endoftext|>


<commit_before><commit_msg>- Missing space..<commit_after><|endoftext|>