void AwareTouch::sendOPC(const string &partTouch, int &typeTouch, const Vector& posTouch, double sizeTouched)
{
Vector dimensions;
dimensions.clear();
dimensions.push_back(0.05);
dimensions.push_back(0.05);
dimensions.push_back(0.05);
cout<<"Touch Pos:"<<posTouch.toString().c_str()<<endl;
(touchLocation ->m_ego_position) = posTouch;
(touchLocation ->m_dimensions) = dimensions;
touchLocation->m_present = 1.0;
world->commit(touchLocation); // add position of touching
world->addRelation(Relation("icub","is",gestureSet[typeTouch], "touchLocation"), recordingPeriod); // add the relation with gesture
Bottle outEvent;
Bottle &what=outEvent.addList();
what.addString(partTouch.c_str());
what.addString(gestureSet[typeTouch].c_str());
Bottle &where=outEvent.addList();
where.read(const_cast<Vector&>(posTouch));
eventsPort.write(outEvent);
cout<<"icub is being "<< gestureSet[typeTouch]<<" at:"<< posTouch.toString().c_str() <<endl;
lastAutoSnapshotTime = Time::now();
}
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;
}
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;
}
bool SpringyFinger::getSensorsData(Value &data) const
{
map<string,Sensor*>::const_iterator In_0=sensors.find("In_0");
map<string,Sensor*>::const_iterator Out_0=sensors.find("Out_0");
map<string,Sensor*>::const_iterator Out_1=sensors.find("Out_1");
map<string,Sensor*>::const_iterator Out_2=sensors.find("Out_2");
bool ok=true;
ok&=In_0!=sensors.end();
ok&=Out_0!=sensors.end();
ok&=Out_1!=sensors.end();
if (lssvm.getCoDomainSize()>2)
ok&=Out_2!=sensors.end();
if (!ok)
return false;
Value val_in, val_out[3];
In_0->second->getOutput(val_in);
Out_0->second->getOutput(val_out[0]);
Out_1->second->getOutput(val_out[1]);
Vector in(lssvm.getDomainSize());
in[0]=val_in.asDouble();
Vector out(lssvm.getCoDomainSize());
out[0]=val_out[0].asDouble();
out[1]=val_out[1].asDouble();
if (lssvm.getCoDomainSize()>2)
{
Out_2->second->getOutput(val_out[2]);
out[2]=val_out[2].asDouble();
}
Property prop;
Bottle b;
b.addList().read(in);
prop.put("in",b.get(0));
b.addList().read(out);
prop.put("out",b.get(1));
b.addList().read(prop);
data=b.get(2);
return true;
}
Bottle NameServer::ncmdList(int argc, char *argv[]) {
Bottle response;
ConstString prefix = "";
if (argc==1) {
prefix = STR(argv[0]);
}
response.addString("ports");
for (PLATFORM_MAP(ConstString,NameRecord)::iterator it = nameMap.begin(); it!=nameMap.end(); it++) {
NameRecord& rec = PLATFORM_MAP_ITERATOR_SECOND(it);
ConstString iname = rec.getAddress().getRegName();
if (iname.find(prefix)==0) {
if (iname==prefix || iname[prefix.length()]=='/' ||
prefix[prefix.length()-1]=='/') {
if (rec.getAddress().isValid()) {
response.addList() = botify(rec.getAddress());
}
}
}
}
return response;
}
Bottle NameServer::ncmdList(int argc, char *argv[]) {
Bottle response;
std::string prefix;
if (argc==1) {
prefix = STR(argv[0]);
}
response.addString("ports");
for (auto& it : nameMap) {
NameRecord& rec = it.second;
std::string iname = rec.getAddress().getRegName();
if (iname.find(prefix)==0) {
if (iname==prefix || iname[prefix.length()]=='/' ||
prefix[prefix.length()-1]=='/') {
if (rec.getAddress().isValid()) {
response.addList() = botify(rec.getAddress());
}
}
}
}
return response;
}
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;
}
Bottle learnPrimitive::primitiveCommand(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 bProtoActionList; //primitive only composed by proto-action
for(std::vector<yarp::os::Bottle>::iterator it = vPrimitiveActionBottle.begin(); it < vPrimitiveActionBottle.end(); it++){
string currentName = it->get(0).toString();
if(currentName == sActionName){
yInfo() << "found " << currentName << "as a known primitive";
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++){
bProtoActionList.addList() = *it->get(2).asList()->get(i).asList() ;
}
break;
} else {
yInfo() << " BUT argument " << currentArg << " does NOT match" ;
}
}
}
if (bProtoActionList.size() == 0){
yError() << " error in learnPrimitive::primitiveCommand | action '" << sActionName << " " << sArg << "' is NOT known";
bOutput.addString("error");
bOutput.addString("action is NOT known");
return bOutput ;
}
yInfo() << "Actions to do : " << bProtoActionList.toString() ;
for(int i = 0; i < bProtoActionList.size(); i++){
yInfo() << "action #" << i << " : "<< bProtoActionList.get(i).asList()->get(0).toString() << " the " << bProtoActionList.get(i).asList()->get(1).asList()->get(0).toString() ;
bOutput.addList() = protoCommand(bProtoActionList.get(i).asList()->get(0).toString(), bProtoActionList.get(i).asList()->get(1).asList()->get(0).toString());
}
bOutput.addString("ack");
return bOutput;
}
void getBusInfo(NodeArgs& na)
{
unsigned int opaque_id = 1;
ROSReport report;
Value v;
Bottle* connections = v.asList();
mutex.lock();
for (std::multimap<std::string, NodeItem>::iterator it = by_part_name.begin(); it != by_part_name.end(); ++it) {
NodeItem& item = it->second;
if (!(item.isSubscriber() || item.isPublisher())) {
continue;
}
item.update();
item.contactable->getReport(report);
}
mutex.unlock();
for (std::multimap<std::string, std::string>::const_iterator it = report.outgoingURIs.begin(); it != report.outgoingURIs.end(); ++it) {
Bottle& lst = connections->addList();
lst.addInt32(opaque_id); // connectionId
lst.addString(it->second);
lst.addString("o");
lst.addString("TCPROS");
NestedContact nc(it->first);
lst.addString(toRosName(nc.getNestedName()));
opaque_id++;
}
for (std::multimap<std::string, std::string>::const_iterator it = report.incomingURIs.begin(); it != report.incomingURIs.end(); ++it) {
Bottle& lst = connections->addList();
lst.addInt32(opaque_id); // connectionId
lst.addString(it->second);
lst.addString("i");
lst.addString("TCPROS");
NestedContact nc(it->first);
lst.addString(toRosName(nc.getNestedName()));
opaque_id++;
}
if (connections->size() == 0) {
connections->addList(); // add empty list
}
na.reply = v;
na.success();
}
void helperPID::addVectorToOption(Bottle &option, const char *key, const Vector &val)
{
Bottle &bKey=option.addList();
bKey.addString(key);
Bottle &bKeyContent=bKey.addList();
for (size_t i=0; i<val.length(); i++)
bKeyContent.addDouble(val[i]);
}
void ff2LayNN::setItem(Property &options, const string &tag, const Vector &item) const
{
Bottle b; Bottle &v=b.addList();
for (size_t i=0; i<item.length(); i++)
v.addDouble(item[i]);
options.put(tag.c_str(),b.get(0));
}
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 Adjective::asBottle()
{
//Get the Object bottle
Bottle b = this->Entity::asBottle();
Bottle bSub;
bSub.addString("qualityType");
bSub.addString(m_quality.c_str());
b.addList() = bSub;
return b;
}
ConstString toString() {
Bottle bot;
for (PLATFORM_MAP(String,PropertyItem)::iterator
it = data.begin(); it!=data.end(); it++) {
PropertyItem& rec = PLATFORM_MAP_ITERATOR_SECOND(it);
Bottle& sub = bot.addList();
sub.copy(rec.bot);
}
return bot.toString();
}
void setStraightness(const double straightness)
{
ICartesianControl *icart;
action->getCartesianIF(icart);
Bottle options;
Bottle &straightOpt=options.addList();
straightOpt.addString("straightness");
straightOpt.addDouble(straightness);
icart->tweakSet(options);
}
void Localizer::getPidOptions(Bottle &options)
{
mutex.lock();
pid->getOptions(options);
Bottle &bDominantEye=options.addList();
bDominantEye.addString("dominantEye");
bDominantEye.addString(dominantEye.c_str());
mutex.unlock();
}
bool LSSVMCalibrator::toProperty(Property &info) const
{
Bottle data;
Bottle &values=data.addList();
values.addString(impl->toString().c_str());
info.clear();
info.put("type",type.c_str());
info.put("calibration_data",data.get(0));
return Calibrator::toProperty(info);
}
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 PMPthread::Matrix2Bottle(Matrix m)
{
Bottle bot;
for (int i=0; i<m.rows(); i++)
{
Bottle &temp = bot.addList();
for(int j=0; j<m.cols(); j++)
{
temp.addDouble(m(i,j));
}
}
return bot;
}
Bottle YarpPoseController::poses_to_bottle(vector<vector<double> > &path) {
Bottle b;
// b.clear();
for (size_t p(0); p < path.size(); ++p) {
vector<double> &pose(path[p]);
Bottle pose_bottle;
for (size_t n(0); n < pose.size(); n++) {
pose_bottle.addInt((int)pose[n]);
}
b.addList() = pose_bottle;
}
return b;
}
Bottle YarpPoseController::path_to_bottle(vector<vector<vector<double> > > &path) {
Bottle root;
for (int i = 0; i < path.size(); i++) {
Bottle node_l1;
for (int j=0; j < path[i].size(); j++) {
Bottle node_l2;
for (int k=0; k < path[i][j].size(); k++)
node_l2.addDouble(path[i][j][k]);
node_l1.addList() = node_l2;
}
root.addList() = node_l1;
}
return root;
}
bool MatrixCalibrator::toProperty(Property &info) const
{
Bottle data;
Bottle &values=data.addList();
values.addDouble(scale);
for (int r=0; r<H.rows(); r++)
for (int c=0; c<H.cols(); c++)
values.addDouble(H(r,c));
info.clear();
info.put("type",type.c_str());
info.put("calibration_data",data.get(0));
return Calibrator::toProperty(info);
}
bool CalibModule::pushExtrinsics(const string &eye, const Matrix &H)
{
if ((eye!="left") && (eye!="right"))
return false;
Bottle options;
Bottle &ext=options.addList();
ext.addString(eye=="left"?"camera_extrinsics_left":"camera_extrinsics_right");
Bottle &val=ext.addList();
for (int r=0; r<H.rows(); r++)
for (int c=0; c<H.cols(); c++)
val.addDouble(H(r,c));
return igaze->tweakSet(options);
}
bool Calibrator::toProperty(Property &info) const
{
Bottle data;
Bottle &values=data.addList();
values.addString(spatialCompetence.extrapolation?"true":"false");
values.addDouble(spatialCompetence.scale);
values.addDouble(spatialCompetence.c[0]);
values.addDouble(spatialCompetence.c[1]);
values.addDouble(spatialCompetence.c[2]);
for (int r=0; r<spatialCompetence.A.rows(); r++)
for (int c=0; c<spatialCompetence.A.cols(); c++)
values.addDouble(spatialCompetence.A(r,c));
info.put("spatial_competence",data.get(0));
return true;
}
bool NodeHelper::read(ConnectionReader& reader) {
if (!reader.isValid()) return false;
NodeArgs na;
na.request.read(reader);
//printf("NODE API for %s received %s\n",
//name.c_str(),
//na.request.toString().c_str());
ConstString key = na.request.get(0).asString();
na.args = na.request.tail().tail();
if (key=="getBusStats") {
getBusStats(na);
} else if (key=="getBusInfo") {
getBusInfo(na);
} else if (key=="getMasterUri") {
getMasterUri(na);
} else if (key=="shutdown") {
shutdown(na);
} else if (key=="getPid") {
getPid(na);
} else if (key=="getSubscriptions") {
getSubscriptions(na);
} else if (key=="getPublications") {
getPublications(na);
} else if (key=="paramUpdate") {
paramUpdate(na);
} else if (key=="publisherUpdate") {
publisherUpdate(na);
} else if (key=="requestTopic") {
requestTopic(na);
} else {
na.error("I have no idea what you are talking about");
}
if (na.should_drop) {
reader.requestDrop(); // ROS likes to close down.
}
if (reader.getWriter()) {
Bottle full;
full.addInt(na.code);
full.addString(na.msg);
full.addList() = na.reply;
//printf("NODE %s <<< %s\n",
//name.c_str(),
//full.toString().c_str());
full.write(*reader.getWriter());
}
return true;
}
void seriesPID::getOptions(Bottle &options)
{
Vector satLimVect(satLim.rows()*satLim.cols());
for (int r=0; r<satLim.rows(); r++)
for (int c=0; c<satLim.cols(); c++)
satLimVect[r*satLim.cols()+c]=satLim(r,c);
options.clear();
addVectorToOption(options,"Kp",Kp);
addVectorToOption(options,"Ti",Ti);
addVectorToOption(options,"Kd",Kd);
addVectorToOption(options,"N",N);
addVectorToOption(options,"satLim",satLimVect);
Bottle &bTs=options.addList();
bTs.addString("Ts");
bTs.addDouble(Ts);
}
void getPublications(NodeArgs& na)
{
Value v;
Bottle* publications = v.asList();
mutex.lock();
for (std::multimap<std::string, NodeItem>::iterator it = by_part_name.begin(); it != by_part_name.end(); ++it) {
NodeItem& item = it->second;
if (!item.isPublisher()) {
continue;
}
item.update();
Bottle& lst = publications->addList();
lst.addString(toRosName(item.nc.getNestedName()));
lst.addString(item.nc.getTypeName());
}
mutex.unlock();
na.reply = v;
na.success();
}
void getPublications(NodeArgs& na)
{
Value v;
Bottle* publications = v.asList();
mutex.lock();
for (auto& it : by_part_name) {
NodeItem& item = it.second;
if (!item.isPublisher()) {
continue;
}
item.update();
Bottle& lst = publications->addList();
lst.addString(toRosName(item.nc.getNestedName()));
lst.addString(item.nc.getTypeName());
}
mutex.unlock();
na.reply = v;
na.success();
}
/*
* Return the list of beliefs of an agent in an OPC given
* bInput format : getBeliefs real/mental agent
*/
Bottle opcManager::getBeliefs(Bottle bInput)
{
Bottle bOutput;
if (bInput.size() != 3)
{
cout << "Error in opcManager::getBeliefs | wrong size of input" << endl;
bOutput.addString("Error in opcManager::getBeliefs | wrong size of input");
return bOutput;
}
if (bInput.get(1).toString() != "real" && bInput.get(1).toString() != "mental")
{
cout << "Error in opcManager::getBeliefs | unknown OPC (real/mental)" << endl;
bOutput.addString("Error in opcManager::getBeliefs | unknown OPC (real/mental)");
return bOutput;
}
Agent *agent;
if (bInput.get(1).toString() == "real")
{
agent = dynamic_cast<Agent*>(realOPC->getEntity(bInput.get(2).toString().c_str()));
}
else
{
agent = dynamic_cast<Agent*>(mentalOPC->getEntity(bInput.get(2).toString().c_str()));
}
list<Relation> lRelation = agent->beliefs();
cout << endl << agent->name() << " has the following beliefs in the " << bInput.get(1).toString() << " OPC (" << lRelation.size() << ") : " << endl;
for (list<Relation>::iterator it_R = lRelation.begin(); it_R != lRelation.end(); it_R++)
{
Bottle bTemp = it_R->asLightBottle();
cout << bTemp.toString() << endl;
bOutput.addList() = bTemp;
}
cout << endl;
return bOutput;
}
void YarpPoseController::follow_path(Roadmap::path_t &path) {
if (path.path.size() == 0 || path.length > 1000000)
throw StringException("No Path Found");
vector<vector<vector<double> > > crazy_path;
for (size_t i(0); i < path.path.size(); ++i) {
vector<double> pose(d_path_planner->getVertex(path.path[i]).q);
//std::cout << pose.size() << std::endl;
vector<vector<double> > cut_pose = d_path_planner->cut_pose(pose);
crazy_path.push_back(cut_pose);
}
Bottle path_bottle = path_to_bottle(crazy_path);
Bottle command;
command.addString("go");
command.addList() = path_bottle;
Bottle response;
cout << "writing path" << endl;
d_mover.write(command, response);
cout << response.toString() << endl;
cout << "done" << endl;
}