void ALRosALVisionToolbox::bindMethods(AL::ALPtr<AL::ALBroker> pNaoQiBroker, ros::NodeHandle& pRosNode)
{
  if (! pNaoQiBroker->isModulePresent("ALVisionToolbox"))
  {
    return;
  }
  fProxy = ALPtr<ALVisionToolboxProxy> (new ALVisionToolboxProxy(pNaoQiBroker));

  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/backlighting"), &ALRosALVisionToolbox::backlighting, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/exit"), &ALRosALVisionToolbox::exit, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/getBrokerName"), &ALRosALVisionToolbox::getBrokerName, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/getMethodList"), &ALRosALVisionToolbox::getMethodList, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/getUsage"), &ALRosALVisionToolbox::getUsage, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/halfPress"), &ALRosALVisionToolbox::halfPress, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/isItDark"), &ALRosALVisionToolbox::isItDark, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/isVideoRecording"), &ALRosALVisionToolbox::isVideoRecording, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/logTPRInstanceInfo"), &ALRosALVisionToolbox::logTPRInstanceInfo, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/ping"), &ALRosALVisionToolbox::ping, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/setWhiteBalance"), &ALRosALVisionToolbox::setWhiteBalance, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/startVideoRecord"), &ALRosALVisionToolbox::startVideoRecord, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/startVideoRecord_adv"), &ALRosALVisionToolbox::startVideoRecord_adv, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/stopTPR"), &ALRosALVisionToolbox::stopTPR, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/takePicture"), &ALRosALVisionToolbox::takePicture, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/takePictureRegularly"), &ALRosALVisionToolbox::takePictureRegularly, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALVisionToolbox/version"), &ALRosALVisionToolbox::version, this));
  bindCustomMethods(pNaoQiBroker, pRosNode);
}
void ALRosALBehaviorManager::bindMethods(AL::ALPtr<AL::ALBroker> pNaoQiBroker, ros::NodeHandle& pRosNode)
{
  if (! pNaoQiBroker->isModulePresent("ALBehaviorManager"))
  {
    return;
  }
  fProxy = ALPtr<ALBehaviorManagerProxy> (new ALBehaviorManagerProxy(pNaoQiBroker));

  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/addDefaultBehavior"), &ALRosALBehaviorManager::addDefaultBehavior, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/exit"), &ALRosALBehaviorManager::exit, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/getBrokerName"), &ALRosALBehaviorManager::getBrokerName, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/getDefaultBehaviors"), &ALRosALBehaviorManager::getDefaultBehaviors, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/getInstalledBehaviors"), &ALRosALBehaviorManager::getInstalledBehaviors, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/getMethodList"), &ALRosALBehaviorManager::getMethodList, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/getRunningBehaviors"), &ALRosALBehaviorManager::getRunningBehaviors, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/getUsage"), &ALRosALBehaviorManager::getUsage, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/isBehaviorInstalled"), &ALRosALBehaviorManager::isBehaviorInstalled, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/isBehaviorRunning"), &ALRosALBehaviorManager::isBehaviorRunning, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/ping"), &ALRosALBehaviorManager::ping, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/playDefaultProject"), &ALRosALBehaviorManager::playDefaultProject, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/preloadBehavior"), &ALRosALBehaviorManager::preloadBehavior, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/removeBehavior"), &ALRosALBehaviorManager::removeBehavior, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/removeDefaultBehavior"), &ALRosALBehaviorManager::removeDefaultBehavior, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/runBehavior"), &ALRosALBehaviorManager::runBehavior, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/stopAllBehaviors"), &ALRosALBehaviorManager::stopAllBehaviors, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/stopBehavior"), &ALRosALBehaviorManager::stopBehavior, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALBehaviorManager/version"), &ALRosALBehaviorManager::version, this));
  bindCustomMethods(pNaoQiBroker, pRosNode);
}
void ALRosALAudioSourceLocalization::bindMethods(AL::ALPtr<AL::ALBroker> pNaoQiBroker, ros::NodeHandle& pRosNode)
{
  if (! pNaoQiBroker->isModulePresent("ALAudioSourceLocalization"))
  {
    return;
  }
  fProxy = ALPtr<ALAudioSourceLocalizationProxy> (new ALAudioSourceLocalizationProxy(pNaoQiBroker));

  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/exit"), &ALRosALAudioSourceLocalization::exit, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/getBrokerName"), &ALRosALAudioSourceLocalization::getBrokerName, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/getCurrentPeriod"), &ALRosALAudioSourceLocalization::getCurrentPeriod, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/getCurrentPrecision"), &ALRosALAudioSourceLocalization::getCurrentPrecision, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/getMethodList"), &ALRosALAudioSourceLocalization::getMethodList, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/getMyPeriod"), &ALRosALAudioSourceLocalization::getMyPeriod, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/getMyPrecision"), &ALRosALAudioSourceLocalization::getMyPrecision, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/getOutputNames"), &ALRosALAudioSourceLocalization::getOutputNames, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/getUsage"), &ALRosALAudioSourceLocalization::getUsage, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/ping"), &ALRosALAudioSourceLocalization::ping, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/setDebugMode"), &ALRosALAudioSourceLocalization::setDebugMode, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/subscribeString"), &ALRosALAudioSourceLocalization::subscribeString, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/subscribe"), &ALRosALAudioSourceLocalization::subscribe, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/unsubscribe"), &ALRosALAudioSourceLocalization::unsubscribe, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/updatePeriod"), &ALRosALAudioSourceLocalization::updatePeriod, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/updatePrecision"), &ALRosALAudioSourceLocalization::updatePrecision, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioSourceLocalization/version"), &ALRosALAudioSourceLocalization::version, this));
  bindCustomMethods(pNaoQiBroker, pRosNode);
}
Beispiel #4
0
/*************************************************************
 * ANOTHER FUNCTIONS
 *************************************************************/
void HingeEncoders::init ( const std::string newName,
                         AL::ALPtr<AL::ALBroker> parentBroker ) {
    this->name = newName;
    
    try {
        this->motion = parentBroker->getMotionProxy();
    } catch ( AL::ALError& e ) {
		std::cerr << "[HingeEncoders ()::init(): " << e.toString() << std::endl;
	}
}
NaoImageCapture::NaoImageCapture(AL::ALPtr<AL::ALBroker> broker)
  : camera(),
    gvmName(ConfigFile::getSingleton().getString("naoImage/gvmName")),
    cameraResolution(AL::kQVGA), // Override below
    cameraColorSpace(AL::kYUVColorSpace),     // Override below
    cameraFPS(ConfigFile::getSingleton().getInt("naoImage/cameraFPS")),
    isImageClean(true),
    localImage(NULL),
    remoteImage(),
    usingBottomCamera(false),
    setTopCamera(false),
    setBottomCamera(false) {

  // Convert the strings in the config file into Aldebaran values
  std::string resolution = ConfigFile::getSingleton().getString("naoImage/cameraResolution");
  if (resolution == "320x240") {
    cameraResolution = AL::kQVGA;
  }
  else if (resolution == "640x480") {
    cameraResolution = AL::kVGA;
  }
  else if (resolution == "160x120") {
    cameraResolution = AL::kQQVGA;
  }
  // Default resolution
  else {
    cameraResolution = AL::kQVGA;
  }
  std::string colorSpace = ConfigFile::getSingleton().getString("naoImage/cameraColorSpace");
  if (colorSpace == "YUV") {
    cameraColorSpace = AL::kYUVColorSpace;
  }
  else if (colorSpace == "YUV422") {
    cameraColorSpace = AL::kYUV422InterlacedColorSpace;
  }

  //Create a proxy to the video input module
  try {
    camera = broker->getProxy("ALVideoDevice");
    LOG_INFO("Created a proxy to ALVideoDevice.")
  }
  catch(const AL::ALError &error) {
    LOG_ERROR("Could not create a proxy to ALVideoDevice.\n%s", error.toString().c_str())
  }

  registerToVIM();

  setCameraParameters();

  // Setting of camera parameters may have switched cameras, so we set it to
  // the default
  setCamera(ConfigFile::getSingleton().getInt("behaviors/defaultUseBottomCamera"));

  remoteImage.arraySetSize(7);
}
int
_createModule(AL::ALPtr<AL::ALBroker> broker)
{      
  // init broker with the main broker inctance 
  // from the parent executable

  AL::ALPtr<AL::ALLoggerProxy> logger = broker->getLoggerProxy();

  logger->info("NaoQiFawkes", "*** Setting broker stuff");
  AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock());
  AL::ALBrokerManager::getInstance()->addBroker(broker);

  fawkes::naoqi::broker = broker;
    
  // create modules instance
  logger->info("NaoQiFawkes", "*** Instantiating Module");
  AL::ALModule::createModule<NaoFawkesModule>(broker, "NaoFawkesModule");

  return 0;
}
  /** Constructor.
   * Initializes and starts the embedded Fawkes, and loads the nao plugin
   * without precondition.
   * @param broker NaoQi broker to use, will be forwarded to the nao plugin
   * @param name name of the module (no idea why NaoQi wants to pass it
   * as a parameter)
   */
  NaoFawkesModule(AL::ALPtr<AL::ALBroker> broker, const std::string &name)
    : AL::ALModule(broker, name), broker(broker)
  {
    setModuleDescription("Fawkes integration module");

    AL::ALPtr<AL::ALLoggerProxy> logger = broker->getLoggerProxy();

    try {
      logger->info("NaoQiFawkes", "*** Initializing embedded Fawkes");

      // The module flags hack is required because otherwise NaoQi segfaults
      // due to problems with boost static initialization after a module
      // has been closed once, unfortunately that prevents loading a
      // new version of a plugin without a restart.

      fawkes::runtime::InitOptions init_options = 
	fawkes::runtime::InitOptions("naofawkes")
	.plugin_module_flags(fawkes::Module::MODULE_FLAGS_DEFAULT |
			     fawkes::Module::MODULE_NODELETE)
	.net_service_name("NaoQi Fawkes on %h")
        .loggers("console;syslog:NaoQiFawkes")
	.load_plugins("naoqi,webview")
        .default_plugin("nao_default");

      if (fawkes::runtime::init(init_options) != 0) {
	//throw AL::ALError(name, "ctor", "Initializing Fawkes failed");
	logger->info("NaoQiFawkes", "--- Fawkes initialization failed");
        play_sound(RESDIR"/sounds/naoshutdown.wav");
      } else {

	logger->info("NaoQiFawkes", "*** Starting embedded Fawkes");
	fawkes::runtime::main_thread->full_start();
        logger->info("NaoQiFawkes", "*** Embedded Fawkes initialization done");
        play_sound(RESDIR"/sounds/naostartup.wav");
      }
    } catch (fawkes::Exception &e) {
      std::string message;
      for (fawkes::Exception::iterator i = e.begin(); i != e.end(); ++i) {
        if (i != e.begin())  message += "\n";
        message += *i;
      }
      logger->info("NaoQiFawkes",
                   "--- Fawkes initialization failed, exception follows.");
      logger->info("NaoQiFawkes", message);
      play_sound(RESDIR"/sounds/naoshutdown.wav");
      //throw AL::ALError(name, "ctor", e.what());
    }

  }
Beispiel #8
0
void
Kick::init(const string newName, AL::ALPtr<AL::ALBroker> parentBroker)
{
	Component::init(newName, parentBroker);

	fin = false;

	try{
			pmotion = parentBroker->getMotionProxy();
	}catch( AL::ALError& e) {
		cerr << "[Kick ()::init(): " << e.toString() << endl;
	}


}
ObstacleAvoidanceController::ObstacleAvoidanceController(AL::ALPtr<AL::ALBroker> pbroker, MessageQueue* mq) :
	Thread(false), Publisher("ObstacleAvoidanceController"), Subscriber("ObstacleAvoidanceController") {
	cout << "ObstacleAvoidance Controller Constructor" << endl;
	try {
		memory = pbroker->getMemoryProxy();
	} catch (AL::ALError& e) {
		cout << "Error in getting memory proxy" << std::endl;
	}

	if (mq != 0) {
		mq->add_publisher(this);
		mq->add_subscriber(this);
		mq->subscribe("sensors", this, 0);
	}

	cout << "ObstacleAvoidance Controller Initialized" << endl;
}
void ALRosALPythonBridge::bindMethods(AL::ALPtr<AL::ALBroker> pNaoQiBroker, ros::NodeHandle& pRosNode)
{
  if (! pNaoQiBroker->isModulePresent("ALPythonBridge"))
  {
    return;
  }
  fProxy = ALPtr<ALPythonBridgeProxy> (new ALPythonBridgeProxy(pNaoQiBroker));

  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALPythonBridge/eval"), &ALRosALPythonBridge::eval, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALPythonBridge/exit"), &ALRosALPythonBridge::exit, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALPythonBridge/getBrokerName"), &ALRosALPythonBridge::getBrokerName, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALPythonBridge/getMethodList"), &ALRosALPythonBridge::getMethodList, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALPythonBridge/getUsage"), &ALRosALPythonBridge::getUsage, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALPythonBridge/ping"), &ALRosALPythonBridge::ping, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALPythonBridge/version"), &ALRosALPythonBridge::version, this));
  bindCustomMethods(pNaoQiBroker, pRosNode);
}
void ALRosALFrameManager::bindMethods(AL::ALPtr<AL::ALBroker> pNaoQiBroker, ros::NodeHandle& pRosNode)
{
  if (! pNaoQiBroker->isModulePresent("ALFrameManager"))
  {
    return;
  }
  fProxy = ALPtr<ALFrameManagerProxy> (new ALFrameManagerProxy(pNaoQiBroker));

  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/cleanBehaviors"), &ALRosALFrameManager::cleanBehaviors, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/completeBehavior"), &ALRosALFrameManager::completeBehavior, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/createAndPlayTimeline"), &ALRosALFrameManager::createAndPlayTimeline, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/deleteBehavior"), &ALRosALFrameManager::deleteBehavior, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/exit"), &ALRosALFrameManager::exit, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/exitBehavior"), &ALRosALFrameManager::exitBehavior, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/getBehaviorPath"), &ALRosALFrameManager::getBehaviorPath, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/getBrokerName"), &ALRosALFrameManager::getBrokerName, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/getMethodList"), &ALRosALFrameManager::getMethodList, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/getMotionLength"), &ALRosALFrameManager::getMotionLength, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/getTimelineFps"), &ALRosALFrameManager::getTimelineFps, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/getUsage"), &ALRosALFrameManager::getUsage, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/gotoAndPlayString"), &ALRosALFrameManager::gotoAndPlayString, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/gotoAndPlay"), &ALRosALFrameManager::gotoAndPlay, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/gotoAndStopString"), &ALRosALFrameManager::gotoAndStopString, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/gotoAndStop"), &ALRosALFrameManager::gotoAndStop, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/newBehavior"), &ALRosALFrameManager::newBehavior, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/newBehaviorFromChoregraphe"), &ALRosALFrameManager::newBehaviorFromChoregraphe, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/newBehaviorFromFile"), &ALRosALFrameManager::newBehaviorFromFile, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/onPythonError"), &ALRosALFrameManager::onPythonError, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/onPythonPrint"), &ALRosALFrameManager::onPythonPrint, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/onRubyError"), &ALRosALFrameManager::onRubyError, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/onRubyPrint"), &ALRosALFrameManager::onRubyPrint, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/onUrbiError"), &ALRosALFrameManager::onUrbiError, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/onUrbiPrint"), &ALRosALFrameManager::onUrbiPrint, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/ping"), &ALRosALFrameManager::ping, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/playBehavior"), &ALRosALFrameManager::playBehavior, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/playDefaultProject"), &ALRosALFrameManager::playDefaultProject, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/playTimeline"), &ALRosALFrameManager::playTimeline, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/setTimelineFps"), &ALRosALFrameManager::setTimelineFps, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/stopTimeline"), &ALRosALFrameManager::stopTimeline, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALFrameManager/version"), &ALRosALFrameManager::version, this));
  bindCustomMethods(pNaoQiBroker, pRosNode);
}
void
RecorderSession::init(const string newName, AL::ALPtr<AL::ALBroker> parentBroker)
{
	Component::init(newName, parentBroker);
	setFreqTime(SHORT_RATE);

	try
		{
			pmotion = parentBroker->getMotionProxy();
			lastpos = pmotion->getRobotPosition(USE_SENSOR);
		} catch (AL::ALError& e) {
			lastpos.push_back(0.0);
			lastpos.push_back(0.0);
			lastpos.push_back(0.0);
			lastpos.push_back(0.0);
			lastpos.push_back(0.0);
			lastpos.push_back(0.0);
			cerr << "Recorder::NEKFLocalization [motion]" << e.toString() << endl;
		}
	initts = lastts = getCurrentTime();
}
void EKFLocalization::init(const string newName,
		AL::ALPtr<AL::ALBroker> parentBroker) {
	Component::init(newName, parentBroker);

	setFreqTime(MEDIUM_RATE);

	ekf.filter = new EKF_3D();

	MatrixCM sinit, Pinit;
	sinit.resize(3, 1);
	sinit.sete(0,0,0.0);
	sinit.sete(1,0,0.0);
	sinit.sete(2,0,0.0);

	Pinit.identity(3);
	Pinit.sete(0, 0, 3000.0*3000.0);
	Pinit.sete(1, 1, 2000.0*2000.0);
	Pinit.sete(2, 2, M_PI * M_PI);

	ekf.filter->restart(sinit, Pinit);

	mgrid = new MarkovGrid();

	try
	{
		pmotion = parentBroker->getMotionProxy();
		lastpos = pmotion->getRobotPosition(USE_SENSOR);
	} catch (AL::ALError& e) {
		lastpos.push_back(0.0);
		lastpos.push_back(0.0);
		lastpos.push_back(0.0);
		lastpos.push_back(0.0);
		lastpos.push_back(0.0);
		lastpos.push_back(0.0);
		cerr << "EKFLocalization::EKFLocalization [motion]" << e.toString() << endl;
	}

	// lastpos = pmotion->getRobotPosition(USE_SENSOR);
	//_MovementModel->initMovement();
}
void ALRosALAudioDevice::bindMethods(AL::ALPtr<AL::ALBroker> pNaoQiBroker, ros::NodeHandle& pRosNode)
{
  if (! pNaoQiBroker->isModulePresent("ALAudioDevice"))
  {
    return;
  }
  fProxy = ALPtr<ALAudioDeviceProxy> (new ALAudioDeviceProxy(pNaoQiBroker));

  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/closeAudioInputs"), &ALRosALAudioDevice::closeAudioInputs, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/closeAudioOutputs"), &ALRosALAudioDevice::closeAudioOutputs, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/disableEnergyComputation"), &ALRosALAudioDevice::disableEnergyComputation, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/enableEnergyComputation"), &ALRosALAudioDevice::enableEnergyComputation, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/exit"), &ALRosALAudioDevice::exit, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/getBrokerName"), &ALRosALAudioDevice::getBrokerName, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/getFrontMicEnergy"), &ALRosALAudioDevice::getFrontMicEnergy, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/getLeftMicEnergy"), &ALRosALAudioDevice::getLeftMicEnergy, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/getMethodList"), &ALRosALAudioDevice::getMethodList, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/getOutputVolume"), &ALRosALAudioDevice::getOutputVolume, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/getParameter"), &ALRosALAudioDevice::getParameter, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/getRearMicEnergy"), &ALRosALAudioDevice::getRearMicEnergy, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/getRightMicEnergy"), &ALRosALAudioDevice::getRightMicEnergy, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/getUsage"), &ALRosALAudioDevice::getUsage, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/openAudioInputs"), &ALRosALAudioDevice::openAudioInputs, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/openAudioOutputs"), &ALRosALAudioDevice::openAudioOutputs, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/ping"), &ALRosALAudioDevice::ping, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/playSine"), &ALRosALAudioDevice::playSine, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/resetAudio"), &ALRosALAudioDevice::resetAudio, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/sendLocalBufferToOutput"), &ALRosALAudioDevice::sendLocalBufferToOutput, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/setFileAsInput"), &ALRosALAudioDevice::setFileAsInput, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/setOutputVolume"), &ALRosALAudioDevice::setOutputVolume, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/setParameter"), &ALRosALAudioDevice::setParameter, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/startMicrophonesRecording"), &ALRosALAudioDevice::startMicrophonesRecording, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/stopAudioOut"), &ALRosALAudioDevice::stopAudioOut, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/stopMicrophonesRecording"), &ALRosALAudioDevice::stopMicrophonesRecording, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/subscribeRemoteModule"), &ALRosALAudioDevice::subscribeRemoteModule, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/unSubscribeRemoteModule"), &ALRosALAudioDevice::unSubscribeRemoteModule, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALAudioDevice/version"), &ALRosALAudioDevice::version, this));
  bindCustomMethods(pNaoQiBroker, pRosNode);
}
void ALRosALRedBallTracker::bindMethods(AL::ALPtr<AL::ALBroker> pNaoQiBroker, ros::NodeHandle& pRosNode)
{
  if (! pNaoQiBroker->isModulePresent("ALRedBallTracker"))
  {
    return;
  }
  fProxy = ALPtr<ALRedBallTrackerProxy> (new ALRedBallTrackerProxy(pNaoQiBroker));

  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALRedBallTracker/exit"), &ALRosALRedBallTracker::exit, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALRedBallTracker/getBrokerName"), &ALRosALRedBallTracker::getBrokerName, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALRedBallTracker/getMethodList"), &ALRosALRedBallTracker::getMethodList, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALRedBallTracker/getPosition"), &ALRosALRedBallTracker::getPosition, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALRedBallTracker/getUsage"), &ALRosALRedBallTracker::getUsage, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALRedBallTracker/isActive"), &ALRosALRedBallTracker::isActive, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALRedBallTracker/isNewData"), &ALRosALRedBallTracker::isNewData, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALRedBallTracker/ping"), &ALRosALRedBallTracker::ping, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALRedBallTracker/setWholeBodyOn"), &ALRosALRedBallTracker::setWholeBodyOn, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALRedBallTracker/startTracker"), &ALRosALRedBallTracker::startTracker, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALRedBallTracker/stopTracker"), &ALRosALRedBallTracker::stopTracker, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALRedBallTracker/version"), &ALRosALRedBallTracker::version, this));
  bindCustomMethods(pNaoQiBroker, pRosNode);
}
void ALRosALSpeechRecognition::bindMethods(AL::ALPtr<AL::ALBroker> pNaoQiBroker, ros::NodeHandle& pRosNode)
{
  if (! pNaoQiBroker->isModulePresent("ALSpeechRecognition"))
  {
    return;
  }
  fProxy = ALPtr<ALSpeechRecognitionProxy> (new ALSpeechRecognitionProxy(pNaoQiBroker));

  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/exit"), &ALRosALSpeechRecognition::exit, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/getAvailableLanguages"), &ALRosALSpeechRecognition::getAvailableLanguages, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/getBrokerName"), &ALRosALSpeechRecognition::getBrokerName, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/getCurrentPeriod"), &ALRosALSpeechRecognition::getCurrentPeriod, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/getCurrentPrecision"), &ALRosALSpeechRecognition::getCurrentPrecision, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/getLanguage"), &ALRosALSpeechRecognition::getLanguage, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/getMethodList"), &ALRosALSpeechRecognition::getMethodList, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/getMyPeriod"), &ALRosALSpeechRecognition::getMyPeriod, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/getMyPrecision"), &ALRosALSpeechRecognition::getMyPrecision, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/getOutputNames"), &ALRosALSpeechRecognition::getOutputNames, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/getParameter"), &ALRosALSpeechRecognition::getParameter, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/getPhoneticTranscription"), &ALRosALSpeechRecognition::getPhoneticTranscription, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/getUsage"), &ALRosALSpeechRecognition::getUsage, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/loadVocabulary"), &ALRosALSpeechRecognition::loadVocabulary, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/ping"), &ALRosALSpeechRecognition::ping, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/setAudioExpression"), &ALRosALSpeechRecognition::setAudioExpression, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/setDebugMode"), &ALRosALSpeechRecognition::setDebugMode, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/setLanguage"), &ALRosALSpeechRecognition::setLanguage, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/setParameter"), &ALRosALSpeechRecognition::setParameter, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/setVisualExpression"), &ALRosALSpeechRecognition::setVisualExpression, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/setWordListAsVocabulary"), &ALRosALSpeechRecognition::setWordListAsVocabulary, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/subscribeString"), &ALRosALSpeechRecognition::subscribeString, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/subscribe"), &ALRosALSpeechRecognition::subscribe, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/unsubscribe"), &ALRosALSpeechRecognition::unsubscribe, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/updatePeriod"), &ALRosALSpeechRecognition::updatePeriod, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/updatePrecision"), &ALRosALSpeechRecognition::updatePrecision, this));
  fServices.push_back(pRosNode.advertiseService(std::string("NaoQi/ALSpeechRecognition/version"), &ALRosALSpeechRecognition::version, this));
  bindCustomMethods(pNaoQiBroker, pRosNode);
}