bool FollowingOrder::handlePoint(string type, string target)
{
// Point an object (from human order). Independent of proactivetagging
iCub->opc->checkout();
yInfo() << " [handlePoint] : opc checkout";
list<Entity*> lEntities = iCub->opc->EntitiesCache();
string e_name = target;
// point RPC useless
//bool pointRPC = false;
for (auto& entity : lEntities)
{
string sName = entity->name();
yDebug() << "Checking entity: " << e_name << " to " << sName;//<<endl;
if (sName == e_name) {
if (entity->entity_type() == "object")//|| (*itEnt)->entity_type() == "agent" || (*itEnt)->entity_type() == "rtobject")
{
yInfo() << "I already knew that the object was in the opc: " << sName;
Object* o = dynamic_cast<Object*>(entity);
if(o && o->m_present) {
//pointRPC=true;
yInfo() << "I'd like to point " << e_name;// <<endl;
Object* obj1 = iCub->opc->addOrRetrieveEntity<Object>(e_name);
string sHand = "right";
if (obj1->m_ego_position[1]<0) sHand = "left";
Bottle bHand(sHand);
//iCub->say("I'm going to point the " + target);
iCub->point(e_name, bHand);
iCub->say("oh! this is a " + e_name);
yarp::os::Time::delay(2.0);
iCub->home();
target = "none";//pointList.pop_back();
return true;
}
}
}
}
return false;
}
bool LRH::AREactions(vector<string> seq)
{
string sPredicat, sObject, sLocation, sadverbs;
float ftime;
sPredicat = seq[0];
sObject = seq[1];
sLocation = seq[2];
sadverbs = seq[3];
if (sadverbs == "slowly"){
ftime = 4.0;
}
else if (sadverbs == "quickly"){
ftime = 0.0;
}
else{
ftime = 2.0;
}
if (sPredicat == "none")
{
cout << "Error in reservoirHandler::AREactions | sPredicat == none" << endl;
return false;
}
// GET LOCATION OF THE OBJECT IN THE OPC + OFFSET IN Z
iCub->opc->update();
//RTObject *rtObject = dynamic_cast<RTObject*>(iCub->opc->getEntity(sObject));
Object *rtObject = iCub->opc->addEntity<Object>(sObject);
Vector value(4);
value = rtObject->m_ego_position;
value[2] += offsetGrasp;
cout << sObject << " is at: " << value.toString() << endl;
bool success = true;
if (rtObject->m_present)
{
// GRASP BEGIN
if (sPredicat == "put" || sPredicat == "take" || sPredicat == "grasp")
{
Bottle bHand(sHand);
bHand.addString("still");
cout << "sHand : " << sHand << endl;
Object* Location = dynamic_cast<Object*>(iCub->opc->getEntity(sLocation));
Vector vGoal = Location->m_ego_position;
vGoal[2] += offsetGrasp;
cout << "vGoal is : " << vGoal.toString() << endl;
// DROP ON LOCATION
if (sLocation != " ")
{
Time::delay(ftime);
bool grasped = iCub->getARE()->take(value, bHand, false);
cout << (grasped ? "grasped!" : "missed!") << endl;
success &= grasped;
Time::delay(ftime);
if (grasped){
Bottle opts("over still " + sHand);
bool dropped = iCub->getARE()->dropOn(vGoal, opts, false);
cout << (dropped ? "dropped!" : "missed!") << endl;
Time::delay(ftime);
iCub->getARE()->home();
success &= dropped;
}
}
// DROP WITHOUT LOCATION
else
{
bool grasped = iCub->getARE()->take(value, bHand, false);
cout << (grasped ? "grasped!" : "missed!") << endl;
Time::delay(ftime);
success &= grasped;
if (grasped)
{
iCub->release(value, bHand);
}
Time::delay(ftime);
iCub->home(bHand.toString());
}
// END GRASP
}
// PUSH
else if (sPredicat == "push")
{
sLocation == "right" ? sHand = "left" : sHand = "right";
Bottle bHand(sHand);
cout << "sHand : " << sHand << endl;
Time::delay(ftime);
bool pushed = iCub->getARE()->push(value, bHand, false);
cout << (pushed ? "pushed!" : "missed!") << endl;
Time::delay(ftime);
success &= pushed;
iCub->getARE()->home();
}
// POINT
else if (sPredicat == "point")
{
Time::delay(ftime);
value[1] < 0.0 ? sHand = "left" : sHand = "right";
Bottle bHand(sHand);
cout << "sHand : " << sHand << endl;
bool pointed = iCub->getARE()->point(value, bHand);
cout << (pointed ? "pointed!" : "missed!") << endl;
success &= pointed;
Time::delay(ftime);
iCub->getARE()->home(sHand);
}
}
else
{
//iCub->say(sObject + " is not present");
cout << sObject << " is not present ! " << endl;
success = false;
}
cout << "Result of the action: " << (success ? "success!" : "missed!") << endl;
return success;
}
