/*
* create the vAugmentedTime for the current used Instance : sql to obtain the foreign key for the feedback table
*
*/
bool abmInteraction::insertFeedback(int feedback, int feedbackInstance)
{
Bottle bResult;
ostringstream osRequest;
//take info from visualdata to put in feedback. In particular the time of original memories is the smallest one from augmented
osRequest << "SELECT DISTINCT instance, augmented_time, img_provider_port, min(\"time\") FROM visualdata WHERE instance = " << rememberedInstance << " AND augmented IS NOT NULL AND augmented_time = '" << *it_augmentedTime << "' AND img_provider_port = '" << img_provider_port << "' GROUP BY instance, augmented_time, img_provider_port;";
bResult = iCub->getABMClient()->requestFromString(osRequest.str());
yInfo() << bResult.toString();
if (bResult.toString() != "NULL") {
//we should only have one line, because instance,augmented_time,img_provider_port is primary key
Bottle bResInsert;
ostringstream osInsertFeedback;
osInsertFeedback << "INSERT INTO feedback VALUES (" << feedbackInstance << ", '" << bResult.get(0).asList()->get(1).toString() << "', '" << bResult.get(0).asList()->get(2).toString() << "', '" << bResult.get(0).asList()->get(3).toString() << "', '" << agentName << "', " << feedback << ", 'none');";
bResInsert = iCub->getABMClient()->requestFromString(osInsertFeedback.str().c_str());
yInfo() << " Request sent : " << osInsertFeedback.str();
}
else {
yError() << "Request to obtain augmented from instance " << rememberedInstance << " with augmented_time = " << *it_augmentedTime << " is NULL";
return false;
}
return true;
}
void RosNameSpace::run() {
int pct = 0;
do {
mutex.wait();
pct = pending.size();
mutex.post();
if (pct>0) {
mutex.wait();
Bottle *bot = pending.get(0).asList();
Bottle curr = *bot;
mutex.post();
dbg_printf("ROS connection begins: %s\n", curr.toString().c_str());
ContactStyle style;
style.admin = true;
style.carrier = "tcp";
Bottle cmd = curr.tail();
Contact contact = Contact::fromString(curr.get(0).asString());
contact.setName("");
Bottle reply;
NetworkBase::write(contact, cmd, reply, style);
dbg_printf("ROS connection ends: %s\n", curr.toString().c_str());
mutex.wait();
pending = pending.tail();
pct = pending.size();
mutex.post();
}
} while (pct>0);
}
bool TouchingOrder::handleSearch(string type, string target)
{
// look if the object (from human order) exist and if not, trigger proactivetagging
iCub->opc->checkout();
yInfo() << " [handleSearch] : opc checkout";
Object *o = dynamic_cast<Object*>(iCub->opc->getEntity(target));
if(o!=NULL) {
yInfo() << "I found the entity in the opc: " << target << " and thus I'll leave handleSearch";
return true;
}
yInfo() << "I need to call proactiveTagging!";// << endl;
//If there is an unknown object (to see with agents and rtobjects), add it to the rpc_command bottle, and return true
Bottle cmd;
Bottle rply;
cmd.addString("searchingEntity");
cmd.addString(type);
cmd.addString(target);
yDebug() << "Send to proactiveTagging: " << cmd.toString();
rpc_out_port.write(cmd,rply);
yDebug() << rply.toString(); //<< endl;
return true;
}
string SpeechRecognizerModule::getFromDictaction(int timeout, LPCWSTR options )
{
bool everythingIsFine = TRUE;
everythingIsFine &= SUCCEEDED(m_cpGrammarDictation->UnloadDictation());
everythingIsFine &= SUCCEEDED(m_cpGrammarDictation->LoadDictation(options, SPLO_STATIC));
everythingIsFine &= SUCCEEDED(m_cpGrammarDictation->SetDictationState( SPRS_ACTIVE ));
yInfo() <<"Dictation is on..." ;
Bottle botTmp;
if (!USE_LEGACY)
{
botTmp = waitNextRecognition(m_timeout);
}
else
{
list< pair<string, double> > results = waitNextRecognitionLEGACY(m_timeout);
for(list< pair<string, double> >::iterator it = results.begin(); it != results.end(); it++)
{
botTmp.addString(it->first.c_str());
//botTmp.addDouble(it->second);
}
}
yInfo() <<"Dictation is off...";
yInfo() <<"Got : "<<botTmp.toString();
//Turn off dictation and go back to the file grammar
everythingIsFine &= SUCCEEDED(m_cpGrammarDictation->SetDictationState( SPRS_INACTIVE ));
everythingIsFine &=SUCCEEDED(m_cpGrammarFromFile->SetGrammarState(SPGS_ENABLED));
return botTmp.toString();
}
void checkAccept() {
report(0, "checking direct object accept...");
PortReaderBuffer<Bottle> buffer;
Bottle dummy;
Bottle data("hello");
Bottle data2("there");
buffer.acceptObject(&data, &dummy);
Bottle *bot = buffer.read();
checkTrue(bot!=NULL,"Inserted message received");
if (bot!=NULL) {
checkEqual(bot->toString().c_str(),"hello","value ok");
}
buffer.acceptObject(&data2, NULL);
bot = buffer.read();
checkTrue(bot!=NULL,"Inserted message received");
if (bot!=NULL) {
checkEqual(bot->toString().c_str(),"there","value ok");
}
buffer.read(false);
}
int main(int argc, char *argv[]) {
if (argc<3) {
fprintf(stderr, "Please supply (1) a port name for the client\n");
fprintf(stderr, " (2) a port name for the server\n");
return 1;
}
Network yarp;
const char *client_name = argv[1];
const char *server_name = argv[2];
RpcClient port;
port.open(client_name);
int ct = 0;
while (true) {
if (port.getOutputCount()==0) {
printf("Trying to connect to %s\n", server_name);
yarp.connect(client_name,server_name);
} else {
Bottle cmd;
cmd.addString("COUNT");
cmd.addInt32(ct);
ct++;
printf("Sending message... %s\n", cmd.toString().c_str());
Bottle response;
port.write(cmd,response);
printf("Got response: %s\n", response.toString().c_str());
}
Time::delay(1);
}
}
//*************************************************************************************************************************
MotorFrictionExcitationThread::MotorFrictionExcitationThread(string _name, string _robotName, int _period, ParamHelperServer *_ph,
wholeBodyInterface *_wbi, ResourceFinder &rf, ParamHelperClient *_identificationModule)
: RateThread(_period), name(_name), robotName(_robotName), paramHelper(_ph), robot(_wbi), identificationModule(_identificationModule)
{
status = EXCITATION_OFF;
sendCmdToMotors = SEND_COMMANDS_TO_MOTORS;
printCountdown = 0;
freeExcCounter = 0;
contactExcCounter = 0;
fricStdDevThrMonitor = 0.0;
ktStdDevThrMonitor = 0.0;
_n = robot->getDoFs();
isFrictionStdDevBelowThreshold = false;
Bottle reply;
if(!contactExc.readFromConfigFile(rf, reply))
printf("Error while reading contact excitation from config file: \n%s\n", reply.toString().c_str());
printf("Results of contact excitation reading: %s\n", reply.toString().c_str());
printf("Contact excitation value read:\n%s\n", contactExc.toString().c_str());
reply.clear();
if(!freeMotionExc.readFromConfigFile(rf, reply))
printf("Error while reading free motion excitation from config file: \n%s\n", reply.toString().c_str());
printf("Results of free motion excitation reading: %s\n", reply.toString().c_str());
printf("Free motion excitation value read:\n%s\n", freeMotionExc.toString().c_str());
}
void testSequence(char *seq,
size_t len,
const char *fmt,
Bottle ref,
bool testWrite = true)
{
char err[1024];
printf("\n");
printf("================================================\n");
printf(" READ %s\n", fmt);
Bytes b1(seq, len);
WireTwiddler tt;
tt.configure(fmt, fmt);
printf(">>> %s\n", tt.toString().c_str());
Bottle bot;
checkTrue(tt.read(bot, b1), "Read failed");
snprintf(err, 1024, "%s: read %s, expected %s", fmt, bot.toString().c_str(), ref.toString().c_str());
checkTrue(bot == ref, err);
printf("[1] %s: read %s as expected\n", fmt, bot.toString().c_str());
StringInputStream sis;
sis.add(b1);
sis.add(b1);
WireTwiddlerReader twiddled_input(sis, tt);
Route route;
bot.clear();
twiddled_input.reset();
ConnectionReader::readFromStream(bot, twiddled_input);
snprintf(err, 1024, "%s: read %s, expected %s", fmt, bot.toString().c_str(), ref.toString().c_str());
checkTrue(bot == ref, err);
printf("[2] %s: read %s as expected\n", fmt, bot.toString().c_str());
bot.clear();
twiddled_input.reset();
ConnectionReader::readFromStream(bot, twiddled_input);
snprintf(err, 1024, "%s: read %s, expected %s", fmt, bot.toString().c_str(), ref.toString().c_str());
checkTrue(bot == ref, err);
printf("[3] %s: read %s as expected\n", fmt, bot.toString().c_str());
if (testWrite) {
printf("\n");
printf("================================================\n");
printf(" WRITE %s\n", fmt);
ManagedBytes output;
checkTrue(tt.write(ref, output), "WRITE FAILED");
snprintf(err, 1024, "WRITE MISMATCH, length %zd, expected %zd", output.length(), len);
checkTrue(output.length() == len, err);
for (size_t i = 0; i < output.length(); i++) {
snprintf(err, 1024, "WRITE MISMATCH, at %zd, have [%d:%c] expected [%d:%c]\n", i, output.get()[i], output.get()[i], seq[i], seq[i]);
checkTrue(output.get()[i] == seq[i], err);
}
printf("[4] %s: wrote %s as expected\n", fmt, bot.toString().c_str());
}
}
bool MotorFrictionIdentificationModule::respond(const Bottle& cmd, Bottle& reply)
{
paramHelper->lock();
if(!paramHelper->processRpcCommand(cmd, reply))
reply.addString( (string("Command ")+cmd.toString().c_str()+" not recognized.").c_str());
paramHelper->unlock();
// if reply is empty put something into it, otherwise the rpc communication gets stuck
if(reply.size()==0)
reply.addString( (string("Command ")+cmd.toString().c_str()+" received.").c_str());
return true;
}
bool MainWindow::step()
{
Bottle reply;
emit internalStep(&reply);
if (reply.toString() == "error"){
return false;
}
if (reply.toString() == "ok"){
return true;
}
return false;
}
/*
* create the vAugmentedTime for the current used Instance : sql to obtain the foreign key for the feedback table
*
*/
bool abmInteraction::createAugmentedTimeVector(pair<string,int> & bestTimeAndRank)
{
Bottle bResult;
ostringstream osRequest;
//only augmented_time is needed but better clarity for the print
osRequest << "SELECT DISTINCT instance, augmented_time, img_provider_port FROM visualdata WHERE instance = " << rememberedInstance << " AND augmented IS NOT NULL AND img_provider_port = '" << img_provider_port << "' " ;
if(resume == "agent"){
// not proposing instance with feedback from the agent already.
osRequest << "AND augmented_time NOT IN (SELECT DISTINCT augmented_time FROM feedback WHERE instance = '" << rememberedInstance << "' AND agent = '" << agentName << "') " ;
createBestAugmentedTime(bestTimeAndRank) ;
} else if (resume == "yes") {
osRequest << "AND augmented_time NOT IN (SELECT DISTINCT augmented_time FROM feedback WHERE instance = '" << rememberedInstance << "') " ;
createBestAugmentedTime(bestTimeAndRank);
} else if (resume == "no") {
//come back to default value for best rank/time
bestTimeAndRank.first = "";
bestTimeAndRank.second = 0;
}
osRequest << " ORDER BY augmented_time ASC ;";
osRequest << " ;";
bResult = iCub->getABMClient()->requestFromString(osRequest.str());
yInfo() << "[createAugmentedTimeVector] SQL request bReply : " << bResult.toString();
vAugmentedTime.clear();
if (bResult.toString() != "NULL") {
for (int i = 0; i < bResult.size(); i++){
//get(1) because augmented is in second column of the result
vAugmentedTime.push_back(bResult.get(i).asList()->get(1).toString());
}
}
else {
yError() << "Request to obtain augmented from instance " << rememberedInstance << " is NULL : are you sure there are some augmented images?";
bestTimeAndRank.first = "";
bestTimeAndRank.second = -1;
return false;
}
ostringstream osAugmentedTime;
const char* const delim = ", ";
copy(vAugmentedTime.begin(), vAugmentedTime.end(), std::ostream_iterator<std::string>(osAugmentedTime, delim));
yInfo() << " vAugmentedTime = " << osAugmentedTime.str();
it_augmentedTime = vAugmentedTime.begin();
return true;
}
virtual void run() {
printf("Listening to terminal (type \"quit\" to stop module)\n");
bool isEof = false;
while (!(isEof||isStopping()||owner.isStopping())) {
ConstString str = NetworkBase::readString(&isEof);
if (!isEof) {
Bottle cmd(str.c_str());
Bottle reply;
bool ok = owner.safeRespond(cmd,reply);
if (ok) {
//printf("ALL: %s\n", reply.toString().c_str());
//printf("ITEM 1: %s\n", reply.get(0).toString().c_str());
if (reply.get(0).toString()=="help") {
for (int i=0; i<reply.size(); i++) {
ACE_OS::printf("%s\n",
reply.get(i).toString().c_str());
}
} else {
ACE_OS::printf("%s\n", reply.toString().c_str());
}
} else {
ACE_OS::printf("Command not understood -- %s\n", str.c_str());
}
}
}
//printf("terminal shutting down\n");
//owner.interruptModule();
}
/*
* Message handler. Just echo all received messages.
*/
bool respond(const Bottle& command, Bottle& reply)
{
cout<<"Got "<<command.toString().c_str()<<endl;
if(command.size() < 3) {
reply.addString("Command error! example: 'sum 3 4'");
return true;
}
int a = command.get(1).asInt();
int b = command.get(2).asInt();
if( command.get(0).asString() == "sum") {
int c = sum(a, b);
reply.addInt(c);
return true;
}
if( command.get(0).asString() == "sub") {
int c = sub(a, b);
reply.addInt(c);
return true;
}
reply.addString("Unknown command");
reply.addString(command.get(0).asString().c_str());
return true;
}
void testStringWithNull() {
report(0,"test string with null");
char buf1[] = "hello world";
char buf2[] = "hello world";
buf2[5] = '\0';
ConstString str1(buf1,12);
ConstString str2(buf2,12);
checkEqual(str1.length(),12,"unmodified string length ok");
checkEqual(str2.length(),12,"modified string length ok");
ConstString str3(str2);
checkEqual(str3.length(),12,"copied string length ok");
Bottle bot;
bot.addString(str2);
checkEqual(bot.get(0).asString().length(),12,"bottled string asString() length ok");
checkEqual(bot.get(0).toString().length(),12,"bottled string toString() length ok");
Bottle bot2 = bot;
checkEqual(bot2.get(0).asString().length(),12,"bottled, copied string length ok");
Bottle bot3;
bot.write(bot3);
checkEqual(bot3.get(0).asString().length(),12,"bottled, serialized string length ok");
Bottle bot4;
bot4.fromString(bot.toString());
checkEqual(bot4.get(0).asString().length(),12,"bottled, text-serialized string length ok");
}
bool SpringyFingersModel::getOutput(Value &out) const
{
if (configured)
{
Value val[5];
fingers[0].getOutput(val[0]);
fingers[1].getOutput(val[1]);
fingers[2].getOutput(val[2]);
fingers[3].getOutput(val[3]);
fingers[4].getOutput(val[4]);
Bottle bOut; Bottle &ins=bOut.addList();
ins.addDouble(val[0].asDouble());
ins.addDouble(val[1].asDouble());
ins.addDouble(val[2].asDouble());
ins.addDouble(val[3].asDouble());
ins.addDouble(val[4].asDouble());
out.fromString(bOut.toString().c_str());
return true;
}
else
return false;
}
bool test_surface_mesh() {
printf("\n*** test_surface_mesh()\n");
SurfaceMesh mesh;
Box3D bb;
mesh.meshName = "testing";
bb.corners.push_back(PointXYZ(1,2,3));
SurfaceMeshWithBoundingBox obj(mesh,bb);
Bottle bot;
bot.read(obj);
SurfaceMeshWithBoundingBox obj2;
bot.write(obj2);
Bottle bot2;
bot2.read(obj2);
printf("mesh copy: %s -> %s\n", bot.toString().c_str(),
bot2.toString().c_str());
if (obj2.mesh.meshName!=obj.mesh.meshName) {
printf("mesh name not copied correctly\n");
return false;
}
if (obj2.boundingBox.corners.size()!=obj.boundingBox.corners.size()) {
printf("corners not copied correctly\n");
return false;
}
if (bot!=bot2) {
printf("not copied correctly\n");
return false;
}
return true;
}
ConstString NameServer::apply(const ConstString& txt, const Contact& remote) {
ConstString result = "no command given";
mutex.wait();
SplitString ss(txt.c_str());
if (ss.size()>=2) {
ConstString key = ss.get(1);
//YARP_DEBUG(Logger::get(),ConstString("dispatching to ") + key);
ss.set(1,remote.getHost().c_str());
result = dispatcher.dispatch(this,key.c_str(),ss.size()-1,
(char **)(ss.get()+1));
if (result == "") {
Bottle b = ndispatcher.dispatch(this,key.c_str(),ss.size()-1,
(char **)(ss.get()+1));
result = b.toString().c_str();
if (result!="") {
result = result + "\n";
result = terminate(result);
}
}
//YARP_DEBUG(Logger::get(), ConstString("name server request -- ") + txt);
//YARP_DEBUG(Logger::get(), ConstString("name server result -- ") + result);
}
mutex.post();
return result;
}
void oInteractorThread::run() {
count++;
if(locationPort.getInputCount()) {
Bottle* locBottle = &locationPort.prepare();
locBottle=locationPort.read();
if(locBottle!=0) {
printf("Bottle: %s \n", locBottle->toString().c_str());
Bottle* list = locBottle->get(1).asList();
if (list!=0) {
double x = list->get(0).asDouble();
double y = list->get(1).asDouble();
double z = list->get(2).asDouble();
printf("x:%f,y:%f,z:%f \n",x,y,z);
if((x>=-0.4) &&(x<=-0.2)&&(y>=-0.4)&&(y<=0.2)&&(z>=-0.1)&&(z<=0.5)) {
Vector xd(3);
xd(0) = x;
xd(1) = y;
xd(2) = z;
armCart->goToPositionSync(xd);
}
}
int command = locBottle->get(0).asVocab();
}
}
}
bool BufferedConnectionWriter::applyConvertTextMode()
{
if (convertTextModePending) {
convertTextModePending = false;
Bottle b;
StringOutputStream sos;
for (size_t i = 0; i < lst_used; i++) {
yarp::os::ManagedBytes& m = *(lst[i]);
sos.write(m.usedBytes());
}
const std::string& str = sos.str();
b.fromBinary(str.c_str(), (int)str.length());
std::string replacement = b.toString() + "\n";
for (auto& i : lst) {
delete i;
}
lst_used = 0;
target = &lst;
lst.clear();
stopPool();
Bytes data((char*)replacement.c_str(), replacement.length());
appendBlockCopy(data);
}
return true;
}
// \todo TODO bug ?
bool checkRequiredParamIsVectorOfString(yarp::os::Searchable& config,
const std::string& paramName,
std::vector<std::string> & output_vector)
{
bool correct = !(config.findGroup(paramName).isNull());
if( correct )
correct = true;
{
Bottle ids = config.findGroup(paramName).tail();
std::cout << "ids : " << ids.toString() << std::endl;
std::cout << "ids : " << config.find(paramName).toString() << std::endl;
output_vector.resize(ids.size());
for(int i = 0; i < ids.size(); i++ )
{
output_vector[i] = ids.get(i).asString().c_str();
}
}
if( !correct )
{
yError("CanBusInertialMTB: problem loading parameter %s as vector of string",paramName.c_str());
}
return correct;
}
bool DeviceGroup::open(yarp::os::Searchable& config) {
if (implementation==NULL) {
implementation = new DeviceGroupHelper;
}
if (implementation==NULL) {
printf("Out of memory\n");
return false;
}
if (config.check("part","a list of section names, with each section containing a device")) {
Bottle bot = config.findGroup("part").tail();
printf("Assembly of: %s\n", bot.toString().c_str());
for (int i=0; i<bot.size(); i++) {
ConstString name = bot.get(i).asString();
printf(" %s -> %s\n", name.c_str(),
config.findGroup(name).toString().c_str());
bool result = HELPER(implementation).add(name,
config.findGroup(name));
if (!result) {
HELPER(implementation).close();
return false;
}
}
return true;
}
return false;
}
virtual bool read(ConnectionReader& connection) {
Bottle b;
b.read(connection);
// process data in b
printf("Got %s\n", b.toString().c_str());
if(waitForFirst) {
xKeep = b.get(0).asInt();
yKeep = b.get(1).asInt();
waitForFirst = false;
} else {
if((b.get(0).asInt()<xKeep)||(b.get(1).asInt()<yKeep)){
x = 0;
y = 0;
w = 0;
h = 0;
} else {
x = xKeep;
y = yKeep;
w = b.get(0).asInt() - x;
h = b.get(1).asInt() - y;
}
waitForFirst = true;
}
return true;
}
bool cartControlReachAvoidThread::getAvoidanceVectorsFromPort()
{
avoidanceStruct_t avoidanceStruct;
avoidanceStruct.skin_part = SKIN_PART_UNKNOWN;
avoidanceStruct.x.resize(3,0.0);
avoidanceStruct.n.resize(3,0.0);
Bottle* avoidanceMultiBottle = inportAvoidanceVectors.read(false);
if(avoidanceMultiBottle != NULL){
yDebug("getAvoidanceVectorsFromPort(): There were %d bottles on the port.\n",avoidanceMultiBottle->size());
for(int i=0; i< avoidanceMultiBottle->size();i++){
Bottle* avoidanceStructBottle = avoidanceMultiBottle->get(i).asList();
yDebug("Bottle %d contains %s", i,avoidanceStructBottle->toString().c_str());
avoidanceStruct.skin_part = (SkinPart)(avoidanceStructBottle->get(0).asInt());
avoidanceStruct.x(0) = avoidanceStructBottle->get(1).asDouble();
avoidanceStruct.x(1) = avoidanceStructBottle->get(2).asDouble();
avoidanceStruct.x(2) = avoidanceStructBottle->get(3).asDouble();
avoidanceStruct.n(0) = avoidanceStructBottle->get(4).asDouble();
avoidanceStruct.n(1) = avoidanceStructBottle->get(5).asDouble();
avoidanceStruct.n(2) = avoidanceStructBottle->get(6).asDouble();
avoidanceVectors.push_back(avoidanceStruct);
}
return true;
}
else{
yDebug("getAvoidanceVectorsFromPort(): no avoidance vectors on the port.") ;
return false;
};
}
void boardDumperThread::run()
{
//printf("Entering the main thread\n");
//enc->getEncoders(data);
//Bottle bData;
//for (int i = 0; i < numberOfJointsRead; i++)
// {
// dataRead[i] = data[dataMap[i]];
// bData.addDouble(dataRead[i]);
// }
//port->write(bData);
if (getter)
{
//printf("Getter is getting something\n");
getter -> getData(data);
//fprintf(stderr, "Time is %lf \n", stmp.getTime());
Bottle bData;
for (int i = 0; i < numberOfJointsRead; i++)
{
//printf("%.2f \n", data[dataMap[i]]);
dataRead[i] = data[dataMap[i]];
bData.addDouble(dataRead[i]);
}
if (getter->getStamp(stmp))
{
if (stmp.isValid())
{
port->setEnvelope(stmp);
}
else
{
//stmp.update();
stmp=Stamp(-1,0.0);
port->setEnvelope(stmp);
}
}
else
{
fprintf(stderr, "boardDumperThread::warning. Trying to get a stamp without a proper IPreciselyTimed defined. \n");
}
if (logFile)
{
char buff [20];
sprintf(buff,"%d ",stmp.getCount());
fputs (buff,logFile);
sprintf(buff,"%f ",stmp.getTime());
fputs (buff,logFile);
fputs (bData.toString().c_str(),logFile);
fputs ("\n",logFile);
}
port->write(bData);
}
}
void SamIter(void)
{
Bottle *b = bRead.read(false);
//while (bRead.getPendingReads() > 0)
// b = bRead.read(false); // get in the input from the port, if you want it to wait use true, else use false
if(b!=NULL) // check theres data
{
if(b->size()>0)
{
task = b->get(0).asString();
int iStatus=atoi(b->get(1).asString().c_str());
//check if message is related to charging
if(task.compare("ChargingStatus")==0)
{
//relay switch: off mobile base and close modules
if(iStatus==0)
{
yarp::os::Time::delay(10);
CPhidgetInterfaceKit_setOutputState (ifKit, 6, 0);
system("stop.bat");
bCharging=true;
}
else if(iStatus==1 && bCharging==true)//relay switch: on mobile base and start modules
{
CPhidgetInterfaceKit_setOutputState (ifKit, 6, 1);
yarp::os::Time::delay(2);
system("start.bat");
bCharging=false;
}
}
else
{
iflag = iStatus;
//start timer
if(iflag==1)
time (&start);
}
}
std::cout << "got a task " << b->toString().c_str() << std::endl;
}
//send phone ring sensor data only if sensor value is changed
if(bPhoneRingChanged)
{
// send back a bottle with current voltage value
Bottle& b3 = bPhone.prepare(); // prepare the bottle/port
b3.clear();
b3.addInt( iPhoneRing ); // indicates robot voltage
bPhone.writeStrict();
bPhoneRingChanged=false;
}
}
void onRead(Bottle &v)
{
mutex.wait();
Datum=v;
//Time::delay(5);
mutex.post();
fprintf(stderr, "Callback got: %s\n",Datum.toString().c_str());
}
bool MapGrid2D::loadROSParams(string ros_yaml_filename, string& pgm_occ_filename, double& resolution, double& orig_x, double& orig_y, double& orig_t )
{
std::string file_string;
std::ifstream file;
file.open(ros_yaml_filename.c_str());
if (!file.is_open())
{
yError() << "failed to open file" << ros_yaml_filename;
return false;
}
string line;
while (getline(file, line))
{
if (line.find("origin") != std::string::npos)
{
std::replace(line.begin(), line.end(), ',', ' ');
std::replace(line.begin(), line.end(), '[', '(');
std::replace(line.begin(), line.end(), ']', ')');
/*
auto it = line.find('[');
if (it != string::npos) line.replace(it, 1, "(");
it = line.find(']');
if(it != string::npos) line.replace(it, 1, ")");*/
}
file_string += (line + '\n');
}
file.close();
bool ret = true;
Bottle bbb;
bbb.fromString(file_string);
string debug_s = bbb.toString();
if (bbb.check("image:") == false) { yError() << "missing image"; ret = false; }
pgm_occ_filename = bbb.find("image:").asString();
//ppm_flg_filename = (pgm_occ_filename.substr(0, pgm_occ_filename.size()-4))+"_yarpflags"+".ppm";
if (bbb.check("resolution:") == false) { yError() << "missing resolution"; ret = false; }
resolution = bbb.find("resolution:").asDouble();
if (bbb.check("origin:") == false) { yError() << "missing origin"; ret = false; }
Bottle* b = bbb.find("origin:").asList();
if (b)
{
orig_x = b->get(0).asDouble();
orig_y = b->get(1).asDouble();
orig_t = b->get(2).asDouble();
}
if (bbb.check("occupied_thresh:"))
{m_occupied_thresh = bbb.find("occupied_thresh:").asDouble();}
if (bbb.check("free_thresh:"))
{m_free_thresh = bbb.find("free_thresh:").asDouble();}
return ret;
}
bool Agent::fromBottle(Bottle b)
{
if (!this->Object::fromBottle(b))
return false;
if (!b.check("belief")||!b.check("emotions"))
return false;
m_belief.clear();
Bottle* beliefs = b.find("belief").asList();
for(int i=0; i<beliefs->size() ; i++)
{
Bottle* bRelation = beliefs->get(i).asList();
Relation r(*bRelation);
m_belief.push_back(r);
}
m_emotions_intrinsic.clear();
Bottle* emotions = b.find("emotions").asList();
for(int i=0; i<emotions->size() ; i++)
{
Bottle* bEmo = emotions->get(i).asList();
string emotionName = bEmo->get(0).asString().c_str();
double emotionValue = bEmo->get(1).asDouble();
m_emotions_intrinsic[emotionName.c_str()] = emotionValue;
}
m_drives.clear();
Bottle* drivesProperty = b.find("drives").asList();
string drivesDebug = drivesProperty->toString().c_str();
for(int i=0; i<drivesProperty->size() ; i++)
{
Bottle* bD = drivesProperty->get(i).asList();
string drivesDebug1 = bD->toString().c_str();
Drive currentDrive;
currentDrive.fromBottle(*bD);
m_drives[currentDrive.name] = currentDrive;
}
Bottle* bodyProperty = b.find("body").asList();
m_body.fromBottle(*bodyProperty);
return true;
}
Bottle SpeechRecognizerModule::waitNextRecognition(int timeout)
{
yInfo() <<"Recognition: blocking mode on" ;
Bottle bOutGrammar;
bool gotSomething = false;
double endTime = Time::now() + timeout/1000.0;
interruptRecognition = false;
cout << endl ;
yInfo() << "=========== GO Waiting for recog! ===========" ;
while(Time::now()<endTime && !gotSomething && !interruptRecognition)
{
//std::cout<<".";
const float ConfidenceThreshold = 0.3f;
SPEVENT curEvent;
ULONG fetched = 0;
HRESULT hr = S_OK;
m_cpRecoCtxt->GetEvents(1, &curEvent, &fetched);
while (fetched > 0)
{
yInfo() << " received something in waitNextRecognition" ;
gotSomething = true;
ISpRecoResult* result = reinterpret_cast<ISpRecoResult*>(curEvent.lParam);
CSpDynamicString dstrText;
result->GetText(SP_GETWHOLEPHRASE, SP_GETWHOLEPHRASE, TRUE, &dstrText, NULL);
string fullSentence = ws2s(dstrText);
yInfo() <<fullSentence ;
if (m_useTalkBack)
say(fullSentence);
bOutGrammar.addString(fullSentence);
SPPHRASE* pPhrase = NULL;
result->GetPhrase(&pPhrase);
bOutGrammar.addList() = toBottle(pPhrase,&pPhrase->Rule);
yInfo() <<"Sending semantic bottle : "<<bOutGrammar.toString() ;
m_cpRecoCtxt->GetEvents(1, &curEvent, &fetched);
if (m_forwardSound)
{
yarp::sig::Sound& rawSnd = m_portSound.prepare();
rawSnd = toSound(result);
m_portSound.write();
}
}
}
if(interruptRecognition) {
yDebug() << "interrupted speech recognizer!";
}
yInfo() <<"Recognition: blocking mode off";
return bOutGrammar;
}
Bottle learnPrimitive::actionCommand(string sActionName, string sArg){
Bottle bOutput;
//1. check if primitive is known
// vPrimitiveActionBottle =
// open (hand) ( (unfold thumb) (unfold index) (unfold middle) (unfold ring) )
// close (hand) ( (fold thumb) (fold index) (fold middle) (fold ring) )
// b.get(1) b.get(2) b.get(3)
// name arg list of proto-action
Bottle bSubActionList;
for(std::vector<yarp::os::Bottle>::iterator it = vActionBottle.begin(); it < vActionBottle.end(); it++){
string currentName = it->get(0).toString();
//yInfo() << "Current name of the knwon actions : " << currentName ;
if(currentName == sActionName){
yInfo() << "found " << currentName << "as a known complex action";
string currentArg = it->get(1).toString();
if(currentArg == sArg){
yInfo() << "and we have a corresponding argument " << currentArg ;
for(int i = 0; i < it->get(2).asList()->size(); i++){
bSubActionList.addList() = *it->get(2).asList()->get(i).asList() ;
}
break;
} else {
yInfo() << " BUT argument " << currentArg << " does NOT match" ;
}
}
}
if (bSubActionList.size() == 0){
yError() << " error in learnPrimitive::actionCommand | action '" << sActionName << " " << sArg << "' is NOT known";
bOutput.addString("error");
bOutput.addString("action is NOT known");
return bOutput ;
}
yInfo() << "Actions to do : " << bSubActionList.toString() ;
for(int i = 0; i < bSubActionList.size(); i++){
yInfo() << "action #" << i << " : "<< bSubActionList.get(i).asList()->get(0).toString() << " the " << bSubActionList.get(i).asList()->get(1).asList()->get(0).toString() ;
//1. check if subaction is a proto
if ( mProtoActionEnd.find(bSubActionList.get(i).asList()->get(0).toString()) != mProtoActionEnd.end() ) { //proto-action
bOutput.addList() = protoCommand(bSubActionList.get(i).asList()->get(0).toString(), bSubActionList.get(i).asList()->get(1).asList()->get(0).toString());
} else { //primitive
bOutput.addList() = primitiveCommand(bSubActionList.get(i).asList()->get(0).toString(), bSubActionList.get(i).asList()->get(1).asList()->get(0).toString());
yarp::os::Time::delay(2);
} //else { //another action
//}
}
bOutput.addString("ack");
return bOutput;
}