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);
}