std::string MetaObjectPrivate::generateErrorString(const std::string& signature,
const std::string& resolvedSignature,
const std::vector<std::pair<MetaMethod, float> >& candidates,
int error,
bool logError) const
{
std::stringstream ss;
if (error == -1 && candidates.size() != 0) {
qiLogError() << "Broken error handling in generateErrorString";
logError = 1;
}
switch (error) {
case -1: {
ss << "Can't find method: " << signature << " (resolved to '" << resolvedSignature << "')" << std::endl;
std::vector<MetaMethod> mmv = findMethod(qi::signatureSplit(signature)[1]);
displayMeths(ss, mmv);
break;
}
case -2:
ss << "Arguments types did not match for " << signature << " (resolved to '" << resolvedSignature << "')" << ":" << std::endl;
displayCandidates(ss, candidates);
break;
case -3:
ss << "Ambiguous overload for " << signature << " (resolved to '" << resolvedSignature << "')" << ":" << std::endl;
displayCandidates(ss, candidates);
break;
default:
qiLogError() << "Invalid error id for generateErrorString";
}
if (logError)
qiLogError() << ss.str();
return ss.str();
}
jobject FutureHandler::futurePointer(JNIEnv *env, qi::CallbackInfo* info)
{
for (std::map<qi::Future<qi::AnyValue>*, qi::CallbackInfo*>::iterator it = globalFutureHandler._map.begin();
it != globalFutureHandler._map.end(); ++it)
{
if ((*it).second == info)
{
jclass futureCls = 0;
jmethodID init = 0;
if ((futureCls = env->FindClass("com/aldebaran/qimessaging/Future")) == 0)
{
qiLogError("qimessaging.jni") << "Cannot find com.aldebaran.qimessaging.Future class";
return 0;
}
if ((init = env->GetMethodID(futureCls, "<init>", "(J)V")) == 0)
{
qiLogError("qimessaging.jni") << "Cannot find com.aldebaran.qimessaging.Future.<init>(J) constructor";
return 0;
}
// Create a new Future class.
// Add a new global ref to object to avoid destruction before entry into Java code.
jobject future = env->NewObject(futureCls, init, (*it).first);
env->NewGlobalRef(future);
return future;
}
}
return 0;
}
/**
* Struct constructor, initializes module instance and callback mutex
*/
Impl(JointAttentionLogger &mod) : module(mod), fCallbackMutex(AL::ALMutex::createALMutex()) {
readConfig(remoteIP, remotePort);
// Create proxy to ALMemory
try {
memoryProxy = boost::shared_ptr<AL::ALMemoryProxy>(new AL::ALMemoryProxy(mod.getParentBroker()));
behaviorProxy = boost::shared_ptr<AL::ALBehaviorManagerProxy>(new AL::ALBehaviorManagerProxy(mod.getParentBroker()));
classificationProxy = boost::shared_ptr<AL::ALProxy>(new AL::ALProxy("LRKlasifikacijaZvukova", remoteIP, remotePort));
memoryProxyRemote = boost::shared_ptr<AL::ALMemoryProxy>(new AL::ALMemoryProxy(remoteIP, remotePort));
}
catch (const AL::ALError& e) {
qiLogError("JointAttentionLogger") << "Error creating proxy to ALMemory" << e.toString() << std::endl;
}
// Declare events generated by this module, subscribe to external events
try {
memoryProxy->declareEvent("CallChildJA", "JointAttentionLogger");
memoryProxy->declareEvent("EndSessionJA", "JointAttentionLogger");
memoryProxyRemote->subscribeToEvent("StartSessionJA", "JointAttentionLogger", "onStartLogger");
childCount = 0;
parametriObrada.arrayPush(10000); //granica glasnoce
parametriObrada.arrayPush(5); //broj okvira koje kupim
parametriObrada.arrayPush(5); //broj buffera po okviru
parametriSnimanje.arrayPush(16000); //frekvencija snimanja
parametriSnimanje.arrayPush(3); //mikrofon ( 3 = AL::FRONTCHANNEL )
parametriSnimanje.arrayPush(0); //interleaving
parametriSnimanje.arrayPush(16384); //velicina buffera
parametri.arrayPush(parametriObrada);
parametri.arrayPush(parametriSnimanje);
}
catch (const AL::ALError& e) {
qiLogError("JointAttentionLogger") << " JointAttention Error setting up Logger" << e.toString() << std::endl;
}
}
qi::MetaObject MetaObject::merge(const qi::MetaObject &source, const qi::MetaObject &dest) {
qi::MetaObject result = source;
if (!result._p->addMethods(dest.methodMap()))
qiLogError() << "can't merge metaobject (methods)";
if (!result._p->addSignals(dest.signalMap()))
qiLogError() << "can't merge metaobject (signals)";
if (!result._p->addProperties(dest.propertyMap()))
qiLogError() << "can't merge metaobject (properties)";
result._p->setDescription(dest.description());
result._p->refreshCache();
return result;
}
void OnFaceDetection::callback() {
/** Use a mutex to make it all thread safe. */
AL::ALCriticalSection section(fCallbackMutex);
try {
/** Retrieve the data raised by the event. */
fFaces = fMemoryProxy.getData("FaceDetected");
qiLogInfo("module.name") <<fFaces<<std::endl;
/** Check that there are faces effectively detected. */
if (fFaces.getSize() < 2 ) {
if (fFacesCount != 0) {
qiLogInfo("module.example") << "No face detected." << std::endl;
fTtsProxy.say("No face detected.");
fFacesCount = 0;
}
fTtsProxy.say("1 face.");
return;
}
/** Check the number of faces from the FaceInfo field, and check that it has
* changed from the last event.*/
if (fFaces[1].getSize() - 1 != fFacesCount) {
qiLogInfo("module.name") << fFaces[1].getSize() - 1 << " face(s) detected." << std::endl;
char buffer[50];
sprintf(buffer, "%d faces detected.", fFaces[1].getSize() - 1);
fTtsProxy.say(std::string(buffer));
/** Update the current number of detected faces. */
fFacesCount = fFaces[1].getSize() - 1;
}
}
catch (const AL::ALError& e) {
qiLogError("module.name") << e.what() << std::endl;
}
}
void ResponseToNameInterface::startTask(const std::string& todo) {
if(impl->started) {
return;
}
impl->started = true;
if(todo == "start") {
// Subscribe to events which can be triggered during the session
try {
impl->memoryProxy->subscribeToEvent("CallChildRTN", "ResponseToNameInterface", "callChild");
impl->memoryProxy->subscribeToEvent("EndSessionRTN", "ResponseToNameInterface", "endSession");
}
catch (const AL::ALError& e) {
qiLogError("ResponseToNameInterface") << "Error subscribing to events" << e.toString() << std::endl;
}
// Signal the start of the session by changing eye color (unblocking call)
impl->ledProxy->post.fadeRGB("FaceLeds", 0x00FF00, 1.5);
// Raise event that the session should start
impl->memoryProxy->raiseEvent("StartSessionRTN", AL::ALValue(1));
}
else if(todo == "enable") {
// Subscribe to event FronTactilTouched, which signals the start of the session
impl->memoryProxy->subscribeToEvent("FrontTactilTouched", "ResponseToNameInterface", "onTactilTouched");
}
}
JNIAttach::JNIAttach(JNIEnv* env)
{
if (!ThreadJNI.get())
ThreadJNI.reset(new JNIHandle);
if (env)
{
assert(!ThreadJNI->env || env == ThreadJNI->env);
JVM(env);
ThreadJNI->env = env;
}
else if (!ThreadJNI->env)
{
JavaVM* jvm = JVM();
assert(jvm);
if (jvm->GetEnv((void**)&ThreadJNI->env, QI_JNI_MIN_VERSION) != JNI_OK ||
ThreadJNI->env == 0)
{
char threadName[] = "qimessaging-thread";
JavaVMAttachArgs args = { JNI_VERSION_1_6, threadName, 0 };
if (JVM()->AttachCurrentThread((envPtr)&ThreadJNI->env, &args) != JNI_OK ||
ThreadJNI->env == 0)
{
qiLogError() << "Cannot attach callback thread to Java VM";
throw std::runtime_error("Cannot attach callback thread to Java VM");
}
ThreadJNI->attached = true;
}
}
++ThreadJNI->lockCount;
}
void SubscribeGyro::onMoveForward(const std::string &key, const AL::ALValue &value, const AL::ALValue &msg){
AL::ALCriticalSection section(fCallbackMutex);
gettimeofday(¤tTime);
file << "Forward\t" << std::endl;
bool test = value;
timer();
qiLogInfo("subscribegyro.gyroswing") << "Executing callback method on move forward: " << test << std::endl;
try {
AL::ALProxy genericProxy ("MovementTools", "127.0.0.1" , 9559);
genericProxy.callVoid("setSpeed", 0.8f);
if (test == true && t > 300) {
genericProxy.callVoid("swingForwards");
qiLogInfo("subscribegyro.gyroswing") << "Forward! " << currentTime.tv_sec << std::endl;
//qi::os::sleep(0.5);
gettimeofday(&lastTime);
}
//fTtsProxy = AL::ALTextToSpeechProxy(getParentBroker());
//fTtsProxy.say("Forward");
}
catch (const AL::ALError& e) {
qiLogError("module.example") << e.what() << std::endl;
}
}
/**
* Function called by the SessionStart callback
* Initializes output file, resets internal variables
*/
void startLogger() {
// Open output file with timestamp
boost::posix_time::ptime now = boost::posix_time::second_clock::local_time();
std::stringstream filename;
filename << "/home/nao/naoqi/modules/logs/" << now.date().year() << "_" << static_cast<int>(now.date().month())
<< "_" << now.date().day() << "_" << now.time_of_day().hours() << now.time_of_day().minutes() << "_JointAttention.txt";
outputFileLock.lock();
outputFile.open(filename.str().c_str(), std::ios::out);
outputFileLock.unlock();
// Calculate sessionStart time, reset internal variables
sessionStart = boost::get_system_time();
iteration = 0;
faceCount = 0;
ended = false;
callFinished = true;
childCount++;
// Session is starting, subscribe to external events
try {
memoryProxy->subscribeToEvent("FaceDetected", "JointAttentionLogger", "onFaceDetected");
memoryProxyRemote->subscribeToEvent("ChildCalledJA", "JointAttentionLogger", "onChildCalled");
memoryProxy->subscribeToEvent("EndSessionJA", "JointAttentionLogger", "onStopLogger");
memoryProxy->subscribeToEvent("SoundClassified", "JointAttentionLogger", "onSoundClassified");
classificationProxy->callVoid("pocni_klasifikaciju", parametri);
}
catch (const AL::ALError& e) {
qiLogError("JointAttentionLogger") << "Error subscribing to events" << e.toString() << std::endl;
}
// Start scheduler thread
t = new boost::thread(boost::ref(*module.impl));
}
unsigned char * FGrab::grab() {
AL::ALImage *img = impl->getFrameBuffer();
if (!img) {
qiLogError("FGrab") << "Could not get the latest camera image" << std::endl;
}
return img->getData();
}
void onWordRecognized(const std::string& name, const AL::ALValue& value, const std::string& myname)
{
try
{
AL::ALCriticalSection section(mutex);
AL::ALValue v = memory.getData("WordRecognized");
std::vector<WordConfidencePair> words;
ParseWordRecognizedArray(words, value, 0.5f);
if (words.size() > 0)
{
speech.say(words[0].word);
}
//speech.say("word");
//speech.say(v.toString());
}
catch (const AL::ALError& e)
{
qiLogError("onWordRecognized") << e.what() << std::endl;
}
// this gets called a lot, try and suppress excessive calls
qi::os::msleep(2000);
}
qi::Future<unsigned int> ObjectRegistrar::registerService(const std::string &name, qi::AnyObject obj)
{
if (Server::endpoints().empty()) {
qiLogError() << "Could not register service: " << name << " because the current server has not endpoint";
return qi::Future<unsigned int>();
}
qi::ServiceInfo si;
si.setName(name);
si.setProcessId(qi::os::getpid());
si.setMachineId(qi::os::getMachineId());
si.setEndpoints(Server::endpoints());
si.setSessionId(_id);
long id = ++_registerServiceRequestIndex;
{
boost::mutex::scoped_lock sl(_registerServiceRequestMutex);
_registerServiceRequest[id] = std::make_pair(obj, si);
}
qi::Promise<unsigned int> prom;
qi::Future<unsigned int> future;
future = _sdClient->registerService(si);
future.connect(boost::bind<void>(&ObjectRegistrar::onFutureFinished, this, _1, id, prom));
return prom.future();
};
int FGrab::getImageSize() {
if (!impl->img) {
qiLogError("FGrab") << "Called getImageSize without an image captured" << std::endl;
return 0;
}
return impl->img->getSize();
}
TimelinePrivate::TimelinePrivate(AnyObject motion, Timeline *timeline)
: _executer(new asyncExecuter(1000 / 25)),
_fps(0),
_enabled(false),
_currentFrame(0),
_startFrame(0),
_endFrame(-1),
_lastFrame(-1),
_currentDoInterpolationMoveOrderId(-1),
_name("Timeline"),
_resourcesAcquisition(AnimationModel::MotionResourcesHandler_Passive),
_methodMonitor(),
_framesFlagsMap(),
_timeline(timeline),
_isValid(true)
{
try
{
boost::shared_ptr<AL::ALProxy> proxyMotion(new AL::ALProxy(motion, "ALMotion"));
_motionProxy = boost::shared_ptr<AL::ALMotionProxy>(new AL::ALMotionProxy(proxyMotion));
}
catch (AL::ALError& e)
{
qiLogError() << "Cannot create proxy on ALMotion :" << std::endl << e.what() << std::endl;
_isValid = false;
}
}
void visitInt(int64_t value, bool isSigned, int byteSize)
{
switch((isSigned ? 1 : -1) * byteSize)
{
case 0: {
bool v = value != 0;
if (v)
out << "true";
else
out << "false";
break;
}
case 1:
case 2:
case 4:
case 8: out << ((int64_t)value); break;
case -1:
case -2:
case -4:
case -8: out << ((uint64_t)value);break;
default:
qiLogError() << "Unknown integer type " << isSigned << " " << byteSize;
}
}
//--------------------------Private Class-------------------------------------//
ParameterModelPrivate::ParameterModelPrivate(const std::string &name,
AutoAnyReference defaultValue,
bool inheritsFromParent,
bool customChoice,
bool password,
const std::string &tooltip,
unsigned int id,
bool resource) :
_metaProperty(id, name, defaultValue.signature()),
_inheritsFromParent(inheritsFromParent),
_customChoice(customChoice),
_password(password),
_tooltip(tooltip),
_choices(),
_isValid(true),
_defaultValue(defaultValue.clone())
{
if(resource)
_metaProperty = MetaProperty(id, name, ParameterModel::signatureRessource());
Signature signature(defaultValue.signature());
//If defaultValue is int or double min and max are not defined so this object is not valid
if(isIntegerOrDouble(defaultValue))
{
qiLogError() << "Parameter.min and Parameter.max is not defined.";
_isValid = false;
}
}
void Server::onMessageReady(const qi::Message &msg, TransportSocketPtr socket) {
qi::BoundAnyObject obj;
// qiLogDebug() << "Server Recv (" << msg.type() << "):" << msg.address();
{
boost::mutex::scoped_lock sl(_boundObjectsMutex);
BoundAnyObjectMap::iterator it;
it = _boundObjects.find(msg.service());
if (it == _boundObjects.end())
{
// The message could be addressed to a bound object, inside a
// remoteobject host, or to a remoteobject, using the same socket.
qiLogVerbose() << "No service for " << msg.address();
if (msg.object() > Message::GenericObject_Main
|| msg.type() == Message::Type_Reply
|| msg.type() == Message::Type_Event
|| msg.type() == Message::Type_Error
|| msg.type() == Message::Type_Canceled)
return;
// ... but only if the object id is >main
qi::Message retval(Message::Type_Error, msg.address());
std::stringstream ss;
ss << "can't find service, address: " << msg.address();
retval.setError(ss.str());
socket->send(retval);
qiLogError() << "Can't find service: " << msg.service() << " on " << msg.address();
return;
}
obj = it->second;
}
// We were called from the thread pool: synchronous call is ok
//qi::getEventLoop()->post(boost::bind<void>(&BoundObject::onMessage, obj, msg, socket));
obj->onMessage(msg, socket);
}
void PeriodicTaskPrivate::_reschedule(qi::int64_t delay)
{
qiLogDebug() << _name <<" rescheduling in " << delay;
_task = getEventLoop()->async(boost::bind(&PeriodicTaskPrivate::_wrap, shared_from_this()), delay);
if (!_state.setIfEquals(Task_Rescheduling, Task_Scheduled))
qiLogError() << "PeriodicTask forbidden state change while rescheduling " << *_state;
}
void Bumper::onRightBumperPressed() {
std::cout<<"onRightBumperPressed()"<<std::endl;
qiLogInfo("module.example") << "Executing callback method on right bumper event" << std::endl;
/**
* As long as this is defined, the code is thread-safe.
*/
AL::ALCriticalSection section(fCallbackMutex);
/**
* Check that the bumper is pressed.
*/
std::cout<<"before memory.getData2"<<std::endl;
fState = fMemoryProxy.getData("RightBumperPressed");
std::cout<<"After memory.getData2"<<std::endl;
if (fState > 0.5f) {
std::cout<<"if"<<std::endl;
return;
}
try {
fTtsProxy = AL::ALTextToSpeechProxy(getParentBroker());
std::cout<<"HELLO"<<std::endl;
fTtsProxy.say("Hello!");
}
catch (const AL::ALError& e) {
qiLogError("module.example") << e.what() << std::endl;
}
std::cout<<"END"<<std::endl;
}
void Bumper::init() {
std::cout<<"Init"<<std::endl;
try {
/** Create a proxy to ALMemory.
*/
std::cout<<"Before memory proxy"<<std::endl;
fMemoryProxy = AL::ALMemoryProxy(getParentBroker());
std::cout<<"Before memory.getdata1"<<std::endl;
fState = fMemoryProxy.getData("RightBumperPressed");
std::cout<<"After memory.getdata1"<<std::endl;
std::cout<<"Before memory.SUBSCRIBE"<<std::endl;
/** Subscribe to event RightBumperPressed
* Arguments:
* - name of the event
* - name of the module to be called for the callback
* - name of the bound method to be called on event
*/
fMemoryProxy.subscribeToEvent("RightBumperPressed", "Bumper",
"onRightBumperPressed");
std::cout<<"After memory.SUBSCRIBE"<<std::endl;
}
catch (const AL::ALError& e) {
qiLogError("module.example") << e.what() << std::endl;
}
}
void visitFloat(double value, int byteSize)
{
if (byteSize == 4)
out << ((float)value);
else if (byteSize == 8)
out << ((double)value);
else
qiLogError() << "serialize on unknown float type " << byteSize;
}
std::string findData(const std::string &applicationName,
const std::string &filename)
{
if(filename == "") {
qiLogError("qi::path:") << "Filename cannot be empty!";
return std::string();
}
return getInstance()->findData(applicationName, filename);
}
void FGrab::init() {
/* Init is called just after construction. */
try {
impl = boost::shared_ptr<Impl>(new Impl(*this));
} catch (std::exception& e) {
qiLogError("FGrab") << "Failed to initialize FGrab module: " << e.what() << std::endl;
exit();
}
}
// Get JNI environment pointer, valid in current thread.
JNIEnv* env()
{
JNIEnv* env = 0;
JVM()->GetEnv(reinterpret_cast<void**>(&env), QI_JNI_MIN_VERSION);
if (!env)
{
qiLogError() << "Cannot get JNI environment from JVM";
return 0;
}
if (JVM()->AttachCurrentThread(reinterpret_cast<envPtr>(&env), (void*)0) != JNI_OK)
{
qiLogError() << "Cannot attach current thread to JVM";
return 0;
}
return env;
}
void NaoMarkServiceDetection::init() {
try
{
fMemoryProxy.subscribeToEvent("LandmarkDetected", "Events", "userDataToIdentifyEvent", "callback");
}
catch (const AL::ALError& e) {
qiLogError("module.example") << e.what() << std::endl;
}
}
bool TransportServer::setIdentity(const std::string& key, const std::string& crt)
{
struct ::stat status;
if (qi::os::stat(key.c_str(), &status) != 0)
{
qiLogError() << "stat of \"" << key << "\": " << strerror(errno);
return false;
}
if (qi::os::stat(crt.c_str(), &status) != 0)
{
qiLogError() << "stat of \"" << crt << "\": " << strerror(errno);
return false;
}
_identityCertificate = crt;
_identityKey = key;
return true;
}
std::string findData(const std::string &applicationName,
const std::string &filename,
bool excludeUserWritablePath)
{
if (filename == "") {
qiLogError() << "Filename cannot be empty!";
return std::string();
}
return getInstance()->findData(applicationName, filename, excludeUserWritablePath);
}
void ObjectRegistrar::onFutureFinished(qi::Future<unsigned int> fut, long id, qi::Promise<unsigned int> result)
{
if (fut.hasError()) {
result.setError(fut.error());
return;
}
qi::ServiceInfo si;
RegisterServiceMap::iterator it;
{
boost::mutex::scoped_lock sl(_registerServiceRequestMutex);
it = _registerServiceRequest.find(id);
if (it != _registerServiceRequest.end())
si = it->second.second;
if (fut.hasError()) {
_registerServiceRequest.erase(id);
result.setError(fut.error());
return;
}
}
unsigned int idx = fut.value();
si.setServiceId(idx);
{
boost::mutex::scoped_lock sl(_servicesMutex);
BoundService bs;
bs.id = idx;
bs.object = it->second.first;
bs.serviceInfo = si;
bs.name = si.name();
BoundServiceMap::iterator it;
it = _services.find(idx);
if (it != _services.end()) {
qiLogError() << "A service is already registered with that id:" << idx;
result.setError("Service already registered.");
return;
}
_services[idx] = bs;
//todo register the object on the server (find a better way)
Server::addObject(idx, bs.object);
}
{
boost::mutex::scoped_lock sl(_serviceNameToIndexMutex);
_serviceNameToIndex[si.name()] = idx;
}
{
boost::mutex::scoped_lock sl(_registerServiceRequestMutex);
_registerServiceRequest.erase(it);
}
// ack the Service directory to tell that we are ready
qi::Future<void> fut2 = _sdClient->serviceReady(idx);
fut2.connect(boost::bind(&serviceReady, _1, result, idx));
}
/*
* JNIEnv structure is thread dependent.
* To use a JNIEnv* pointer in another thread (such as QiMessaging callback)
* it must be attach to current thread (via JavaVM->AttachCurrentThread())
* Therefore we keep a pointer to the JavaVM, which is thread safe.
*/
JavaVM* JVM(JNIEnv* env)
{
static JavaVM* gJVM = 0;
if (env != 0 && gJVM == 0)
env->GetJavaVM(&gJVM);
if (gJVM == 0 && env == 0)
qiLogError() << "JVM singleton wasn't initialized";
return gJVM;
}
void FGrab::grab_to_file(const std::string &file) {
qiLogVerbose("FGrab") << "Trying to save one frame to " << file << std::endl;
// ALImage ref doc at
// http://www.aldebaran-robotics.com/documentation/ref/libalvision/classAL_1_1ALImage.html
// Should not be a boost::scoped_ptr because freed by the vision module.
AL::ALImage *img = impl->getFrameBuffer();
if (!img) {
qiLogError("FGrab") << "Could not get the latest camera image" << std::endl;
impl->releaseFrameBuffer();
return;
}
int width = img->getWidth();
int height = img->getHeight();
int nbLayers = img->getNbLayers();
int colorSpace = img->getColorSpace();
long long timeStamp = img->getTimeStamp();
unsigned int size = img->getSize(); // data filed size in bytes
qiLogVerbose("FGrab") << "Image: " << width << "x" << height
<< ", nbLayers: " << nbLayers
<< ", colorspace: " << colorSpace
<< ", timestamp: " << timeStamp
<< ", size: " << size
<< std::endl;
FILE *f = fopen(file.c_str(), "wb"); // "wb" for pics
if (!f) {
qiLogError("FGrab", "Error opening the file %s for writing", file.c_str());
} else {
try {
fwrite(img->getData(), 1, img->getSize(), f);
} catch (std::bad_alloc& e) {
qiLogError("FGrab") << "Could not write image: out of memory" << std::endl;
} catch (std::exception& e) {
qiLogError("FGrab") << "File write failed" << std::endl;
}
fclose(f);
}
impl->releaseFrameBuffer();
}
