<commit_before>/*
    This file is part of KOrganizer.

    Copyright (c) 2001,2003 Cornelius Schumacher <schumacher@kde.org>
    Copyright (C) 2003-2004 Reinhold Kainhofer <reinhold@kainhofer.com>

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

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

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

    As a special exception, permission is given to link this program
    with any edition of Qt, and distribute the resulting executable,
    without including the source code for Qt in the source distribution.
*/

#include "kocore.h"

#include "koprefs.h"
#include "koglobals.h"
#include "koidentitymanager.h"

#include <calendar/plugin.h>
#include <korganizer/part.h>

#include <kdebug.h>
#include <kconfig.h>
#include <kxmlguifactory.h>
#include <kstandarddirs.h>
#include <klocale.h>
#include <kservicetypetrader.h>

#include <QWidget>

KOCore *KOCore::mSelf = 0;

KOCore *KOCore::self()
{
  if ( !mSelf ) {
    mSelf = new KOCore;
  }

  return mSelf;
}

KOCore::KOCore()
  : mCalendarDecorationsLoaded( false ), mIdentityManager( 0 )
{
}

KOCore::~KOCore()
{
  mSelf = 0;
}

KService::List KOCore::availablePlugins( const QString &type, int version )
{
  QString constraint;
  if ( version >= 0 ) {
    constraint =
      QString( "[X-KDE-PluginInterfaceVersion] == %1" ).arg( QString::number( version ) );
  }

  return KServiceTypeTrader::self()->query( type, constraint );
}

KService::List KOCore::availablePlugins()
{
  return availablePlugins( KOrg::Plugin::serviceType(), KOrg::Plugin::interfaceVersion() );
}

KService::List KOCore::availableCalendarDecorations()
{
  return availablePlugins( KOrg::CalendarDecoration::Decoration::serviceType(),
                           KOrg::CalendarDecoration::Decoration::interfaceVersion() );
}

KService::List KOCore::availableParts()
{
  return availablePlugins( KOrg::Part::serviceType(), KOrg::Part::interfaceVersion() );
}

KService::List KOCore::availablePrintPlugins()
{
  return
    availablePlugins( KOrg::PrintPlugin::serviceType(), KOrg::PrintPlugin::interfaceVersion() );
}

KOrg::Plugin *KOCore::loadPlugin( KService::Ptr service )
{
  kDebug() << service->library();

  if ( !service->hasServiceType( KOrg::Plugin::serviceType() ) ) {
    return 0;
  }

  KPluginLoader loader( *service );
  KPluginFactory *factory = loader.factory();

  if ( !factory ) {
    kDebug() << "Factory creation failed";
    return 0;
  }

  KOrg::PluginFactory *pluginFactory = static_cast<KOrg::PluginFactory *>( factory );

  if ( !pluginFactory ) {
    kDebug() << "Cast failed";
    return 0;
  }

  return pluginFactory->createPluginFactory();
}

KOrg::Plugin *KOCore::loadPlugin( const QString &name )
{
  KService::List list = availablePlugins();
  KService::List::ConstIterator it;
  for ( it = list.constBegin(); it != list.constEnd(); ++it ) {
    if ( (*it)->desktopEntryName() == name ) {
      return loadPlugin( *it );
    }
  }
  return 0;
}

KOrg::CalendarDecoration::Decoration *KOCore::loadCalendarDecoration( KService::Ptr service )
{
  kDebug() << service->library();

  KPluginLoader loader( *service );
  KPluginFactory *factory = loader.factory();

  if ( !factory ) {
    kDebug() << "Factory creation failed";
    return 0;
  }

  KOrg::CalendarDecoration::DecorationFactory *pluginFactory =
      static_cast<KOrg::CalendarDecoration::DecorationFactory *>( factory );

  if ( !pluginFactory ) {
    kDebug() << "Cast failed";
    return 0;
  }

  return pluginFactory->createPluginFactory();
}

KOrg::CalendarDecoration::Decoration *KOCore::loadCalendarDecoration( const QString &name )
{
  KService::List list = availableCalendarDecorations();
  KService::List::ConstIterator it;
  for ( it = list.constBegin(); it != list.constEnd(); ++it ) {
    if ( (*it)->desktopEntryName() == name ) {
      return loadCalendarDecoration( *it );
    }
  }
  return 0;
}

KOrg::Part *KOCore::loadPart( KService::Ptr service, KOrg::MainWindow *parent )
{
  kDebug() << service->library();

  if ( !service->hasServiceType( KOrg::Part::serviceType() ) ) {
    return 0;
  }

  KPluginLoader loader( *service );
  KPluginFactory *factory = loader.factory();

  if ( !factory ) {
    kDebug() << "Factory creation failed";
    return 0;
  }

  KOrg::PartFactory *pluginFactory =
      static_cast<KOrg::PartFactory *>( factory );

  if ( !pluginFactory ) {
    kDebug() << "Cast failed";
    return 0;
  }

  return pluginFactory->createPluginFactory( parent );
}

KOrg::PrintPlugin *KOCore::loadPrintPlugin( KService::Ptr service )
{
  kDebug() << service->library();

  if ( !service->hasServiceType( KOrg::PrintPlugin::serviceType() ) ) {
    return 0;
  }

  KPluginLoader loader( *service );
  KPluginFactory *factory = loader.factory();

  if ( !factory ) {
    kDebug() << "Factory creation failed";
    return 0;
  }

  KOrg::PrintPluginFactory *pluginFactory =
      static_cast<KOrg::PrintPluginFactory *>( factory );

  if ( !pluginFactory ) {
    kDebug() << "Cast failed";
    return 0;
  }

  return pluginFactory->createPluginFactory();
}

void KOCore::addXMLGUIClient( QWidget *wdg, KXMLGUIClient *guiclient )
{
  mXMLGUIClients.insert( wdg, guiclient );
}

void KOCore::removeXMLGUIClient( QWidget *wdg )
{
  mXMLGUIClients.remove( wdg );
}

KXMLGUIClient *KOCore::xmlguiClient( QWidget *wdg ) const
{
  QWidget *topLevel = wdg->topLevelWidget();
  QMap<QWidget*, KXMLGUIClient*>::ConstIterator it = mXMLGUIClients.find( topLevel );
  if ( it != mXMLGUIClients.constEnd() ) {
    return it.value();
  }

  return 0;
}

KOrg::Part *KOCore::loadPart( const QString &name, KOrg::MainWindow *parent )
{
  KService::List list = availableParts();
  KService::List::ConstIterator it;
  for ( it = list.constBegin(); it != list.constEnd(); ++it ) {
    if ( (*it)->desktopEntryName() == name ) {
      return loadPart( *it, parent );
    }
  }
  return 0;
}

KOrg::PrintPlugin *KOCore::loadPrintPlugin( const QString &name )
{
  KService::List list = availablePrintPlugins();
  KService::List::ConstIterator it;
  for ( it = list.constBegin(); it != list.constEnd(); ++it ) {
    if ( (*it)->desktopEntryName() == name ) {
      return loadPrintPlugin( *it );
    }
  }
  return 0;
}

KOrg::CalendarDecoration::Decoration::List KOCore::loadCalendarDecorations()
{
  if ( !mCalendarDecorationsLoaded ) {
    QStringList selectedPlugins = KOPrefs::instance()->mSelectedPlugins;

    mCalendarDecorations.clear();
    KService::List plugins = availableCalendarDecorations();
    KService::List::ConstIterator it;
    for ( it = plugins.constBegin(); it != plugins.constEnd(); ++it ) {
      if ( (*it)->hasServiceType( KOrg::CalendarDecoration::Decoration::serviceType() ) ) {
        QString name = (*it)->desktopEntryName();
        if ( selectedPlugins.contains( name ) ) {
          KOrg::CalendarDecoration::Decoration *d = loadCalendarDecoration(*it);
          mCalendarDecorations.append( d );
        }
      }
    }
    mCalendarDecorationsLoaded = true;
  }

  return mCalendarDecorations;
}

KOrg::Part::List KOCore::loadParts( KOrg::MainWindow *parent )
{
  KOrg::Part::List parts;

  QStringList selectedPlugins = KOPrefs::instance()->mSelectedPlugins;

  KService::List plugins = availableParts();
  KService::List::ConstIterator it;
  for ( it = plugins.constBegin(); it != plugins.constEnd(); ++it ) {
    if ( selectedPlugins.contains( (*it)->desktopEntryName() ) ) {
      KOrg::Part *part = loadPart( *it, parent );
      if ( part ) {
        if ( !parent->mainGuiClient() ) {
          kError() << "parent has no mainGuiClient.";
        } else {
          parent->mainGuiClient()->insertChildClient( part );
          parts.append( part );
        }
      }
    }
  }
  return parts;
}

KOrg::PrintPlugin::List KOCore::loadPrintPlugins()
{
  KOrg::PrintPlugin::List loadedPlugins;

  QStringList selectedPlugins = KOPrefs::instance()->mSelectedPlugins;

  KService::List plugins = availablePrintPlugins();
  KService::List::ConstIterator it;
  for ( it = plugins.constBegin(); it != plugins.constEnd(); ++it ) {
    if ( selectedPlugins.contains( (*it)->desktopEntryName() ) ) {
      KOrg::PrintPlugin *part = loadPrintPlugin( *it );
      if ( part ) {
        loadedPlugins.append( part );
      }
    }
  }
  return loadedPlugins;
}

void KOCore::unloadPlugins()
{
  qDeleteAll( mCalendarDecorations );
  mCalendarDecorations.clear();
  mCalendarDecorationsLoaded = false;
}

void KOCore::unloadParts( KOrg::MainWindow *parent, KOrg::Part::List &parts )
{
  foreach ( KOrg::Part *part, parts ) {
    parent->mainGuiClient()->removeChildClient( part );
    delete part;
  }
  parts.clear();
}

KOrg::Part::List KOCore::reloadParts( KOrg::MainWindow *parent, KOrg::Part::List &parts )
{
  KXMLGUIFactory *factory = parent->mainGuiClient()->factory();
  factory->removeClient( parent->mainGuiClient() );

  unloadParts( parent, parts );
  KOrg::Part::List list = loadParts( parent );

  factory->addClient( parent->mainGuiClient() );

  return list;
}

void KOCore::reloadPlugins()
{
  // TODO: does this still apply?
  // Plugins should be unloaded, but e.g. komonthview keeps using the old ones
  unloadPlugins();
  loadCalendarDecorations();
}

KPIMIdentities::IdentityManager *KOCore::identityManager()
{
  if ( !mIdentityManager ) {
    mIdentityManager = new KOrg::IdentityManager;
  }
  return mIdentityManager;
}
<commit_msg>remove annoying kDebug message. SVN_SILENT:<commit_after>/*
    This file is part of KOrganizer.

    Copyright (c) 2001,2003 Cornelius Schumacher <schumacher@kde.org>
    Copyright (C) 2003-2004 Reinhold Kainhofer <reinhold@kainhofer.com>

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

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

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

    As a special exception, permission is given to link this program
    with any edition of Qt, and distribute the resulting executable,
    without including the source code for Qt in the source distribution.
*/

#include "kocore.h"

#include "koprefs.h"
#include "koglobals.h"
#include "koidentitymanager.h"

#include <calendar/plugin.h>
#include <korganizer/part.h>

#include <kdebug.h>
#include <kconfig.h>
#include <kxmlguifactory.h>
#include <kstandarddirs.h>
#include <klocale.h>
#include <kservicetypetrader.h>

#include <QWidget>

KOCore *KOCore::mSelf = 0;

KOCore *KOCore::self()
{
  if ( !mSelf ) {
    mSelf = new KOCore;
  }

  return mSelf;
}

KOCore::KOCore()
  : mCalendarDecorationsLoaded( false ), mIdentityManager( 0 )
{
}

KOCore::~KOCore()
{
  mSelf = 0;
}

KService::List KOCore::availablePlugins( const QString &type, int version )
{
  QString constraint;
  if ( version >= 0 ) {
    constraint =
      QString( "[X-KDE-PluginInterfaceVersion] == %1" ).arg( QString::number( version ) );
  }

  return KServiceTypeTrader::self()->query( type, constraint );
}

KService::List KOCore::availablePlugins()
{
  return availablePlugins( KOrg::Plugin::serviceType(), KOrg::Plugin::interfaceVersion() );
}

KService::List KOCore::availableCalendarDecorations()
{
  return availablePlugins( KOrg::CalendarDecoration::Decoration::serviceType(),
                           KOrg::CalendarDecoration::Decoration::interfaceVersion() );
}

KService::List KOCore::availableParts()
{
  return availablePlugins( KOrg::Part::serviceType(), KOrg::Part::interfaceVersion() );
}

KService::List KOCore::availablePrintPlugins()
{
  return
    availablePlugins( KOrg::PrintPlugin::serviceType(), KOrg::PrintPlugin::interfaceVersion() );
}

KOrg::Plugin *KOCore::loadPlugin( KService::Ptr service )
{
  kDebug() << service->library();

  if ( !service->hasServiceType( KOrg::Plugin::serviceType() ) ) {
    return 0;
  }

  KPluginLoader loader( *service );
  KPluginFactory *factory = loader.factory();

  if ( !factory ) {
    kDebug() << "Factory creation failed";
    return 0;
  }

  KOrg::PluginFactory *pluginFactory = static_cast<KOrg::PluginFactory *>( factory );

  if ( !pluginFactory ) {
    kDebug() << "Cast failed";
    return 0;
  }

  return pluginFactory->createPluginFactory();
}

KOrg::Plugin *KOCore::loadPlugin( const QString &name )
{
  KService::List list = availablePlugins();
  KService::List::ConstIterator it;
  for ( it = list.constBegin(); it != list.constEnd(); ++it ) {
    if ( (*it)->desktopEntryName() == name ) {
      return loadPlugin( *it );
    }
  }
  return 0;
}

KOrg::CalendarDecoration::Decoration *KOCore::loadCalendarDecoration( KService::Ptr service )
{
  KPluginLoader loader( *service );
  KPluginFactory *factory = loader.factory();

  if ( !factory ) {
    kDebug() << "Factory creation failed";
    return 0;
  }

  KOrg::CalendarDecoration::DecorationFactory *pluginFactory =
      static_cast<KOrg::CalendarDecoration::DecorationFactory *>( factory );

  if ( !pluginFactory ) {
    kDebug() << "Cast failed";
    return 0;
  }

  return pluginFactory->createPluginFactory();
}

KOrg::CalendarDecoration::Decoration *KOCore::loadCalendarDecoration( const QString &name )
{
  KService::List list = availableCalendarDecorations();
  KService::List::ConstIterator it;
  for ( it = list.constBegin(); it != list.constEnd(); ++it ) {
    if ( (*it)->desktopEntryName() == name ) {
      return loadCalendarDecoration( *it );
    }
  }
  return 0;
}

KOrg::Part *KOCore::loadPart( KService::Ptr service, KOrg::MainWindow *parent )
{
  kDebug() << service->library();

  if ( !service->hasServiceType( KOrg::Part::serviceType() ) ) {
    return 0;
  }

  KPluginLoader loader( *service );
  KPluginFactory *factory = loader.factory();

  if ( !factory ) {
    kDebug() << "Factory creation failed";
    return 0;
  }

  KOrg::PartFactory *pluginFactory =
      static_cast<KOrg::PartFactory *>( factory );

  if ( !pluginFactory ) {
    kDebug() << "Cast failed";
    return 0;
  }

  return pluginFactory->createPluginFactory( parent );
}

KOrg::PrintPlugin *KOCore::loadPrintPlugin( KService::Ptr service )
{
  kDebug() << service->library();

  if ( !service->hasServiceType( KOrg::PrintPlugin::serviceType() ) ) {
    return 0;
  }

  KPluginLoader loader( *service );
  KPluginFactory *factory = loader.factory();

  if ( !factory ) {
    kDebug() << "Factory creation failed";
    return 0;
  }

  KOrg::PrintPluginFactory *pluginFactory =
      static_cast<KOrg::PrintPluginFactory *>( factory );

  if ( !pluginFactory ) {
    kDebug() << "Cast failed";
    return 0;
  }

  return pluginFactory->createPluginFactory();
}

void KOCore::addXMLGUIClient( QWidget *wdg, KXMLGUIClient *guiclient )
{
  mXMLGUIClients.insert( wdg, guiclient );
}

void KOCore::removeXMLGUIClient( QWidget *wdg )
{
  mXMLGUIClients.remove( wdg );
}

KXMLGUIClient *KOCore::xmlguiClient( QWidget *wdg ) const
{
  QWidget *topLevel = wdg->topLevelWidget();
  QMap<QWidget*, KXMLGUIClient*>::ConstIterator it = mXMLGUIClients.find( topLevel );
  if ( it != mXMLGUIClients.constEnd() ) {
    return it.value();
  }

  return 0;
}

KOrg::Part *KOCore::loadPart( const QString &name, KOrg::MainWindow *parent )
{
  KService::List list = availableParts();
  KService::List::ConstIterator it;
  for ( it = list.constBegin(); it != list.constEnd(); ++it ) {
    if ( (*it)->desktopEntryName() == name ) {
      return loadPart( *it, parent );
    }
  }
  return 0;
}

KOrg::PrintPlugin *KOCore::loadPrintPlugin( const QString &name )
{
  KService::List list = availablePrintPlugins();
  KService::List::ConstIterator it;
  for ( it = list.constBegin(); it != list.constEnd(); ++it ) {
    if ( (*it)->desktopEntryName() == name ) {
      return loadPrintPlugin( *it );
    }
  }
  return 0;
}

KOrg::CalendarDecoration::Decoration::List KOCore::loadCalendarDecorations()
{
  if ( !mCalendarDecorationsLoaded ) {
    QStringList selectedPlugins = KOPrefs::instance()->mSelectedPlugins;

    mCalendarDecorations.clear();
    KService::List plugins = availableCalendarDecorations();
    KService::List::ConstIterator it;
    for ( it = plugins.constBegin(); it != plugins.constEnd(); ++it ) {
      if ( (*it)->hasServiceType( KOrg::CalendarDecoration::Decoration::serviceType() ) ) {
        QString name = (*it)->desktopEntryName();
        if ( selectedPlugins.contains( name ) ) {
          KOrg::CalendarDecoration::Decoration *d = loadCalendarDecoration(*it);
          mCalendarDecorations.append( d );
        }
      }
    }
    mCalendarDecorationsLoaded = true;
  }

  return mCalendarDecorations;
}

KOrg::Part::List KOCore::loadParts( KOrg::MainWindow *parent )
{
  KOrg::Part::List parts;

  QStringList selectedPlugins = KOPrefs::instance()->mSelectedPlugins;

  KService::List plugins = availableParts();
  KService::List::ConstIterator it;
  for ( it = plugins.constBegin(); it != plugins.constEnd(); ++it ) {
    if ( selectedPlugins.contains( (*it)->desktopEntryName() ) ) {
      KOrg::Part *part = loadPart( *it, parent );
      if ( part ) {
        if ( !parent->mainGuiClient() ) {
          kError() << "parent has no mainGuiClient.";
        } else {
          parent->mainGuiClient()->insertChildClient( part );
          parts.append( part );
        }
      }
    }
  }
  return parts;
}

KOrg::PrintPlugin::List KOCore::loadPrintPlugins()
{
  KOrg::PrintPlugin::List loadedPlugins;

  QStringList selectedPlugins = KOPrefs::instance()->mSelectedPlugins;

  KService::List plugins = availablePrintPlugins();
  KService::List::ConstIterator it;
  for ( it = plugins.constBegin(); it != plugins.constEnd(); ++it ) {
    if ( selectedPlugins.contains( (*it)->desktopEntryName() ) ) {
      KOrg::PrintPlugin *part = loadPrintPlugin( *it );
      if ( part ) {
        loadedPlugins.append( part );
      }
    }
  }
  return loadedPlugins;
}

void KOCore::unloadPlugins()
{
  qDeleteAll( mCalendarDecorations );
  mCalendarDecorations.clear();
  mCalendarDecorationsLoaded = false;
}

void KOCore::unloadParts( KOrg::MainWindow *parent, KOrg::Part::List &parts )
{
  foreach ( KOrg::Part *part, parts ) {
    parent->mainGuiClient()->removeChildClient( part );
    delete part;
  }
  parts.clear();
}

KOrg::Part::List KOCore::reloadParts( KOrg::MainWindow *parent, KOrg::Part::List &parts )
{
  KXMLGUIFactory *factory = parent->mainGuiClient()->factory();
  factory->removeClient( parent->mainGuiClient() );

  unloadParts( parent, parts );
  KOrg::Part::List list = loadParts( parent );

  factory->addClient( parent->mainGuiClient() );

  return list;
}

void KOCore::reloadPlugins()
{
  // TODO: does this still apply?
  // Plugins should be unloaded, but e.g. komonthview keeps using the old ones
  unloadPlugins();
  loadCalendarDecorations();
}

KPIMIdentities::IdentityManager *KOCore::identityManager()
{
  if ( !mIdentityManager ) {
    mIdentityManager = new KOrg::IdentityManager;
  }
  return mIdentityManager;
}
<|endoftext|>


<commit_before>#ifndef SLAP_TOOL_HPP
#define SLAP_TOOL_HPP

#include <string>

namespace tool {

std::string quote(const std::string& s, char q='"');
std::string type_name(const std::type_info& self);

template<class F, class ... Args, std::size_t ... I>
static typename std::result_of< F(const Args&...) >::type
apply(const F& f, const std::tuple<const Args&...>& args, std::index_sequence<I...>) {
  return f(std::get<I>(args)...);
}

}

#endif
<commit_msg>header fix<commit_after>#ifndef SLAP_TOOL_HPP
#define SLAP_TOOL_HPP

#include <string>
#include <utility>

namespace tool {

std::string quote(const std::string& s, char q='"');
std::string type_name(const std::type_info& self);

template<class F, class ... Args, std::size_t ... I>
static typename std::result_of< F(const Args&...) >::type
apply(const F& f, const std::tuple<const Args&...>& args, std::index_sequence<I...>) {
  return f(std::get<I>(args)...);
}

}

#endif
<|endoftext|>


<commit_before>/*
 *  ZenStateSystem.cpp
 *  MWorksCore
 *
 *  Created by David Cox on 6/16/05.
 *  Copyright 2005 __MyCompanyName__. All rights reserved.
 *
 */

#include "MWorksCore/Experiment.h"
#include "ZenStateSystem.h"
#include "StateSystem.h"
#include "MWorksCore/Utilities.h"
#include "MWorksCore/Scheduler.h"
#include "MWorksCore/StateSystem.h"
#include "MWorksCore/StandardVariables.h"

#include "MachUtilities.h"

BEGIN_NAMESPACE_MW

StandardStateSystem::StandardStateSystem(const shared_ptr <Clock> &a_clock) :
    StateSystem(a_clock)
{
    in_action = false;
    in_transition = false;
    is_running = false;
    is_paused = false;
}

StandardStateSystem::~StandardStateSystem() {
    if (state_system_thread.joinable()) {
        state_system_thread.join();
    }
}

void StandardStateSystem::start(){
    boost::mutex::scoped_lock lock(state_system_mutex);

    if (is_running) {
        return;
    }

    //E->setInt(taskMode_edit, RUNNING);

//	(*state_system_mode) = RUNNING;

	mprintf("Called start on state system");

    // Make a copy of the experiment to ensure that it isn't destroyed before we're done with it
    shared_ptr<Experiment> current_experiment = GlobalCurrentExperiment;
	if (!current_experiment) {
		merror(M_STATE_SYSTEM_MESSAGE_DOMAIN,
			  "Cannot start state system without a valid experiment defined");
		return;
	}

    // Clean up the previous thread, if any
    if (state_system_thread.joinable()) {
        state_system_thread.join();
    }

	weak_ptr<State> exp_ref(current_experiment);
	current_experiment->setCurrentState(exp_ref);

    auto sharedThis = component_shared_from_this<StandardStateSystem>();
    state_system_thread = std::thread([sharedThis]() { sharedThis->run(); });

    is_running = true;
	sendSystemStateEvent();

}

void StandardStateSystem::stop(){
    boost::mutex::scoped_lock lock(state_system_mutex);

    if (!is_running) {
        return;
    }

    mprintf("Called stop on state system");
    (*state_system_mode) = STOPPING;

	// TODO: need to stop ongoing schedules...
	// esp. IO devices
    sendSystemStateEvent();
}

void StandardStateSystem::pause(){
    boost::mutex::scoped_lock lock(state_system_mutex);

    if (!is_running || is_paused) {
        return;
    }

    mprintf("Pausing state system");
    is_paused = true;
    (*state_system_mode) = PAUSED;

    sendSystemStateEvent();
}

void StandardStateSystem::resume(){
    boost::mutex::scoped_lock lock(state_system_mutex);

    if (!is_running || !is_paused) {
        return;
    }

    mprintf("Resuming paused state system");
    is_paused = false;
    (*state_system_mode) = RUNNING;

    sendSystemStateEvent();
}

bool StandardStateSystem::isRunning(){
    return is_running;
}

bool StandardStateSystem::isPaused(){
    return is_paused;
}

bool StandardStateSystem::isInAction(){
    return in_action;
}

bool StandardStateSystem::isInTransition(){
    return in_transition;
}

//void StandardStateSystem::setInAction(bool isit){
//    in_action = isit;
//}

//void StandardStateSystem::setInTransition(bool isit){
//    in_transition = isit;
//}

weak_ptr<State> StandardStateSystem::getCurrentState(){
    // Allow access to the current state only on the state system thread
    if (std::this_thread::get_id() == state_system_thread.get_id()) {
        return current_state;
    }
    return weak_ptr<State>();
}

void StandardStateSystem::run() {
    if (!(MachThreadSelf("MWorks State System").setPriority(TaskPriority::Default))) {
        merror(M_SCHEDULER_MESSAGE_DOMAIN, "Failed to set priority of state system thread");
    }

    // Make a copy of the experiment to ensure that it isn't destroyed before we're done with it
    shared_ptr<Experiment> current_experiment = GlobalCurrentExperiment;
    if (!current_experiment) {
        merror(M_STATE_SYSTEM_MESSAGE_DOMAIN,
               "Cannot start state system without a valid experiment defined");
        return;
    }

	mprintf("Starting state system....");

    //mprintf("----------setting task  mode to running------------");
	(*state_system_mode) = (long) RUNNING;
	current_state = current_experiment->getCurrentState();

	if(current_state.expired()){
		merror(M_STATE_SYSTEM_MESSAGE_DOMAIN,
				"current state is NULL. Shutting down state system...");

		(*state_system_mode) = (long)IDLE;
	}

	if(current_state.expired() == true){
		// TODO: better throw
		throw SimpleException("Invalid current state within state system");
	}

	shared_ptr<State> current_state_shared(current_state);
    weak_ptr<State> next_state;

	while (current_state_shared) {
        const bool canInterrupt = current_state_shared->isInterruptible();  // might not be an okay place to stop

        if (canInterrupt &&
            (long(*state_system_mode) == IDLE ||      // hard stop
             long(*state_system_mode) == STOPPING))   // stop requested
        {
            break;
        }
        else if (in_transition ||              // waiting for the next state
                 (canInterrupt && is_paused))  // paused
        {
            getClock()->yield();
            if (canInterrupt && is_paused) {
                continue;
            }
        }

		//mprintf("State system main loop, current state = %d", current_state);
		if (!in_transition) {
			in_action = true;

			//mState *test_state = current_state;

			try {
				current_state_shared->action();
			} catch(std::exception& e){
				merror(M_PARADIGM_MESSAGE_DOMAIN,
					   "Stopping state system: %s", e.what());
				state_system_mode->setValue((long)STOPPING);
				break;
			}

			// finished performing action
			in_action = false;
		}

		if (!in_action) {
			in_transition = true;

			try {
				next_state = current_state_shared->next();
			} catch (std::exception& e){
				merror(M_PARADIGM_MESSAGE_DOMAIN,
					  "Stopping state system: %s", e.what());
				state_system_mode->setValue((long)STOPPING);
				break;
			}

            if (next_state.expired()) {
                // no next state yet, sleep until the next tick
                continue;
            }

            shared_ptr<State> next_state_shared;

            try{
                shared_ptr<State> attempt(next_state); // cast weak_ptr into shared_ptr
                next_state_shared = attempt; // machination required because of weak to shared conversion semantics
			} catch (std::exception& e){
                mwarning(M_STATE_SYSTEM_MESSAGE_DOMAIN, "Failed to acquire shared_ptr from next_state; coming to an abrupt halt");
                (*state_system_mode) = IDLE;
                continue;
            }

			//mprintf("State system moving on... %d", next_state);

			current_experiment->setCurrentState(next_state);

			// If we've finished
			if(current_state_shared.get() == current_experiment.get() &&
					next_state_shared.get() == current_experiment.get()){
					mprintf("Returned to Experiment node, halting state system...");
					(*state_system_mode) = IDLE;
					current_state = weak_ptr<State>();
					next_state = weak_ptr<State>();
					continue;
			}

			current_state = next_state;
			current_state_shared = shared_ptr<State>(current_state);

			next_state = weak_ptr<State>();
			next_state_shared = shared_ptr<State>();

			// finished transition
			in_transition = false;
		}
	}

    {
        boost::mutex::scoped_lock lock(state_system_mutex);

        in_action = false;
        in_transition = false;
        is_running = false;
        is_paused = false;

        (*state_system_mode) = IDLE;
        mprintf("State system ending");

        // DDC: graceful stop?
        mprintf("Resetting experiment");
        current_experiment->reset();
    }
}

END_NAMESPACE_MW

<commit_msg>Eliminated a reference cycle in StandardStateSystem<commit_after>/*
 *  ZenStateSystem.cpp
 *  MWorksCore
 *
 *  Created by David Cox on 6/16/05.
 *  Copyright 2005 __MyCompanyName__. All rights reserved.
 *
 */

#include "MWorksCore/Experiment.h"
#include "ZenStateSystem.h"
#include "StateSystem.h"
#include "MWorksCore/Utilities.h"
#include "MWorksCore/Scheduler.h"
#include "MWorksCore/StateSystem.h"
#include "MWorksCore/StandardVariables.h"

#include "MachUtilities.h"

BEGIN_NAMESPACE_MW

StandardStateSystem::StandardStateSystem(const shared_ptr <Clock> &a_clock) :
    StateSystem(a_clock)
{
    in_action = false;
    in_transition = false;
    is_running = false;
    is_paused = false;
}

StandardStateSystem::~StandardStateSystem() {
    if (state_system_thread.joinable()) {
        state_system_thread.join();
    }
}

void StandardStateSystem::start(){
    boost::mutex::scoped_lock lock(state_system_mutex);

    if (is_running) {
        return;
    }

    //E->setInt(taskMode_edit, RUNNING);

//	(*state_system_mode) = RUNNING;

	mprintf("Called start on state system");

    // Make a copy of the experiment to ensure that it isn't destroyed before we're done with it
    shared_ptr<Experiment> current_experiment = GlobalCurrentExperiment;
	if (!current_experiment) {
		merror(M_STATE_SYSTEM_MESSAGE_DOMAIN,
			  "Cannot start state system without a valid experiment defined");
		return;
	}

    // Clean up the previous thread, if any
    if (state_system_thread.joinable()) {
        state_system_thread.join();
    }

	weak_ptr<State> exp_ref(current_experiment);
	current_experiment->setCurrentState(exp_ref);

    state_system_thread = std::thread([this]() { run(); });

    is_running = true;
	sendSystemStateEvent();

}

void StandardStateSystem::stop(){
    boost::mutex::scoped_lock lock(state_system_mutex);

    if (!is_running) {
        return;
    }

    mprintf("Called stop on state system");
    (*state_system_mode) = STOPPING;

	// TODO: need to stop ongoing schedules...
	// esp. IO devices
    sendSystemStateEvent();
}

void StandardStateSystem::pause(){
    boost::mutex::scoped_lock lock(state_system_mutex);

    if (!is_running || is_paused) {
        return;
    }

    mprintf("Pausing state system");
    is_paused = true;
    (*state_system_mode) = PAUSED;

    sendSystemStateEvent();
}

void StandardStateSystem::resume(){
    boost::mutex::scoped_lock lock(state_system_mutex);

    if (!is_running || !is_paused) {
        return;
    }

    mprintf("Resuming paused state system");
    is_paused = false;
    (*state_system_mode) = RUNNING;

    sendSystemStateEvent();
}

bool StandardStateSystem::isRunning(){
    return is_running;
}

bool StandardStateSystem::isPaused(){
    return is_paused;
}

bool StandardStateSystem::isInAction(){
    return in_action;
}

bool StandardStateSystem::isInTransition(){
    return in_transition;
}

//void StandardStateSystem::setInAction(bool isit){
//    in_action = isit;
//}

//void StandardStateSystem::setInTransition(bool isit){
//    in_transition = isit;
//}

weak_ptr<State> StandardStateSystem::getCurrentState(){
    // Allow access to the current state only on the state system thread
    if (std::this_thread::get_id() == state_system_thread.get_id()) {
        return current_state;
    }
    return weak_ptr<State>();
}

void StandardStateSystem::run() {
    if (!(MachThreadSelf("MWorks State System").setPriority(TaskPriority::Default))) {
        merror(M_SCHEDULER_MESSAGE_DOMAIN, "Failed to set priority of state system thread");
    }

    // Make a copy of the experiment to ensure that it isn't destroyed before we're done with it
    shared_ptr<Experiment> current_experiment = GlobalCurrentExperiment;
    if (!current_experiment) {
        merror(M_STATE_SYSTEM_MESSAGE_DOMAIN,
               "Cannot start state system without a valid experiment defined");
        return;
    }

	mprintf("Starting state system....");

    //mprintf("----------setting task  mode to running------------");
	(*state_system_mode) = (long) RUNNING;
	current_state = current_experiment->getCurrentState();

	if(current_state.expired()){
		merror(M_STATE_SYSTEM_MESSAGE_DOMAIN,
				"current state is NULL. Shutting down state system...");

		(*state_system_mode) = (long)IDLE;
	}

	if(current_state.expired() == true){
		// TODO: better throw
		throw SimpleException("Invalid current state within state system");
	}

	shared_ptr<State> current_state_shared(current_state);
    weak_ptr<State> next_state;

	while (current_state_shared) {
        const bool canInterrupt = current_state_shared->isInterruptible();  // might not be an okay place to stop

        if (canInterrupt &&
            (long(*state_system_mode) == IDLE ||      // hard stop
             long(*state_system_mode) == STOPPING))   // stop requested
        {
            break;
        }
        else if (in_transition ||              // waiting for the next state
                 (canInterrupt && is_paused))  // paused
        {
            getClock()->yield();
            if (canInterrupt && is_paused) {
                continue;
            }
        }

		//mprintf("State system main loop, current state = %d", current_state);
		if (!in_transition) {
			in_action = true;

			//mState *test_state = current_state;

			try {
				current_state_shared->action();
			} catch(std::exception& e){
				merror(M_PARADIGM_MESSAGE_DOMAIN,
					   "Stopping state system: %s", e.what());
				state_system_mode->setValue((long)STOPPING);
				break;
			}

			// finished performing action
			in_action = false;
		}

		if (!in_action) {
			in_transition = true;

			try {
				next_state = current_state_shared->next();
			} catch (std::exception& e){
				merror(M_PARADIGM_MESSAGE_DOMAIN,
					  "Stopping state system: %s", e.what());
				state_system_mode->setValue((long)STOPPING);
				break;
			}

            if (next_state.expired()) {
                // no next state yet, sleep until the next tick
                continue;
            }

            shared_ptr<State> next_state_shared;

            try{
                shared_ptr<State> attempt(next_state); // cast weak_ptr into shared_ptr
                next_state_shared = attempt; // machination required because of weak to shared conversion semantics
			} catch (std::exception& e){
                mwarning(M_STATE_SYSTEM_MESSAGE_DOMAIN, "Failed to acquire shared_ptr from next_state; coming to an abrupt halt");
                (*state_system_mode) = IDLE;
                continue;
            }

			//mprintf("State system moving on... %d", next_state);

			current_experiment->setCurrentState(next_state);

			// If we've finished
			if(current_state_shared.get() == current_experiment.get() &&
					next_state_shared.get() == current_experiment.get()){
					mprintf("Returned to Experiment node, halting state system...");
					(*state_system_mode) = IDLE;
					current_state = weak_ptr<State>();
					next_state = weak_ptr<State>();
					continue;
			}

			current_state = next_state;
			current_state_shared = shared_ptr<State>(current_state);

			next_state = weak_ptr<State>();
			next_state_shared = shared_ptr<State>();

			// finished transition
			in_transition = false;
		}
	}

    {
        boost::mutex::scoped_lock lock(state_system_mutex);

        in_action = false;
        in_transition = false;
        is_running = false;
        is_paused = false;

        (*state_system_mode) = IDLE;
        mprintf("State system ending");

        // DDC: graceful stop?
        mprintf("Resetting experiment");
        current_experiment->reset();
    }
}

END_NAMESPACE_MW

<|endoftext|>


<commit_before>#include <stdio.h>      /* printf */
#include <stdlib.h>
#include <cmath>        // std::abs
#include <vector>
#include <sndfile.hh>

/* 8K chunks vs 1K chunks = about a 4x speedup */
#define		BUFFER_LEN		8192

/* track goes here */
std::vector<float> samples;

static void read_file (const char * fname)
{
  SndfileHandle mySndfileHandle;
  mySndfileHandle = SndfileHandle(fname);

  int channels = mySndfileHandle.channels();
  int frames = mySndfileHandle.frames();

  printf("%d channels, %d frames\n" , channels, frames);

  // We'll load in BUFFER_LEN samples at a time, and stuff them into a buffer
  // We need to ensure that the buffer is long enough to hold all of the
  // Channel data  */
  uint bufferSize = BUFFER_LEN * channels;

  // initialize our read buffer
  float readbuffer[bufferSize];
  int readcount;
  int i;
  int j;
  int readpointer = 0;

  // converts all multichannel files to mono by averaging the channels
  // This is probably not an optimal way to convert to mono
  float monoAverage;

  while ((readcount = mySndfileHandle.readf(readbuffer, 1024))) {
    readpointer = 0;
    for (i = 0; i < readcount; i++) {
      // for each frame...
      monoAverage = 0;
      for(j = 0; j < channels; j++) {
	monoAverage += readbuffer[readpointer + j];
      }
      monoAverage /= channels;
      readpointer += channels;
      // add the averaged sample to our vector of samples
      samples.push_back(monoAverage);
    }
  }

} /* read_file */

void analyze_file() {
  /* attempt to calcuate a "quality" metric for the track.
   *
   * my quick and dirty calculation, aka the working algorithm is:
   *
   *   divide up the track into a number of buckets, around 500mS / bucket
   *   for each bucket, calculate rms for the entire bucket
   *     at the end of the bucket, calculate the rms / "loudness" of the bucket
   *     if loudness > threshold
   *        count bucket as good
   *     reset the per-bucket counter
   *   divide the total number of buckets by the successful buckets.
   *   return this as a score between 0-100%.
   */

  float threshold = 0.75; // Anything above this threshold, we consider to be "good"

  int bucket_size;
  int bucket_pos;
  int buckets_good = 0;
  float this_bucket_total = 0;

  unsigned long buckets;

  printf("samples: %ld\n", samples.size());

  /* 24000 samples = 500 mS @ 44.KHz */
  bucket_size = 24000;
  bucket_pos = 0;
  buckets = samples.size() / bucket_size;
  printf("buckets: %ld\n", buckets);

  // for all samples
  for (int i=0; i < samples.size(); i++) {

    this_bucket_total += std::abs(samples[i]) * std::abs(samples[i]);
    bucket_pos++;

    if (bucket_pos > bucket_size) {
      float rms = sqrt((this_bucket_total / bucket_size));

      //      printf("rms: %f\n", rms);

      if (rms > 0.15) {
	buckets_good++;
      }

      /* reset */
      bucket_pos = 0;
      this_bucket_total = 0;

    }

  }
  printf("   good: %i\n    pct: %.02f %%\n" , buckets_good, ( (float)buckets_good/buckets ) * 100);

};

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

  if (argc != 2) {
    printf("\nUsage: %s soundfile\n", argv[0]);
    exit(1);
  }

  read_file (argv[1]) ;
  analyze_file();

  puts ("Done.\n") ;
  return 0 ;
} /* main */

<commit_msg>global cleanups<commit_after>#include <stdio.h>      /* printf */
#include <stdlib.h>
#include <cmath>        // std::abs
#include <vector>
#include <sndfile.hh>

/* 8K chunks vs 1K chunks = about a 4x speedup */

#define		BUFFER_LEN		8192

float threshold = 0.75; // Anything above this threshold, we consider to be "good"
int  bucket_size = 24000; // number of samples per bucket, should really be derived from file...

/* track goes here */
std::vector<float> samples;

static void read_file (const char * fname)
{
  SndfileHandle mySndfileHandle;
  mySndfileHandle = SndfileHandle(fname);

  int channels = mySndfileHandle.channels();
  int frames = mySndfileHandle.frames();

  printf("%d channels, %d frames\n" , channels, frames);

  // We'll load in BUFFER_LEN samples at a time, and stuff them into a buffer
  // We need to ensure that the buffer is long enough to hold all of the
  // Channel data  */
  uint bufferSize = BUFFER_LEN * channels;

  // initialize our read buffer
  float readbuffer[bufferSize];
  int readcount;
  int i;
  int j;
  int readpointer = 0;

  // converts all multichannel files to mono by averaging the channels
  // This is probably not an optimal way to convert to mono
  float monoAverage;

  while ((readcount = mySndfileHandle.readf(readbuffer, 1024))) {
    readpointer = 0;
    for (i = 0; i < readcount; i++) {
      // for each frame...
      monoAverage = 0;
      for(j = 0; j < channels; j++) {
	monoAverage += readbuffer[readpointer + j];
      }
      monoAverage /= channels;
      readpointer += channels;
      // add the averaged sample to our vector of samples
      samples.push_back(monoAverage);
    }
  }

} /* read_file */

void analyze_file() {
  /* attempt to calcuate a "quality" metric for the track.
   *
   * my quick and dirty calculation, aka the working algorithm is:
   *
   *   divide up the track into a number of buckets, around 500mS / bucket
   *   for each bucket, calculate rms for the entire bucket
   *     at the end of the bucket, calculate the rms / "loudness" of the bucket
   *     if loudness > threshold
   *        count bucket as good
   *     reset the per-bucket counter
   *   divide the total number of buckets by the successful buckets.
   *   return this as a score between 0-100%.
   */

  int bucket_pos;
  int buckets_good = 0;
  float this_bucket_total = 0;

  unsigned long buckets;

  printf("samples: %ld\n", samples.size());

  /* 24000 samples = 500 mS @ 44.KHz */
  bucket_pos = 0;
  buckets = samples.size() / bucket_size;
  printf("buckets: %ld\n", buckets);

  // for all samples
  for (int i=0; i < samples.size(); i++) {

    this_bucket_total += std::abs(samples[i]) * std::abs(samples[i]);
    bucket_pos++;

    if (bucket_pos > bucket_size) {
      float rms = sqrt((this_bucket_total / bucket_size));

      //      printf("rms: %f\n", rms);

      if (rms > threshold) {
	buckets_good++;
      }

      /* reset */
      bucket_pos = 0;
      this_bucket_total = 0;

    }

  }
  printf("   good: %i\n    pct: %.02f %%\n" , buckets_good, ( (float)buckets_good/buckets ) * 100);

};

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

  if (argc != 2) {
    printf("\nUsage: %s soundfile\n", argv[0]);
    exit(1);
  }

  read_file (argv[1]) ;
  analyze_file();

  puts ("Done.\n") ;
  return 0 ;
} /* main */

<|endoftext|>


<commit_before>/*
 * bacteria-core, core for cellular automaton
 * Copyright (C) 2016 Pavel Dolgov
 *
 * See the LICENSE file for terms of use.
 */

#include "Model.hpp"
#include "random.hpp"

namespace Abstract {

Model::Model(
    int /*width*/,
    int /*height*/,
    int /*bacteria*/,
    int /*teams*/
) {
}

Point::Point() {
}

Point::Point(
    int x,
    int y
)
    : x(x)
    , y(y) {
}

bool Point::operator==(const Point& p) const {
    return (p.x == x) && (p.y == y);
}

void Model::clearBeforeMove(int team) {
    return clearBeforeMove_impl(team);
}

CellState Model::cellState(const Point& coordinates) const {
    return cellState_impl(coordinates);
}

int Model::getDirectionByCoordinates(
    const Point& coordinates
) const {
    return getDirectionByCoordinates_impl(coordinates);
}

int Model::getMassByCoordinates(
    const Point& coordinates
) const {
    return getMassByCoordinates_impl(coordinates);
}

int Model::getTeamByCoordinates(
    const Point& coordinates
) const {
    return getTeamByCoordinates_impl(coordinates);
}

int Model::getWidth() const {
    return getWidth_impl();
}

int Model::getHeight() const {
    return getHeight_impl();
}

int Model::getBacteriaNumber(int team) const {
    return getBacteriaNumber_impl(team);
}

bool Model::isAlive(int team, int bacterium_index) const {
    return isAlive_impl(team, bacterium_index);
}

int Model::getInstruction(int team, int bacterium_index) const {
    return getInstruction_impl(team, bacterium_index);
}

Point Model::getCoordinates(
    int team,
    int bacterium_index
) const {
    return getCoordinates_impl(team, bacterium_index);
}

int Model::getDirection(int team, int bacterium_index) const {
    return getDirection_impl(team, bacterium_index);
}

int Model::getMass(int team, int bacterium_index) const {
    return getMass_impl(team, bacterium_index);
}

void Model::kill(int team, int bacterium_index) {
    return kill_impl(team, bacterium_index);
}

void Model::changeMass(int team, int bacterium_index, int change) {
    return changeMass_impl(team, bacterium_index, change);
}

void Model::setDirection(
    int team,
    int bacterium_index,
    int new_direction
) {
    return setDirection_impl(
        team,
        bacterium_index,
        new_direction
    );
}

void Model::setInstruction(
    int team,
    int bacterium_index,
    int new_instruction
) {
    return setInstruction_impl(
        team,
        bacterium_index,
        new_instruction
    );
}

void Model::setCoordinates(
    int team,
    int bacterium_index,
    const Point& coordinates
) {
    return setCoordinates_impl(
        team,
        bacterium_index,
        coordinates
    );
}

void Model::killByCoordinates(
    const Point& coordinates
) {
    return killByCoordinates_impl(coordinates);
}

void Model::changeMassByCoordinates(
    const Point& coordinates,
    int change
) {
    return changeMassByCoordinates_impl(coordinates, change);
}

void Model::createNewByCoordinates(
    const Point& coordinates,
    int mass,
    int direction,
    int team,
    int instruction
) {
    return createNewByCoordinates_impl(
        coordinates,
        mass,
        direction,
        team,
        instruction
    );
}

}

namespace Implementation {

static bool isNull(UnitPtr unit_ptr) {
    return unit_ptr.isNull();
}

static bool checkIndex(int index, int size) {
    return (index >= 0) && (index < size);
}

/* get global coordinate from horizontal and
   vertical coordinates
*/
static int getIndex(
    const Abstract::Point& coordinates,
    int width,
    int height
) {
    bool less = ((coordinates.x < 0) || (coordinates.y < 0));
    bool greater = ((coordinates.x >= width) ||
                    (coordinates.y >= height));
    if (less || greater) {
        throw Exception("Model: index of cell in arguments "
                        "of some methods is out of range.");
    }
    int index = coordinates.y * width + coordinates.x;
    return index;
}

Unit::Unit(
    const Abstract::Point& coordinates,
    int mass,
    int direction,
    int team,
    int instruction
)
    : coordinates(coordinates)
    , mass(mass)
    , direction(direction)
    , team(team)
    , instruction(instruction) {
}

Model::Model(
    int width,
    int height,
    int bacteria,
    int teams
)
    : Abstract::Model(width, height, bacteria, teams)
    , width_(width)
    , height_(height) {
    board_.resize(width * height);
    teams_.resize(teams);
    initializeBoard(bacteria, teams);
}

void Model::clearBeforeMove_impl(int team) {
    Units::iterator begin = teams_[team].begin();
    Units::iterator end = teams_[team].end();
    teams_[team].erase(std::remove_if(begin, end, isNull), end);
}

Abstract::CellState Model::cellState_impl(
    const Abstract::Point& coordinates
) const {
    int index = getIndex(coordinates, width_, height_);
    UnitPtr unit_ptr = board_[index];
    if (!unit_ptr.isNull()) {
        return Abstract::BACTERIUM;
    } else {
        return Abstract::EMPTY;
    }
}

int Model::getDirectionByCoordinates_impl(
    const Abstract::Point& coordinates
) const {
    int index = getIndex(coordinates, width_, height_);
    UnitPtr unit_ptr = board_[index];
    if (!unit_ptr.isNull()) {
        return unit_ptr->direction;
    } else {
        throw Exception("Error: Attempt to get direction of empty cell.");
    }
}

int Model::getMassByCoordinates_impl(
    const Abstract::Point& coordinates
) const {
    int index = getIndex(coordinates, width_, height_);
    UnitPtr unit_ptr = board_[index];
    if (!unit_ptr.isNull()) {
        return unit_ptr->mass;
    } else {
        throw Exception("Error: Attempt to get mass of empty cell.");
    }
}

int Model::getTeamByCoordinates_impl(
    const Abstract::Point& coordinates
) const {
    int index = getIndex(coordinates, width_, height_);
    UnitPtr unit_ptr = board_[index];
    if (!unit_ptr.isNull()) {
        return unit_ptr->team;
    } else {
        // no unit in the current cell
        throw Exception("Error: Attempt to get team of empty cell.");
    }
}

int Model::getWidth_impl() const {
    return width_;
}

int Model::getHeight_impl() const {
    return height_;
}

int Model::getBacteriaNumber_impl(int team) const {
    if (!checkIndex(team, teams_.size())) {
        throw Exception("Model: team argument of "
                        "getBacteriaNumber() method is out of "
                        "allowable range.");
    }
    return teams_[team].size();
}

bool Model::isAlive_impl(
    int team,
    int bacterium_index
) const {
    checkParams(team, bacterium_index, "isAlive()", false);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    return !unit_ptr.isNull();
}

int Model::getInstruction_impl(
    int team,
    int bacterium_index
) const {
    checkParams(team, bacterium_index, "getInstruction()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    return unit_ptr->instruction;
}

Abstract::Point Model::getCoordinates_impl(
    int team,
    int bacterium_index
) const {
    checkParams(team, bacterium_index, "getCoordinates()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    Abstract::Point coordinates = unit_ptr->coordinates;
    return coordinates;
}

int Model::getDirection_impl(int team, int bacterium_index) const {
    checkParams(team, bacterium_index, "getDirection()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    return unit_ptr->direction;
}

int Model::getMass_impl(int team, int bacterium_index) const {
    checkParams(team, bacterium_index, "getMass()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    return unit_ptr->mass;
}

void Model::kill_impl(
    int team,
    int bacterium_index
) {
    checkParams(team, bacterium_index, "kill()", true);
    Abstract::Point coordinates =
        teams_[team][bacterium_index]->coordinates;
    teams_[team][bacterium_index] = UnitPtr(0);
    int index = getIndex(coordinates, width_, height_);
    board_[index] = UnitPtr(0);
}

void Model::changeMass_impl(
    int team,
    int bacterium_index,
    int change
) {
    checkParams(team, bacterium_index, "changeMass()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    unit_ptr->mass += change;
}

void Model::setDirection_impl(
    int team,
    int bacterium_index,
    int new_direction
) {
    checkParams(team, bacterium_index, "setDirection()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    unit_ptr->direction = new_direction;
}

void Model::setInstruction_impl(
    int team,
    int bacterium_index,
    int new_instruction
) {
    checkParams(team, bacterium_index, "setInstruction()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    unit_ptr->instruction = new_instruction;
}

void Model::setCoordinates_impl(
    int team,
    int bacterium_index,
    const Abstract::Point& coordinates
) {
    checkParams(team, bacterium_index, "setCoordinates()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    Abstract::Point prev_coordinates = unit_ptr->coordinates;
    int prev_index = getIndex(prev_coordinates, width_, height_);
    int new_index = getIndex(coordinates, width_, height_);
    board_[prev_index] = UnitPtr(0);
    board_[new_index] = unit_ptr;
    unit_ptr->coordinates = coordinates;
}

void Model::killByCoordinates_impl(
    const Abstract::Point& coordinates
) {
    int index = getIndex(coordinates, width_, height_);
    UnitPtr murdered = board_[index];
    int team = murdered->team;
    board_[index] = UnitPtr(0);
    Units::iterator for_kill = std::find(
        teams_[team].begin(),
        teams_[team].end(),
        murdered
    );
    *for_kill = UnitPtr(0);
}

void Model::changeMassByCoordinates_impl(
    const Abstract::Point& coordinates,
    int change
) {
    int index = getIndex(coordinates, width_, height_);
    UnitPtr unit_ptr = board_[index];
    if (!unit_ptr.isNull()) {
        unit_ptr->mass += change;
    } else {
        throw Exception("Error: Attempt to change mass of empty cell.");
    }
}

void Model::createNewByCoordinates_impl(
    const Abstract::Point& coordinates,
    int mass,
    int direction,
    int team,
    int instruction
) {
    UnitPtr unit_ptr(new Unit(
        coordinates,
        mass,
        direction,
        team,
        instruction
    ));
    teams_[team].push_back(unit_ptr);
    int index = getIndex(coordinates, width_, height_);
    board_[index] = unit_ptr;
}

void Model::initializeBoard(int bacteria, int teams) {
    for (int team = 0; team < teams; team++) {
        for (int bacterium = 0; bacterium < bacteria; bacterium++) {
            tryToPlace(team);
        }
    }
}

void Model::tryToPlace(int team) {
    int x = random(width_);
    int y = random(height_);
    while (cellState(Abstract::Point(x, y)) != Abstract::EMPTY) {
        x = random(width_);
        y = random(height_);
    }
    int direction = random(4);
    UnitPtr unit_ptr(new Unit(
        Abstract::Point(x, y),
        DEFAULT_MASS,
        direction,
        team,
        0
    ));
    teams_[team].push_back(unit_ptr);
    int index = getIndex(Abstract::Point(x, y), width_, height_);
    board_[index] = unit_ptr;
}

void Model::checkParams(
    int team,
    int bacterium_index,
    const char* method_name,
    bool check_alive
) const {
#define TO_S std::string
    if (!checkIndex(team, teams_.size())) {
        throw Exception(
            "Model: team argument of " + TO_S(method_name) +
            " is out of allowable range."
        );
    }
    if (!checkIndex(bacterium_index, teams_[team].size())) {
        throw Exception(
            "Model: bacterium_index argument of " +
            TO_S(method_name) + " is out of allowable range"
        );
    }
    if (check_alive) {
        UnitPtr unit_ptr = teams_[team][bacterium_index];
        if (unit_ptr.isNull()) {
            throw Exception(
                "Model: Attempt to call " + TO_S(method_name) +
                " with NULL ptr."
            );
        }
    }
#undef TO_S
}

}
<commit_msg>Model: increase dead_bacteria_ in kill() methods<commit_after>/*
 * bacteria-core, core for cellular automaton
 * Copyright (C) 2016 Pavel Dolgov
 *
 * See the LICENSE file for terms of use.
 */

#include "Model.hpp"
#include "random.hpp"

namespace Abstract {

Model::Model(
    int /*width*/,
    int /*height*/,
    int /*bacteria*/,
    int /*teams*/
) {
}

Point::Point() {
}

Point::Point(
    int x,
    int y
)
    : x(x)
    , y(y) {
}

bool Point::operator==(const Point& p) const {
    return (p.x == x) && (p.y == y);
}

void Model::clearBeforeMove(int team) {
    return clearBeforeMove_impl(team);
}

CellState Model::cellState(const Point& coordinates) const {
    return cellState_impl(coordinates);
}

int Model::getDirectionByCoordinates(
    const Point& coordinates
) const {
    return getDirectionByCoordinates_impl(coordinates);
}

int Model::getMassByCoordinates(
    const Point& coordinates
) const {
    return getMassByCoordinates_impl(coordinates);
}

int Model::getTeamByCoordinates(
    const Point& coordinates
) const {
    return getTeamByCoordinates_impl(coordinates);
}

int Model::getWidth() const {
    return getWidth_impl();
}

int Model::getHeight() const {
    return getHeight_impl();
}

int Model::getBacteriaNumber(int team) const {
    return getBacteriaNumber_impl(team);
}

bool Model::isAlive(int team, int bacterium_index) const {
    return isAlive_impl(team, bacterium_index);
}

int Model::getInstruction(int team, int bacterium_index) const {
    return getInstruction_impl(team, bacterium_index);
}

Point Model::getCoordinates(
    int team,
    int bacterium_index
) const {
    return getCoordinates_impl(team, bacterium_index);
}

int Model::getDirection(int team, int bacterium_index) const {
    return getDirection_impl(team, bacterium_index);
}

int Model::getMass(int team, int bacterium_index) const {
    return getMass_impl(team, bacterium_index);
}

void Model::kill(int team, int bacterium_index) {
    return kill_impl(team, bacterium_index);
}

void Model::changeMass(int team, int bacterium_index, int change) {
    return changeMass_impl(team, bacterium_index, change);
}

void Model::setDirection(
    int team,
    int bacterium_index,
    int new_direction
) {
    return setDirection_impl(
        team,
        bacterium_index,
        new_direction
    );
}

void Model::setInstruction(
    int team,
    int bacterium_index,
    int new_instruction
) {
    return setInstruction_impl(
        team,
        bacterium_index,
        new_instruction
    );
}

void Model::setCoordinates(
    int team,
    int bacterium_index,
    const Point& coordinates
) {
    return setCoordinates_impl(
        team,
        bacterium_index,
        coordinates
    );
}

void Model::killByCoordinates(
    const Point& coordinates
) {
    return killByCoordinates_impl(coordinates);
}

void Model::changeMassByCoordinates(
    const Point& coordinates,
    int change
) {
    return changeMassByCoordinates_impl(coordinates, change);
}

void Model::createNewByCoordinates(
    const Point& coordinates,
    int mass,
    int direction,
    int team,
    int instruction
) {
    return createNewByCoordinates_impl(
        coordinates,
        mass,
        direction,
        team,
        instruction
    );
}

}

namespace Implementation {

static bool isNull(UnitPtr unit_ptr) {
    return unit_ptr.isNull();
}

static bool checkIndex(int index, int size) {
    return (index >= 0) && (index < size);
}

/* get global coordinate from horizontal and
   vertical coordinates
*/
static int getIndex(
    const Abstract::Point& coordinates,
    int width,
    int height
) {
    bool less = ((coordinates.x < 0) || (coordinates.y < 0));
    bool greater = ((coordinates.x >= width) ||
                    (coordinates.y >= height));
    if (less || greater) {
        throw Exception("Model: index of cell in arguments "
                        "of some methods is out of range.");
    }
    int index = coordinates.y * width + coordinates.x;
    return index;
}

Unit::Unit(
    const Abstract::Point& coordinates,
    int mass,
    int direction,
    int team,
    int instruction
)
    : coordinates(coordinates)
    , mass(mass)
    , direction(direction)
    , team(team)
    , instruction(instruction) {
}

Model::Model(
    int width,
    int height,
    int bacteria,
    int teams
)
    : Abstract::Model(width, height, bacteria, teams)
    , width_(width)
    , height_(height) {
    board_.resize(width * height);
    teams_.resize(teams);
    dead_bacteria_.resize(teams, 0);
    initializeBoard(bacteria, teams);
}

void Model::clearBeforeMove_impl(int team) {
    Units::iterator begin = teams_[team].begin();
    Units::iterator end = teams_[team].end();
    teams_[team].erase(std::remove_if(begin, end, isNull), end);
    dead_bacteria_[team] = 0;
}

Abstract::CellState Model::cellState_impl(
    const Abstract::Point& coordinates
) const {
    int index = getIndex(coordinates, width_, height_);
    UnitPtr unit_ptr = board_[index];
    if (!unit_ptr.isNull()) {
        return Abstract::BACTERIUM;
    } else {
        return Abstract::EMPTY;
    }
}

int Model::getDirectionByCoordinates_impl(
    const Abstract::Point& coordinates
) const {
    int index = getIndex(coordinates, width_, height_);
    UnitPtr unit_ptr = board_[index];
    if (!unit_ptr.isNull()) {
        return unit_ptr->direction;
    } else {
        throw Exception("Error: Attempt to get direction of empty cell.");
    }
}

int Model::getMassByCoordinates_impl(
    const Abstract::Point& coordinates
) const {
    int index = getIndex(coordinates, width_, height_);
    UnitPtr unit_ptr = board_[index];
    if (!unit_ptr.isNull()) {
        return unit_ptr->mass;
    } else {
        throw Exception("Error: Attempt to get mass of empty cell.");
    }
}

int Model::getTeamByCoordinates_impl(
    const Abstract::Point& coordinates
) const {
    int index = getIndex(coordinates, width_, height_);
    UnitPtr unit_ptr = board_[index];
    if (!unit_ptr.isNull()) {
        return unit_ptr->team;
    } else {
        // no unit in the current cell
        throw Exception("Error: Attempt to get team of empty cell.");
    }
}

int Model::getWidth_impl() const {
    return width_;
}

int Model::getHeight_impl() const {
    return height_;
}

int Model::getBacteriaNumber_impl(int team) const {
    if (!checkIndex(team, teams_.size())) {
        throw Exception("Model: team argument of "
                        "getBacteriaNumber() method is out of "
                        "allowable range.");
    }
    return teams_[team].size();
}

bool Model::isAlive_impl(
    int team,
    int bacterium_index
) const {
    checkParams(team, bacterium_index, "isAlive()", false);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    return !unit_ptr.isNull();
}

int Model::getInstruction_impl(
    int team,
    int bacterium_index
) const {
    checkParams(team, bacterium_index, "getInstruction()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    return unit_ptr->instruction;
}

Abstract::Point Model::getCoordinates_impl(
    int team,
    int bacterium_index
) const {
    checkParams(team, bacterium_index, "getCoordinates()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    Abstract::Point coordinates = unit_ptr->coordinates;
    return coordinates;
}

int Model::getDirection_impl(int team, int bacterium_index) const {
    checkParams(team, bacterium_index, "getDirection()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    return unit_ptr->direction;
}

int Model::getMass_impl(int team, int bacterium_index) const {
    checkParams(team, bacterium_index, "getMass()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    return unit_ptr->mass;
}

void Model::kill_impl(
    int team,
    int bacterium_index
) {
    checkParams(team, bacterium_index, "kill()", true);
    Abstract::Point coordinates =
        teams_[team][bacterium_index]->coordinates;
    teams_[team][bacterium_index] = UnitPtr(0);
    dead_bacteria_[team]++;
    int index = getIndex(coordinates, width_, height_);
    board_[index] = UnitPtr(0);
}

void Model::changeMass_impl(
    int team,
    int bacterium_index,
    int change
) {
    checkParams(team, bacterium_index, "changeMass()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    unit_ptr->mass += change;
}

void Model::setDirection_impl(
    int team,
    int bacterium_index,
    int new_direction
) {
    checkParams(team, bacterium_index, "setDirection()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    unit_ptr->direction = new_direction;
}

void Model::setInstruction_impl(
    int team,
    int bacterium_index,
    int new_instruction
) {
    checkParams(team, bacterium_index, "setInstruction()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    unit_ptr->instruction = new_instruction;
}

void Model::setCoordinates_impl(
    int team,
    int bacterium_index,
    const Abstract::Point& coordinates
) {
    checkParams(team, bacterium_index, "setCoordinates()", true);
    UnitPtr unit_ptr = teams_[team][bacterium_index];
    Abstract::Point prev_coordinates = unit_ptr->coordinates;
    int prev_index = getIndex(prev_coordinates, width_, height_);
    int new_index = getIndex(coordinates, width_, height_);
    board_[prev_index] = UnitPtr(0);
    board_[new_index] = unit_ptr;
    unit_ptr->coordinates = coordinates;
}

void Model::killByCoordinates_impl(
    const Abstract::Point& coordinates
) {
    int index = getIndex(coordinates, width_, height_);
    UnitPtr murdered = board_[index];
    int team = murdered->team;
    board_[index] = UnitPtr(0);
    Units::iterator for_kill = std::find(
        teams_[team].begin(),
        teams_[team].end(),
        murdered
    );
    *for_kill = UnitPtr(0);
    dead_bacteria_[team]++;
}

void Model::changeMassByCoordinates_impl(
    const Abstract::Point& coordinates,
    int change
) {
    int index = getIndex(coordinates, width_, height_);
    UnitPtr unit_ptr = board_[index];
    if (!unit_ptr.isNull()) {
        unit_ptr->mass += change;
    } else {
        throw Exception("Error: Attempt to change mass of empty cell.");
    }
}

void Model::createNewByCoordinates_impl(
    const Abstract::Point& coordinates,
    int mass,
    int direction,
    int team,
    int instruction
) {
    UnitPtr unit_ptr(new Unit(
        coordinates,
        mass,
        direction,
        team,
        instruction
    ));
    teams_[team].push_back(unit_ptr);
    int index = getIndex(coordinates, width_, height_);
    board_[index] = unit_ptr;
}

void Model::initializeBoard(int bacteria, int teams) {
    for (int team = 0; team < teams; team++) {
        for (int bacterium = 0; bacterium < bacteria; bacterium++) {
            tryToPlace(team);
        }
    }
}

void Model::tryToPlace(int team) {
    int x = random(width_);
    int y = random(height_);
    while (cellState(Abstract::Point(x, y)) != Abstract::EMPTY) {
        x = random(width_);
        y = random(height_);
    }
    int direction = random(4);
    UnitPtr unit_ptr(new Unit(
        Abstract::Point(x, y),
        DEFAULT_MASS,
        direction,
        team,
        0
    ));
    teams_[team].push_back(unit_ptr);
    int index = getIndex(Abstract::Point(x, y), width_, height_);
    board_[index] = unit_ptr;
}

void Model::checkParams(
    int team,
    int bacterium_index,
    const char* method_name,
    bool check_alive
) const {
#define TO_S std::string
    if (!checkIndex(team, teams_.size())) {
        throw Exception(
            "Model: team argument of " + TO_S(method_name) +
            " is out of allowable range."
        );
    }
    if (!checkIndex(bacterium_index, teams_[team].size())) {
        throw Exception(
            "Model: bacterium_index argument of " +
            TO_S(method_name) + " is out of allowable range"
        );
    }
    if (check_alive) {
        UnitPtr unit_ptr = teams_[team][bacterium_index];
        if (unit_ptr.isNull()) {
            throw Exception(
                "Model: Attempt to call " + TO_S(method_name) +
                " with NULL ptr."
            );
        }
    }
#undef TO_S
}

}
<|endoftext|>


<commit_before>#include "stereo_slam_base.h"
#include <boost/shared_ptr.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <libpq-fe.h>
#include <Eigen/Geometry>
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/nonfree/features2d.hpp"
#include "postgresql_interface.h"
#include "utils.h"

/** \brief Messages callback. This function is called when syncronized odometry and image
  * message are received.
  * @return
  * \param l_ptr pointer to left image
  * \param r_ptr pointer to right image
  * \param current_pose from visual odometry
  * \param corrected_pose pose corrected by the graph
  * \param timestamp message timestamp
  */
bool stereo_slam::StereoSlamBase::graphUpdater()
{
  // True when new edges to force optimization
  bool edge_added = false;

  // Find possible candidates for loop-closing
  unsigned int graph_size = graph_optimizer_.vertices().size();
  for (unsigned int i=0; i<graph_size; i++)
  {
    // Extract the pose of vertex i
    g2o::VertexSE3* v_i = dynamic_cast<g2o::VertexSE3*>(graph_optimizer_.vertices()[i]);
    tf::Transform pose_i = stereo_slam::Utils::getVertexPose(v_i);

    for (unsigned int j=i+params_.neighbor_offset; j<graph_size; j++)
    {
      // Extract the pose of vertex j and compare
      g2o::VertexSE3* v_j = dynamic_cast<g2o::VertexSE3*>(graph_optimizer_.vertices()[j]);
      tf::Transform pose_j = stereo_slam::Utils::getVertexPose(v_j);

      // Check if this have been discarted previously
      bool false_cand = stereo_slam::Utils::searchFalseCandidates(false_candidates_, v_i->id(), v_j->id());

      // Proceed if this combination is possible
      if (!false_cand)
      {
        // Check if there is an edge that currently join both vertices
        bool edge_found = false;
        for (g2o::OptimizableGraph::EdgeSet::iterator it=graph_optimizer_.edges().begin();
         it!=graph_optimizer_.edges().end(); it++)
        {
          g2o::EdgeSE3* e = dynamic_cast<g2o::EdgeSE3*> (*it);
          if (e)
          {
            if (  (e->vertices()[0]->id() == v_i->id() && e->vertices()[1]->id() == v_j->id()) ||
                  (e->vertices()[0]->id() == v_j->id() && e->vertices()[1]->id() == v_i->id()) )
            {
              edge_found = true;
              break;
            }
          }
        }

        // If no edges found connecting this vertices, try to find loop closures
        bool is_false = true;
        double pose_diff = stereo_slam::Utils::poseDiff(pose_i, pose_j);
        if (!edge_found && (pose_diff < params_.max_candidate_threshold))
        {
          // Get the data of both vertices from database
          std::string where_i = "(id = " + boost::lexical_cast<std::string>(v_i->id() + 1) + ")";
          std::string where_j = "(id = " + boost::lexical_cast<std::string>(v_j->id() + 1) + ")";
          std::vector< boost::shared_ptr<stereo_slam::GraphDB> > vert_i;
          std::vector< boost::shared_ptr<stereo_slam::GraphDB> > vert_j;
          pg_db_ptr_thread_2_->getList(vert_i, where_i);
          pg_db_ptr_thread_2_->getList(vert_j, where_j);
          if (vert_i.size() == 1 && vert_j.size() == 1)
          {
            cv::Mat desc_i = cv::Mat_<std::vector<float> >();
            cv::Mat desc_j = cv::Mat_<std::vector<float> >();
            desc_i = stereo_slam::Utils::stdMatrixToCvMat(vert_i[0]->descriptors_.data());
            desc_j = stereo_slam::Utils::stdMatrixToCvMat(vert_j[0]->descriptors_.data());

            // Check for enought descriptors
            if (desc_i.rows > params_.matches_threshold && desc_j.rows > params_.matches_threshold)
            {
              // Compute matchings
              std::vector<cv::DMatch> matches;
              stereo_slam::Utils::thresholdMatching(desc_i, desc_j, matches, params_.descriptor_threshold);

              if (params_.stereo_vision_verbose)
                ROS_INFO_STREAM("[StereoSlam:] Found " << matches.size() <<
                   " matches between vertices " << v_i->id() << " and " << v_j->id() <<
                   " (matches_threshold is: " << params_.matches_threshold << ")");

              if ((int)matches.size() > params_.matches_threshold)
              {
                // Extract keypoints and 3d points of vertex i and j
                std::vector<cv::Point2f> keypoints_j;
                std::vector<cv::Point3f> points3d_i;
                keypoints_j = stereo_slam::Utils::stdMatrixToCvPoint2f(vert_j[0]->keypoints_.data());
                points3d_i = stereo_slam::Utils::stdMatrixToCvPoint3f(vert_i[0]->points3d_.data());
                std::vector<cv::Point2f> matched_keypoints;
                std::vector<cv::Point3f> matched_3d_points;
                for (size_t i = 0; i < matches.size(); ++i)
                {
                  int index_left = matches[i].queryIdx;
                  int index_right = matches[i].trainIdx;;
                  matched_3d_points.push_back(points3d_i[index_left]);
                  matched_keypoints.push_back(keypoints_j[index_right]);
                }

                // Compute the transformation between the vertices
                cv::Mat rvec, tvec;
                std::vector<int> inliers;
                cv::solvePnPRansac(matched_3d_points, matched_keypoints, camera_matrix_,
                                   cv::Mat(), rvec, tvec, false,
                                   params_.max_solvepnp_iter, params_.allowed_reprojection_err,
                                   params_.max_inliers, inliers);

                if (params_.stereo_vision_verbose)
                  ROS_INFO_STREAM("[StereoSlam:] Found " << inliers.size() <<
                   " inliers between vertices " << v_i->id() << " and " << v_j->id() <<
                   " (min_inliers is: " << params_.min_inliers << ")");

                if (static_cast<int>(inliers.size()) >= params_.min_inliers)
                {
                  // Good! Loop closure, get the transformation matrix
                  tf::Transform cl_edge = stereo_slam::Utils::buildTransformation(rvec, tvec);

                  // To prevent for possible errors, compare previous transform with the new edge found
                  Eigen::Isometry3d t = v_i->estimate().inverse() * v_j->estimate();
                  tf::Transform cl_edge_prev = stereo_slam::Utils::eigenToTf(t);

                  // Check edge geometry
                  if (stereo_slam::Utils::poseDiff(cl_edge, cl_edge_prev) < params_.max_edge_err)
                  {
                    // Add the new edge to graph
                    g2o::EdgeSE3* e = new g2o::EdgeSE3();
                    Eigen::Isometry3d t = stereo_slam::Utils::tfToEigen(cl_edge);
                    e->setVertex(0, v_j);
                    e->setVertex(1, v_i);
                    e->setMeasurement(t);
                    graph_optimizer_.addEdge(e);
                    edge_added = true;
                    is_false = false;

                    ROS_INFO_STREAM("[StereoSlam:] Loop closed between vertices " << v_i->id() << " and " << v_j->id());
                  }
                }
              }
            }
          }
        }

        // Bad candidate, save to prevent future processes
        if (is_false)
        {
          cv::Point2i vert(v_i->id(), v_j->id());
          false_candidates_.push_back(vert);
        }
      }
    }
  }

  return edge_added;
}<commit_msg>Added new verbose<commit_after>#include "stereo_slam_base.h"
#include <boost/shared_ptr.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <libpq-fe.h>
#include <Eigen/Geometry>
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/nonfree/features2d.hpp"
#include "postgresql_interface.h"
#include "utils.h"

/** \brief Messages callback. This function is called when syncronized odometry and image
  * message are received.
  * @return
  * \param l_ptr pointer to left image
  * \param r_ptr pointer to right image
  * \param current_pose from visual odometry
  * \param corrected_pose pose corrected by the graph
  * \param timestamp message timestamp
  */
bool stereo_slam::StereoSlamBase::graphUpdater()
{
  // True when new edges to force optimization
  bool edge_added = false;

  // Find possible candidates for loop-closing
  unsigned int graph_size = graph_optimizer_.vertices().size();
  for (unsigned int i=0; i<graph_size; i++)
  {
    // Extract the pose of vertex i
    g2o::VertexSE3* v_i = dynamic_cast<g2o::VertexSE3*>(graph_optimizer_.vertices()[i]);
    tf::Transform pose_i = stereo_slam::Utils::getVertexPose(v_i);

    for (unsigned int j=i+params_.neighbor_offset; j<graph_size; j++)
    {
      // Extract the pose of vertex j and compare
      g2o::VertexSE3* v_j = dynamic_cast<g2o::VertexSE3*>(graph_optimizer_.vertices()[j]);
      tf::Transform pose_j = stereo_slam::Utils::getVertexPose(v_j);

      // Check if this have been discarted previously
      bool false_cand = stereo_slam::Utils::searchFalseCandidates(false_candidates_, v_i->id(), v_j->id());

      // Proceed if this combination is possible
      if (!false_cand)
      {
        // Check if there is an edge that currently join both vertices
        bool edge_found = false;
        for (g2o::OptimizableGraph::EdgeSet::iterator it=graph_optimizer_.edges().begin();
         it!=graph_optimizer_.edges().end(); it++)
        {
          g2o::EdgeSE3* e = dynamic_cast<g2o::EdgeSE3*> (*it);
          if (e)
          {
            if (  (e->vertices()[0]->id() == v_i->id() && e->vertices()[1]->id() == v_j->id()) ||
                  (e->vertices()[0]->id() == v_j->id() && e->vertices()[1]->id() == v_i->id()) )
            {
              edge_found = true;
              break;
            }
          }
        }

        // If no edges found connecting this vertices, try to find loop closures
        bool is_false = true;
        double pose_diff = stereo_slam::Utils::poseDiff(pose_i, pose_j);
        if (!edge_found && (pose_diff < params_.max_candidate_threshold))
        {
          // Get the data of both vertices from database
          std::string where_i = "(id = " + boost::lexical_cast<std::string>(v_i->id() + 1) + ")";
          std::string where_j = "(id = " + boost::lexical_cast<std::string>(v_j->id() + 1) + ")";
          std::vector< boost::shared_ptr<stereo_slam::GraphDB> > vert_i;
          std::vector< boost::shared_ptr<stereo_slam::GraphDB> > vert_j;
          pg_db_ptr_thread_2_->getList(vert_i, where_i);
          pg_db_ptr_thread_2_->getList(vert_j, where_j);
          if (vert_i.size() == 1 && vert_j.size() == 1)
          {
            cv::Mat desc_i = cv::Mat_<std::vector<float> >();
            cv::Mat desc_j = cv::Mat_<std::vector<float> >();
            desc_i = stereo_slam::Utils::stdMatrixToCvMat(vert_i[0]->descriptors_.data());
            desc_j = stereo_slam::Utils::stdMatrixToCvMat(vert_j[0]->descriptors_.data());

            // Check for enought descriptors
            if (desc_i.rows > params_.matches_threshold && desc_j.rows > params_.matches_threshold)
            {
              // Compute matchings
              std::vector<cv::DMatch> matches;
              stereo_slam::Utils::thresholdMatching(desc_i, desc_j, matches, params_.descriptor_threshold);

              if (params_.stereo_vision_verbose)
                ROS_INFO_STREAM("[StereoSlam:] Found " << matches.size() <<
                   " matches between vertices " << v_i->id() << " and " << v_j->id() <<
                   " (matches_threshold is: " << params_.matches_threshold << ")");

              if ((int)matches.size() > params_.matches_threshold)
              {
                // Extract keypoints and 3d points of vertex i and j
                std::vector<cv::Point2f> keypoints_j;
                std::vector<cv::Point3f> points3d_i;
                keypoints_j = stereo_slam::Utils::stdMatrixToCvPoint2f(vert_j[0]->keypoints_.data());
                points3d_i = stereo_slam::Utils::stdMatrixToCvPoint3f(vert_i[0]->points3d_.data());
                std::vector<cv::Point2f> matched_keypoints;
                std::vector<cv::Point3f> matched_3d_points;
                for (size_t i = 0; i < matches.size(); ++i)
                {
                  int index_left = matches[i].queryIdx;
                  int index_right = matches[i].trainIdx;;
                  matched_3d_points.push_back(points3d_i[index_left]);
                  matched_keypoints.push_back(keypoints_j[index_right]);
                }

                // Compute the transformation between the vertices
                cv::Mat rvec, tvec;
                std::vector<int> inliers;
                cv::solvePnPRansac(matched_3d_points, matched_keypoints, camera_matrix_,
                                   cv::Mat(), rvec, tvec, false,
                                   params_.max_solvepnp_iter, params_.allowed_reprojection_err,
                                   params_.max_inliers, inliers);

                if (params_.stereo_vision_verbose)
                  ROS_INFO_STREAM("[StereoSlam:] Found " << inliers.size() <<
                   " inliers between vertices " << v_i->id() << " and " << v_j->id() <<
                   " (min_inliers is: " << params_.min_inliers << ")");

                if (static_cast<int>(inliers.size()) >= params_.min_inliers)
                {
                  // Good! Loop closure, get the transformation matrix
                  tf::Transform cl_edge = stereo_slam::Utils::buildTransformation(rvec, tvec);

                  // To prevent for possible errors, compare previous transform with the new edge found
                  Eigen::Isometry3d t = v_i->estimate().inverse() * v_j->estimate();
                  tf::Transform cl_edge_prev = stereo_slam::Utils::eigenToTf(t);

                  double edge_diff = stereo_slam::Utils::poseDiff(cl_edge, cl_edge_prev);
                  if (params_.stereo_vision_verbose)
                    ROS_INFO_STREAM("[StereoSlam:] The error between node " << v_i->id()
                      << " and " << v_j->id() << " is: " << edge_diff <<
                      " (max_edge_err is: " << params_.max_edge_err << ")");

                  // Check edge geometry
                  if (edge_diff < params_.max_edge_err)
                  {
                    // Add the new edge to graph
                    g2o::EdgeSE3* e = new g2o::EdgeSE3();
                    Eigen::Isometry3d t = stereo_slam::Utils::tfToEigen(cl_edge);
                    e->setVertex(0, v_j);
                    e->setVertex(1, v_i);
                    e->setMeasurement(t);
                    graph_optimizer_.addEdge(e);
                    edge_added = true;
                    is_false = false;

                    ROS_INFO_STREAM("[StereoSlam:] Loop closed between vertices " << v_i->id() << " and " << v_j->id());
                  }
                }
              }
            }
          }
        }

        // Bad candidate, save to prevent future processes
        if (is_false)
        {
          cv::Point2i vert(v_i->id(), v_j->id());
          false_candidates_.push_back(vert);
        }
      }
    }
  }

  return edge_added;
}<|endoftext|>