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); }
/************************************************************* * 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()); } }
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); }