Bottle SpeechRecognizerModule::toBottle(SPPHRASE* pPhrase, const SPPHRASERULE* pRule)
{
Bottle bCurrentLevelGlobal;
const SPPHRASERULE* siblingRule = pRule;
while (siblingRule != NULL)
{
Bottle bCurrentSubLevel;
bCurrentSubLevel.addString(ws2s(siblingRule->pszName));
//we backtrack
if(siblingRule->pFirstChild != NULL )
{
bCurrentSubLevel.addList()=toBottle(pPhrase, siblingRule->pFirstChild);
}
else
{
string nodeString = "";
for(unsigned int i=0; i<siblingRule->ulCountOfElements; i++)
{
nodeString += ws2s(pPhrase->pElements[siblingRule->ulFirstElement + i].pszDisplayText);
if (i<siblingRule->ulCountOfElements-1)
nodeString += " ";
}
bCurrentSubLevel.addString(nodeString);
}
siblingRule = siblingRule->pNextSibling;
if (pRule->pNextSibling !=NULL)
bCurrentLevelGlobal.addList() = bCurrentSubLevel;
else
bCurrentLevelGlobal = bCurrentSubLevel;
}
return bCurrentLevelGlobal;
}
/*
* 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;
}
bool SpeechRecognizerModule::respond(const Bottle& cmd, Bottle& reply)
{
reply.addString("ACK");
string firstVocab = cmd.get(0).asString().c_str();
if (firstVocab == "tts")
{
string sentence = cmd.get(1).asString().c_str();
say(sentence);
reply.addString("OK");
}
else if (firstVocab == "RGM" || firstVocab == "rgm" )
{
string secondVocab = cmd.get(1).asString().c_str();
if (secondVocab=="vocabulory")
handleRGMCmd(cmd.tail().tail(), reply);
}
else if (firstVocab == "recog")
{
handleRecognitionCmd(cmd.tail(), reply);
}
else if (firstVocab == "asyncrecog")
{
handleAsyncRecognitionCmd(cmd.tail(), reply);
}
else if (firstVocab == "interrupt")
{
handleInterrupt(cmd.tail(), reply);
}
else
reply.addString("UNKNOWN");
return true;
}
bool PARTICLEModule::respond(const Bottle& command, Bottle& reply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n";
reply.clear();
if (command.get(0).asString()=="quit")
{
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help")
{
cout << helpMessage;
reply.addString("ok");
}
else if (command.get(0).asString()=="reset")
{
cout << "reset has been asked "<< endl;
particleManager->shouldSend=false;
reply.addString("ok");
}
else
{
cout << "command not known - type help for more info" << endl;
}
return true;
}
/*
* Dump in the port DumperPort the human skeleton, and the object of the OPC: sObjectToDump
*
*/
void humanRobotDump::DumpHumanObject()
{
Agent* ag = iCub->opc->addOrRetrieveEntity<Agent>(sAgentName);
Bottle bDump;
bDump.addString(sActionName);
bDump.addString(sObjectToDump);
bDump.addList() = ag->m_body.asBottle();
iCub->opc->checkout();
list<Entity*> lEntity = iCub->opc->EntitiesCacheCopy();
for (list<Entity*>::iterator itEnt = lEntity.begin(); itEnt != lEntity.end(); itEnt++)
{
if ((*itEnt)->entity_type() == EFAA_OPC_ENTITY_AGENT ||
(*itEnt)->entity_type() == EFAA_OPC_ENTITY_OBJECT ||
(*itEnt)->entity_type() == EFAA_OPC_ENTITY_RTOBJECT)
{
if ((*itEnt)->name() != "icub")
{
Object* ob = iCub->opc->addOrRetrieveEntity<Object>((*itEnt)->name());
Bottle bObject;
bObject.addString(ob->name());
bObject.addDouble(ob->m_ego_position[0]);
bObject.addDouble(ob->m_ego_position[1]);
bObject.addDouble(ob->m_ego_position[2]);
bObject.addInt(ob->m_present);
bDump.addList() = bObject;
}
}
}
bDump.addInt(m_iterator);
DumperPort.write(bDump);
}
// rpcPort commands handler
bool respond(const Bottle & command,
Bottle & reply)
{
// This method is called when a command string is sent via RPC
// Get command string
string receivedCmd = command.get(0).asString().c_str();
reply.clear(); // Clear reply bottle
if (receivedCmd == "help")
{
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands are:");
reply.addString("help");
reply.addString("quit");
}
else if (receivedCmd == "quit")
{
reply.addString("Quitting.");
return false; //note also this
}
else
reply.addString("Invalid command, type [help] for a list of accepted commands.");
return true;
}
Bottle Action::asBottle()
{
Bottle b = this->Entity::asBottle();
Bottle bSub;
bSub.addString("description");
bSub.addList() = initialDescription.asBottle();
b.addList() = bSub;
bSub.clear();
bSub.addString("subactions");
Bottle& subs = bSub.addList();
for(list<Action>::iterator sIt = subActions.begin() ; sIt != subActions.end(); sIt++)
{
subs.addList()=sIt->asBottle();
}
b.addList() = bSub;
bSub.clear();
bSub.addString("estimatedDriveEffects");
Bottle &subss = bSub.addList();
for(map<string, double>::iterator sIt = estimatedDriveEffects.begin() ; sIt != estimatedDriveEffects.end(); sIt++)
{
Bottle &ss = subss.addList();
ss.addString(sIt->first.c_str());
ss.addDouble(sIt->second);
}
b.addList() = bSub;
return b;
}
Bottle abmReasoning::connectOPC(Bottle bInput)
{
Bottle bOutput;
if (bInput.size() != 2)
{
bOutput.addString("Error in connect, wrong number of input");
}
if (!bInput.get(1).isString())
{
bOutput.addString("Error in connect, wrong format of input");
}
string sPortTemp = moduleName + "/" + abmReasoningFunction::s_realOPC;
realOPC = new OPCClient(sPortTemp.c_str());
int iTry = 0;
while (!realOPC->isConnected())
{
yInfo() << "\t" << "abmReasoning Connecting to " << abmReasoningFunction::s_realOPC << "..." << realOPC->connect(abmReasoningFunction::s_realOPC);
if (!realOPC->isConnected())
Time::delay(0.5);
iTry++;
if (iTry > 1)
{
yInfo() << "\t" << "abmReasoning failed to connect to " << abmReasoningFunction::s_realOPC;
bOutput.addString("Connection failed, please check your port");
break;
}
}
if (realOPC->isConnected())
{
realOPC->checkout();
realOPC->update();
}
sPortTemp = moduleName + "/" + abmReasoningFunction::s_mentalOPC;
mentalOPC = new OPCClient(sPortTemp.c_str());
iTry = 0;
while (!mentalOPC->isConnected())
{
yInfo() << "\t" << "abmReasoning Connecting to " << abmReasoningFunction::s_mentalOPC << "..." << mentalOPC->connect(abmReasoningFunction::s_mentalOPC);
if (!mentalOPC->isConnected())
Time::delay(0.5);
iTry++;
if (iTry > 1)
{
yInfo() << "\t" << "abmReasoning failed to connect to " << abmReasoningFunction::s_mentalOPC;
bOutput.addString("Connection failed, please check your port");
return bOutput;
}
mentalOPC->isVerbose = false;
}
mentalOPC->checkout();
mentalOPC->update();
bOutput.addString("Connection done");
return bOutput;
}
Bottle PMPthread::initHead(Bottle angles)
{
Bottle reply;
reply.clear();
string msg;
//check if the connection is established. If not, try to connect to DevDriver input ports
if (!Network::exists(("/" + rpcServerName + "/rpc").c_str()) )
{
//printf("Error: device ports not active\n");
reply.addString("Error: device ports not active");
return reply;
}
if( !Network::isConnected(rpcPort.getName().c_str(), ("/" + rpcServerName + "/rpc").c_str()) )
{
if(!Network::connect(rpcPort.getName().c_str(), ("/" + rpcServerName + "/rpc").c_str()) )
{
//printf("Error: unable to connect to device port %s\n", ("/" + rpcServerName + "/head:i").c_str());
msg = "Error: unable to connect to device port /" + rpcServerName + "/rpc";
reply.addString(msg.c_str());
return reply;
}
}
//cout << "thread: " << angles.toString() << endl;
updateSem.wait();
rpcPort.write(angles,reply);
//cout << "out of thread!!!!!!!" << endl;
updateSem.post();
return reply;
}
bool ImageSource::respond(const Bottle& command, Bottle& reply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n" +
"set noise <n> ... set the noise level \n" +
"(where <n> is an integer number in the range 0-255) \n";
reply.clear();
if (command.get(0).asString()=="quit") {
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help") {
cout << helpMessage;
reply.addString("ok");
}
else if (command.get(0).asString()=="set") {
if (command.get(1).asString()=="noise") {
noiseLevel = command.get(2).asInt(); // set parameter value
reply.addString("ok");
}
}
return true;
}
Bottle SimSBox::makeObjectBottle(vector<int>& ind, bool collision) {
Bottle cmd;
cmd.addString("world");
cmd.addString("mk");
cmd.addString("sbox");
cmd.addDouble(sizeX);
cmd.addDouble(sizeY);
cmd.addDouble(sizeZ);
cmd.addDouble(positionX);
cmd.addDouble(positionY);
cmd.addDouble(positionZ);
cmd.addDouble(colorR);
cmd.addDouble(colorG);
cmd.addDouble(colorB);
if (collision == false) {
cmd.addString("false");
cout << "Collision with Static box set to False" << endl;
}
ind[SBOX]++;
objSubIndex=ind[SBOX];
return cmd;
}
void requestTopic(NodeArgs& na)
{
std::string topic = na.args.get(0).asString();
topic = fromRosName(topic);
std::vector<Contact> contacts = query(topic, "+");
if (contacts.size() < 1) {
na.fail("Cannot find topic");
return;
}
for (std::vector<Contact>::iterator it = contacts.begin(); it != contacts.end(); ++it) {
Contact &c = *it;
if (!c.isValid()) {
continue;
}
Value v;
Bottle* lst = v.asList();
lst->addString("TCPROS");
lst->addString(c.getHost());
lst->addInt32(c.getPort());
na.reply = v;
na.success();
return;
}
na.fail("Cannot find topic");
}
bool demoModule::respond(const Bottle& command, Bottle& reply) {
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n" +
"set thr <n> ... set the threshold \n" +
"(where <n> is an integer number) \n";
reply.clear();
if (command.get(0).asString()=="quit") {
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help") {
cout << helpMessage;
reply.addString("ok");
}
else if (command.get(0).asString()=="set") {
if (command.get(1).asString()=="thr") {
thresholdValue = command.get(2).asInt(); // set parameter value
reply.addString("ok");
}
}
return true;
}
bool respond(const Bottle &command, Bottle &reply)
{
int cmd0=command.get(0).asVocab();
if (cmd0==Vocab::encode("stat"))
{
reply.addString(speaker.isSpeaking()?"speaking":"quiet");
return true;
}
if (command.size()>1)
{
int cmd1=command.get(1).asVocab();
if (cmd1==Vocab::encode("opt"))
{
if (cmd0==Vocab::encode("get"))
{
reply.addString(speaker.get_package_options().c_str());
return true;
}
if (cmd0==Vocab::encode("set"))
{
string cmd2=command.get(2).asString().c_str();
speaker.set_package_options(cmd2);
reply.addString("ack");
return true;
}
}
}
return RFModule::respond(command,reply);
}
void GuiUpdaterModule::addObject(Object* o, const string &opcTag)
{
//Get the position of the object in the current reference frame of the robot (not the initial one)
Vector inCurrentRootReference = iCub->getSelfRelativePosition(o->m_ego_position);
//cout<<o->name()<<" init Root: \t \t"<<o->m_ego_position.toString(3,3)<<endl
// <<o->name()<<" current Root: \t \t"<<inCurrentRootReference.toString(3,3)<<endl;
Bottle cmd;
cmd.addString("object");
cmd.addString(opcTag.c_str());
cmd.addDouble(o->m_dimensions[0] *1000.0); // dimX in [mm]
cmd.addDouble(o->m_dimensions[1] *1000.0); // dimY in [mm]
cmd.addDouble(o->m_dimensions[2] *1000.0); // dimZ in [mm]
cmd.addDouble(inCurrentRootReference[0] *1000.0); // posX in [mm]
cmd.addDouble(inCurrentRootReference[1] *1000.0); // posY in [mm]
cmd.addDouble(inCurrentRootReference[2] *1000.0); // posZ in [mm]
cmd.addDouble(o->m_ego_orientation[0] - iCub->m_ego_orientation[0]); // Deal with the object orientation that is moving with the base
cmd.addDouble(o->m_ego_orientation[1] - iCub->m_ego_orientation[1]); // "
cmd.addDouble(o->m_ego_orientation[2] - iCub->m_ego_orientation[2]); // "
cmd.addInt((int)o->m_color[0]); // color R
cmd.addInt((int)o->m_color[1]); // color G
cmd.addInt((int)o->m_color[2]); // color B
cmd.addDouble(1); // alpha coefficient [0,1]
toGui.write(cmd);
}
void utManagerThread::sendGuiTarget()
{
if (outPortGui.getOutputCount()>0)
{
Bottle obj;
obj.addString("object");
obj.addString("utTarget");
// size
obj.addDouble(50.0);
obj.addDouble(50.0);
obj.addDouble(50.0);
// positions
obj.addDouble(1000.0*kalOut[0]);
obj.addDouble(1000.0*kalOut[1]);
obj.addDouble(1000.0*kalOut[2]);
// orientation
obj.addDouble(0.0);
obj.addDouble(0.0);
obj.addDouble(0.0);
// color
obj.addInt(255);
obj.addInt(125);
obj.addInt(125);
// transparency
obj.addDouble(0.9);
outPortGui.write(obj);
}
}
bool visualFilterModule::respond(const Bottle& command, Bottle& reply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n" +
"par <n> <d> ... set parameter's value \n" +
"(where <n> and <d> are an integer and a double respectively) \n";
reply.clear();
if (command.get(0).asString()=="quit") {
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help") {
cout << helpMessage;
reply.addString("ok");
}
else if (command.get(0).asString()=="par") {
int p = command.get(1).asInt();
double v = command.get(2).asDouble();
if(p>0 && p<8 && v>=0){
vfThread->setPar(p,v);
reply.addString("New kernel generated with updated parameters.");
}
else {
reply.addString("Invalid values");
}
}
return true;
}
/* Connect the opc client to an OPC */
Bottle opcManager::connect(Bottle bInput)
{
Bottle bOutput;
if (bInput.size() != 2) {
bOutput.addString("Error in connect, wrong number of input");
}
if (!bInput.get(1).isString()){
bOutput.addString("Error in connect, wrong format of input");
}
realOPC = new OPCClient(moduleName.c_str());
int iTry = 0;
while (!realOPC->isConnected())
{
cout << "Connecting to realOPC..." << realOPC->connect("OPC") << endl;
Time::delay(0.5);
iTry++;
if (iTry > 20)
{
bOutput.addString("Connection failed, please check your port");
return bOutput;
}
}
realOPC->update();
bOutput.addString("Connection done");
return bOutput;
}
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;
}
void CB::YARPConfigurationVariables::setVelocityControlMode(bool mode, string portName) {
bool ok = true;
// specify the local port names that connect to the velocityControl module
string velocityOutputPortName = "/cb/configuration" + deviceName + "/vel:o";
string velocityRPCOutputPortName = "/cb/configuration" + deviceName + "/vel/rpc:o";
// velocityPortName = portName + "/command";
velocityPortName = portName + "/fastCommand";
velocityRPCPortName = portName + "/input";
Bottle b;
if(mode && !velocityControlMode) {
// if we're turning on the velocityControlMode (and it wasnt previously on)
// open and connect the data and config portsport
ok &= velocityPort.open(velocityOutputPortName.c_str());
ok &= Network::connect(velocityOutputPortName.c_str(),velocityPortName.c_str(),"tcp");
Time::delay(0.5);
ok &= velocityRPCPort.open(velocityRPCOutputPortName.c_str());
ok &= Network::connect(velocityRPCOutputPortName.c_str(),velocityRPCPortName.c_str(),"tcp");
// send gain and maxVel paramaters to the vc module
for(int i=0; i<mask.size(); i++) {
if(mask[i]) {
cout << "setting vc params joint[" << i << "]: gain=" << velocityGain << ", svel=" << maxSetVal << endl;
b.clear();
b.addString("gain");
b.addInt(i);
b.addDouble(velocityGain);
velocityRPCPort.write(b);
Time::delay(0.1);
b.clear();
b.addString("svel");
b.addInt(i);
b.addDouble(maxSetVal);
velocityRPCPort.write(b);
Time::delay(0.1);
}
}
} else if(!mode && velocityControlMode) {
// turning off velocity control mode by disconnecting and closing ports
ok &= Network::disconnect(velocityOutputPortName.c_str(),velocityPortName.c_str());
ok &= Network::disconnect(velocityRPCOutputPortName.c_str(),velocityRPCPortName.c_str());
velocityRPCPort.close();
velocityPort.close();
}
// set the current mode
velocityControlMode = mode;
}
bool SpeechRecognizerModule::handleAsyncRecognitionCmd(const Bottle& cmd, Bottle& reply)
{
HRESULT hr;
string firstVocab = cmd.get(0).asString().c_str();
if (firstVocab == "getGrammar")
{
reply.addString("NOT_IMPLEMENTED");
return true;
}
if (firstVocab == "clear")
{
bool everythingIsFine=true;
SPSTATEHANDLE rootRule;
everythingIsFine &= SUCCEEDED(m_cpGrammarFromFile->SetGrammarState(SPGS_DISABLED));
everythingIsFine &= SUCCEEDED(m_cpGrammarFromFile->GetRule(L"rootRule", NULL, SPRAF_TopLevel | SPRAF_Active, TRUE, &rootRule));
everythingIsFine &= SUCCEEDED(m_cpGrammarFromFile->ClearRule(rootRule));
everythingIsFine &= SUCCEEDED(hr = m_cpGrammarFromFile->Commit(NULL));
everythingIsFine &= SUCCEEDED(m_cpGrammarFromFile->SetGrammarState(SPGS_ENABLED));
everythingIsFine &= SUCCEEDED(m_cpGrammarFromFile->SetRuleState(NULL, NULL, SPRS_ACTIVE));
everythingIsFine &= SUCCEEDED(m_cpRecoCtxt->Resume(0));
reply.addString("Cleared");
return true;
}
if (firstVocab == "addGrammar")
{
string grammar = cmd.get(1).asString().c_str();
bool everythingIsFine = setGrammarCustom(m_cpGrammarFromFile,grammar,true);
reply.addString("Added");
return true;
}
if (firstVocab == "loadXML")
{
string xml = cmd.get(1).asString().c_str();
ofstream fileTmp("grammarTmp.grxml");
fileTmp<<xml;
fileTmp.close();
std::wstring tmp = s2ws("grammarTmp.grxml");
LPCWSTR cwgrammarfile = tmp.c_str();
bool everythingIsFine =true;
//everythingIsFine &= SUCCEEDED( m_cpRecoCtxt->CreateGrammar( 1, &m_cpGrammarFromFile ));
everythingIsFine &= SUCCEEDED( m_cpGrammarFromFile->SetGrammarState(SPGS_DISABLED));
everythingIsFine &= SUCCEEDED( m_cpGrammarFromFile->LoadCmdFromFile(cwgrammarfile, SPLO_DYNAMIC));
everythingIsFine &= SUCCEEDED( m_cpGrammarFromFile->SetGrammarState(SPGS_ENABLED));
everythingIsFine &= SUCCEEDED(m_cpGrammarFromFile->SetRuleState(NULL, NULL, SPRS_ACTIVE));
everythingIsFine &= SUCCEEDED(m_cpRecoCtxt->Resume(0));
refreshFromVocabulories(m_cpGrammarFromFile);
reply.addString("Loaded");
return true;
}
return false;
}
void SimoxHandEyeCalibrationWindow::processAllBlobs()
{
cout << "processing image (search all blobs)..." << endl;
Bottle cmd;
cmd.addString("process");
cmd.addString("blobs");
sendToHandTracker(cmd);
}
yarp::os::Bottle NameServer::ncmdVersion(int argc, char *argv[]) {
YARP_UNUSED(argc);
YARP_UNUSED(argv);
Bottle response;
response.addString("version");
response.addString(YARP_VERSION);
return response;
}
Bottle PMPthread::initTorso(Bottle angles)
{
Bottle reply;
reply.clear();
string msg;
//check if the connection is established. If not, try to connect to DevDriver input ports
if (!Network::exists(("/" + rpcServerName + "/rpc").c_str()) )
{
//printf("Error: device ports not active\n");
reply.addString("Error: device ports not active");
return reply;
}
if( !Network::isConnected(rpcPort.getName().c_str(), ("/" + rpcServerName + "/rpc").c_str()) )
{
if(!Network::connect(rpcPort.getName().c_str(), ("/" + rpcServerName + "/rpc").c_str()) )
{
//printf("Error: unable to connect to device port %s\n", ("/" + rpcServerName + "/head:i").c_str());
msg = "Error: unable to connect to device port /" + rpcServerName + "/rpc";
reply.addString(msg.c_str());
return reply;
}
}
double t[3] = {0.0, 0.0, 0.0};
// default initialization
if (angles.size() == 1)
{
for (int i=0; i<3; i++) angles.addDouble(t[i]);
}
// else user defined initialization
updateSem.wait();
rpcPort.write(angles,reply);
// update PMP joint angles
if (reply.get(0).asString() == "Done")
{
PMP->q_rightArm(0) = PMP->q_leftArm(0) = angles.get(0).asDouble()*M_PI/180;
PMP->q_rightArm(1) = PMP->q_leftArm(1) = angles.get(1).asDouble()*M_PI/180;
PMP->q_rightArm(2) = PMP->q_leftArm(2) = angles.get(2).asDouble()*M_PI/180;
PMP->Rchain->setAng(PMP->q_rightArm);
PMP->Lchain->setAng(PMP->q_leftArm);
PMP->x_0R = PMP->get_EEPose("right");
PMP->x_0L = PMP->get_EEPose("left");
//update VTGS starting position
VTGS_r->setStartingPoint(PMP->x_0R);
VTGS_l->setStartingPoint(PMP->x_0L);
}
updateSem.post();
return reply;
}
void MainWindow::stepFromCommand(Bottle &reply)
{
if (subDirCnt > 0){
utilities->stepThread();
reply.addString("ok");
} else {
reply.addString("error");
}
}
Bottle SimWorld::deleteAll() {
Bottle cmd;
cmd.addString("world");
cmd.addString("del");
cmd.addString("all");
resetSimObjectIndex();
return cmd;
}
void rndMove()
{
Bottle cmd;
Bottle response;
cmd.addString("go");
cmd.addString(randomDirection());
port.write(cmd, response);
}
void rndShoot()
{
Bottle cmd;
Bottle response;
cmd.addString("fire");
cmd.addString(randomDirection());
port.write(cmd, response);
}
void testClear() {
report(0,"testing clear...");
Bottle b;
b.addString("hello");
checkEqual(1,b.size(),"size ok");
b.clear();
b.addString("there");
checkEqual(1,b.size(),"size ok");
}
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;
}
