void SpecificWorker::action_ChangeRoom(bool newAction)
{
static float lastX = std::numeric_limits<float>::quiet_NaN();
static float lastZ = std::numeric_limits<float>::quiet_NaN();
auto symbols = worldModel->getSymbolsMap(params, "r2", "robot");
try
{
printf("trying to get _in_ edge from %d to %d\n", symbols["robot"]->identifier, symbols["r2"]->identifier);
AGMModelEdge e = worldModel->getEdge(symbols["robot"], symbols["r2"], "in");
printf("STOP! WE ALREADY GOT THERE!\n");
stop();
return;
}
catch(...)
{
}
int32_t roomId = symbols["r2"]->identifier;
printf("room symbol: %d\n", roomId);
std::string imName = symbols["r2"]->getAttribute("imName");
printf("imName: <%s>\n", imName.c_str());
const float refX = str2float(symbols["r2"]->getAttribute("x"));
const float refZ = str2float(symbols["r2"]->getAttribute("z"));
QVec roomPose = innerModel->transformS("world", QVec::vec3(refX, 0, refZ), imName);
roomPose.print("goal pose");
// AGMModelSymbol::SPtr goalRoom = worldModel->getSymbol(str2int(params["r2"].value));
// const float x = str2float(goalRoom->getAttribute("tx"));
// const float z = str2float(goalRoom->getAttribute("tz"));
const float x = roomPose(0);
const float z = roomPose(2);
bool proceed = true;
if ( (planningState.state=="PLANNING" or planningState.state=="EXECUTING") )
{
if (abs(lastX-x)<10 and abs(lastZ-z)<10)
proceed = false;
else
printf("proceed because the coordinates differ (%f, %f), (%f, %f)\n", x, z, lastX, lastZ);
}
else
{
printf("proceed because it's stoped\n");
}
if (proceed)
{
lastX = x;
lastZ = z;
printf("changeroom to %d\n", symbols["r2"]->identifier);
go(x, z, 0, false, 0, 0, 1200);
}
else
{
}
}
///UNFiNISHED
bool Sampler::searchRobotValidStateCloseToTarget(QVec& target)
{
//If current is good, return
if( std::get<bool>(checkRobotValidStateAtTarget(target,QVec::zeros(3))) == true)
return true;
target.print("target");
//Start searching radially from target to origin and adding the vertices of a n regular polygon of radius 1000 and center "target"
const int nVertices = 12;
const float radius = 1000.f;
QVec lastPoint, minVertex, vertex;
float fi,vert;
float minDist = radius;
for(int i=0; i< nVertices; i++)
{
fi = (2.f*M_PI/nVertices) * i;
int k;
bool free;
for(k=100; k<radius; k=k+100)
{
vertex = QVec::vec3(target.x() + k*sin(fi), target.y(), target.z() + k*cos(fi));
free = std::get<bool>(checkRobotValidStateAtTarget(vertex, QVec::zeros(3)));
if (free == true)
break;
}
if( free and k < minDist )
{
minVertex = vertex;
minDist = k;
vert = fi;
}
}
if( minDist < radius)
{
target = minVertex;
target.print("new target");
qDebug() << minDist << vert;
return true;
}
else
return false;
}
void SpecificWorker::action_ReachPose(bool newAction)
{
printf("action_ReachPose,%d: %d\n", __LINE__, newAction);
static float lastX = std::numeric_limits<float>::quiet_NaN();
printf("action_ReachPose,%d: %d\n", __LINE__, newAction);
static float lastZ = std::numeric_limits<float>::quiet_NaN();
printf("action_ReachPose,%d: %d\n", __LINE__, newAction);
auto symbols = worldModel->getSymbolsMap(params, "room", "pose");
printf("action_ReachPose,%d: %d\n", __LINE__, newAction);
int32_t poseId = symbols["pose"]->identifier;
printf("pose symbol: %d\n", poseId);
std::string imName = symbols["pose"]->getAttribute("imName");
printf("imName: <%s>\n", imName.c_str());
QVec pose = innerModel->transform6D("world", QString::fromStdString(imName));
pose.print("goal pose");
const float x = pose(0);
const float z = pose(2);
const float ry = pose(4);
bool proceed = true;
if ( (planningState.state=="PLANNING" or planningState.state=="EXECUTING") )
{
if (abs(lastX-x)<10 and abs(lastZ-z)<10)
proceed = false;
else
printf("proceed because the coordinates differ (%f, %f), (%f, %f)\n", x, z, lastX, lastZ);
}
else
{
printf("proceed because it's stoped\n");
}
if (proceed)
{
lastX = x;
lastZ = z;
printf("setpose %d\n", symbols["room"]->identifier);
go(x, z, ry, true);
}
else
{
}
}
void Worker::compute()
{
/// Clear laser measurement
for (int32_t i=0; i<LASER_SIZE; ++i)
{
(*laserDataW)[i].dist = maxLength;
}
/// Update InnerModel with joint information
if (updateJoint)
{
RoboCompJointMotor::MotorStateMap motorMap;
try
{
jointmotor->getAllMotorState(motorMap);
for (RoboCompJointMotor::MotorStateMap::iterator it=motorMap.begin(); it!=motorMap.end(); ++it)
{
innerModel->updateJointValue(it->first.c_str(), it->second.pos);
}
}
catch (const Ice::Exception &ex)
{
cout << "Can't connect to jointMotor: " << ex << endl;
}
}
else
{
printf("not using joint\n");
}
/// FOR EACH OF THE CONFIGURED PROXIES
// #pragma omp parallel for
for (uint r=0; r<rgbds.size(); ++r)
{
#ifdef STORE_POINTCLOUDS_AND_EXIT
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); // PCL
#endif
if (rgbds[r].bus == true) /// If the proxy is a bus
{
/// FOR EACH OF THE CAMERAS OF THE BUS
RoboCompRGBDBus::CameraList clist;
clist.resize(1);
RoboCompRGBDBus::CameraParamsMap::iterator iter;
for (iter=rgbds[r].cameras.begin(); iter!=rgbds[r].cameras.end(); iter++)
{
if (iter->first == rgbds[r].id)
{
clist[0] = iter->first;
RoboCompRGBDBus::ImageMap images;
try
{
if (DECIMATION_LEVEL == 0)
rgbds[r].proxyRGBDBus->getImages(clist,images);
else
rgbds[r].proxyRGBDBus->getDecimatedImages(clist, DECIMATION_LEVEL, images);
}
catch (const Ice::Exception &ex)
{
cout << "Can't connect to rgbd: " << ex << endl;
continue;
}
/// Get the corresponding (stored) protocloud
RoboCompRGBDBus::PointCloud pointCloud = rgbds[r].protoPointClouds[clist[0]];
/// Multiply the protocloud by the depth
for (uint32_t pi=0; pi<pointCloud.size(); pi++)
{
pointCloud[pi].x *= images[iter->first].depthImage[pi];
pointCloud[pi].y *= images[iter->first].depthImage[pi];
pointCloud[pi].z = images[iter->first].depthImage[pi];
}
/// Inserts the resulting points in the virtual laser
RTMat TR = innerModel->getTransformationMatrix(base, QString::fromStdString(iter->first));
#ifdef STORE_POINTCLOUDS_AND_EXIT
cloud->points.resize(pointCloud.size());
#endif
for (uint32_t ioi=0; ioi<pointCloud.size(); ioi+=3)
{
if ((not isnan(images[iter->first].depthImage[ioi])) and images[iter->first].depthImage[ioi] > 10)
{
QVec p = (TR * QVec::vec4(pointCloud[ioi].x, pointCloud[ioi].y, pointCloud[ioi].z, 1)).fromHomogeneousCoordinates();
#ifdef STORE_POINTCLOUDS_AND_EXIT
cloud->points[ioi].x = p(0)/1000;
cloud->points[ioi].y = p(1)/1000;
cloud->points[ioi].z = -p(2)/1000;
#endif
if ( (p(1)>=minHeight and p(1)<=maxHeight) /* or (p(1)<minHeightNeg) */)
{
p(1) = 0;
float d = sqrt(p(0)*p(0) + p(2)*p(2));
if (d>maxLength) d = maxLength;
const float a = atan2(p(0), p(2));
const int32_t bin = angle2bin(a);
if (bin>=0 and bin<LASER_SIZE and (*laserDataW)[bin].dist > d)
{
if ( (*laserDataW)[bin].dist > d)
(*laserDataW)[bin].dist = d;
}
}
}
}
}
}
}
else /// If the proxy is a good old RGBD interface
{
RoboCompRGBD::PointSeq points;
try
{
rgbds[r].proxyRGBD->getXYZ(points, hState, bState);
}
catch (const Ice::Exception &ex)
{
cout << "Can't connect to rgbd: " << ex << endl;
continue;
}
RTMat TR = innerModel->getTransformationMatrix(base, QString::fromStdString(rgbds[r].id));
#ifdef STORE_POINTCLOUDS_AND_EXIT
cloud->points.resize(points.size());
#endif
// printf("%d\n", __LINE__);
uint32_t pw = 640;
uint32_t ph = 480;
uint32_t step = 13;
if (points.size() == 320*240) { pw=320; ph=240; step=11; }
if (points.size() == 160*120) { pw=160; ph=120; step=5; }
if (points.size() == 80*60) { pw=80; ph=60; step=3; }
for (uint32_t rr=0; rr<ph; rr+=step)
{
for (uint32_t cc=rr%5; cc<pw; cc+=2)
{
uint32_t ioi = rr*pw+cc;
if (ioi<points.size())
{
const QVec p = (TR * QVec::vec4(points[ioi].x, points[ioi].y, points[ioi].z, 1)).fromHomogeneousCoordinates();
#ifdef STORE_POINTCLOUDS_AND_EXIT
cloud->points[ioi].x = p(0)/1000;
cloud->points[ioi].y = p(1)/1000;
cloud->points[ioi].z = -p(2)/1000;
#endif
if ( (p(1)>=minHeight and p(1)<=maxHeight) or (p(1)<minHeightNeg) )
{
// p(1) = 0;
float d = sqrt(p(0)*p(0) + p(2)*p(2));
if (d>maxLength) d = maxLength;
const float a = atan2(p(0), p(2));
const int32_t bin = angle2bin(a);
if (bin>=0 and bin<LASER_SIZE and (*laserDataW)[bin].dist > d)
{
(*laserDataW)[bin].dist = d;
}
}
}
}
}
// printf("%d\n", __LINE__);
}
#ifdef STORE_POINTCLOUDS_AND_EXIT
writePCD(rgbds[r].id+".pcd", cloud);
#endif
}
#ifdef STORE_POINTCLOUDS_AND_EXIT
qFatal("done");
#endif
try
{
RoboCompLaser::TLaserData alData = laser->getLaserData();
for (uint i=0; i<alData.size(); i++)
{
if (i==alData.size()/2) printf("PC %d (%f _ %f)\n", i, alData[i].dist, alData[i].angle);
const QVec p = innerModel->laserTo(actualLaserID, actualLaserID, alData[i].dist, alData[i].angle);
if (i==alData.size()/2) p.print("en base");
if (i==alData.size()/2) printf("(%s)", base.toStdString().c_str());
const float angle = atan2(p(0), p(2));
const float dist = p.norm2();
if (i==alData.size()/2) printf("enlaser %f %f\n", dist, angle);
const int j = LASER_SIZE*angle/FOV + (LASER_SIZE/2);
if (i==alData.size()/2) printf("index %d\n", j);
// printf("FOV:%f, angle:%f, LASER_SIZE=%f, j:%d\n", (float)FOV, angle, (float)LASER_SIZE, j);
if (j>=0 and j<(int)laserDataW->size())
{
if ((*laserDataW)[j].dist > dist)
{
(*laserDataW)[j].dist = dist;
}
}
}
}
catch (const Ice::Exception &ex)
{
cout << "Can't connect to laser: " << ex << endl;
}
RoboCompOmniRobot::TBaseState oState;
try { omnirobot->getBaseState(oState); }
catch (Ice::Exception e) { qDebug()<<"error talking to base"<<e.what(); }
bStateOut.x = oState.x;
bStateOut.z = oState.z;
bStateOut.alpha = oState.alpha;
// Double buffer swap
RoboCompLaser::TLaserData *t = laserDataR;
mutex->lock();
laserDataR = laserDataW;
mutex->unlock();
innerModel->updateTransformValues("robot", bStateOut.x, 0, bStateOut.z, 0, bStateOut.alpha,0);
// printf("%f %f ___ %f\n", bStateOut.x, bStateOut.z, bStateOut.alpha);
#ifdef USE_EXTENSION
extended->update(*laserDataR);
static QTime te = QTime::currentTime();
float re = 11.*te.elapsed()/1000.;
te = QTime::currentTime();
// printf("S ------------ %d %f\n", extended->size(), re);
extended->relax(re, innerModel, "laser", "root");
#endif
// medianFilter();
laserDataW = t;
}
/**
* \brief Called when the robot is sent close to an object's location
*/
void SpecificWorker::action_SetObjectReach(bool newAction)
{ // Get symbols' map
std::map<std::string, AGMModelSymbol::SPtr> symbols;
try
{
symbols = worldModel->getSymbolsMap(params); //ALL THE SYMBOLS GIVEN IN THE RULE
}
catch(...)
{
printf("navigationAgent: Couldn't retrieve action's parameters\n");
printf("<<WORLD\n");
AGMModelPrinter::printWorld(worldModel);
printf("WORLD>>\n");
if (worldModel->size() > 0) { exit(-1); }
}
// Get target
int roomID, objectID, robotID;
try
{
if (symbols["room"].get() and symbols["object"].get() and symbols["robot"].get() )
{
roomID = symbols["room"]->identifier;
objectID =symbols["object"]->identifier;
robotID = symbols["robot"]->identifier;
try // If we can access the 'reach' edge for the object of the action
{ // is not really necessary. The planner is probably replanning.
worldModel->getEdgeByIdentifiers(objectID, objectID, "reach");
{
static QTime lastMsg = QTime::currentTime().addSecs(-1000);
if (lastMsg.elapsed() > 1000)
{
rDebug2(("navigationAgent ignoring action setObjectReach (object currently reached)"));
lastMsg = QTime::currentTime();
return;
}
printf("ask the platform to stop\n");
stop();
}
}
catch(...)
{
}
}
else
{
printf("parameters not in the model yet\n");
return;
}
}
catch(...)
{
printf("ERROR: SYMBOL DOESN'T EXIST \n");
exit(2);
}
// GET THE INNERMODEL NAMES OF TH SYMBOLS
QString robotIMID;
QString roomIMID;
QString objectIMID;
try
{
robotIMID = QString::fromStdString(worldModel->getSymbol(robotID)->getAttribute("imName"));
roomIMID = QString::fromStdString(worldModel->getSymbol(roomID)->getAttribute("imName"));
objectIMID = QString::fromStdString(worldModel->getSymbol(objectID)->getAttribute("imName"));
// check if object has reachPosition
AGMModelSymbol::SPtr object = worldModel->getSymbol(objectID);
for (auto edge = object->edgesBegin(worldModel); edge != object->edgesEnd(worldModel); edge++)
{
if (edge->getLabel() == "reachPosition")
{
const std::pair<int32_t, int32_t> symbolPair = edge->getSymbolPair();
objectID = symbolPair.second;
objectIMID = QString::fromStdString(worldModel->getSymbol(objectID)->getAttribute("imName"));
qDebug() << __FUNCTION__ << "Target object " << symbolPair.first<<"->"<<symbolPair.second<<" object "<<objectIMID;
}
}
}
catch(...)
{
printf("ERROR IN GET THE INNERMODEL NAMES\n");
exit(2);
}
// GET THE TARGET POSE:
RoboCompTrajectoryRobot2D::TargetPose tgt;
try
{
if (not (innerModel->getNode(roomIMID) and innerModel->getNode(objectIMID)))
{
printf("Cant find objects to reach %d\n", __LINE__);
return;
}
QVec poseInRoom = innerModel->transform6D(roomIMID, objectIMID); // FROM OBJECT TO ROOM
qDebug() << __FUNCTION__ <<" Target pose: "<< poseInRoom;
tgt.x = poseInRoom.x();
tgt.y = 0;
tgt.z = poseInRoom.z();
tgt.rx = 0;
//tgt.ry = 0;
tgt.ry = poseInRoom.ry(); //needed to reach tables
tgt.rz = 0;
tgt.doRotation = true;
}
catch (...)
{
qDebug()<< __FUNCTION__ << "InnerModel Exception. Element not found in tree";
}
// Execute target
try
{
// if (!haveTarget)
{
try
{
QVec O = innerModel->transform("shellyArm_grasp_pose", objectIMID);
//O.print(" O pose relativa");
//qDebug() << __FUNCTION__ << "O norm:" << O.norm2();
QVec graspRef = innerModel->transform("robot", "shellyArm_grasp_pose");
graspRef.print("graspRef");
float th=20;
tgt.ry = -3.1415926535/2.;
go(tgt.x, tgt.z, tgt.ry, tgt.doRotation, graspRef.x(), graspRef.z(), th);
qDebug() << __FUNCTION__ << "trajectoryrobot2d->go(" << tgt.x << ", " << tgt.z << ", " << tgt.ry << ", " << graspRef.x() << ", " << graspRef.z() << " )\n";
haveTarget = true;
}
catch(const RoboCompTrajectoryRobot2D::RoboCompException &ex)
{
std::cout << ex << " " << ex.text << std::endl;
throw;
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
string state;
try
{
state = trajectoryrobot2d_proxy->getState().state;
}
catch(const Ice::Exception &ex)
{
std::cout <<"trajectoryrobot2d->getState().state "<< ex << std::endl;
throw ex;
}
//state="IDLE";
std::cout<<state<<" haveTarget "<<haveTarget;
if (state=="IDLE" && haveTarget)
{
//std::cout<<"\ttrajectoryrobot2d_proxy->getState() "<<trajectoryrobot2d_proxy->getState().state<<"\n";
try
{
// AGMModel::SPtr newModel(new AGMModel(worldModel));
// int statusID =symbols["status"]->identifier;
// newModel->getEdgeByIdentifiers(objectID, statusID, "noReach").setLabel("reach");
// sendModificationProposal(worldModel, newModel);
haveTarget=false;
}
catch (...)
{
std::cout<<"\neeeee"<< "\n";
}
}
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
void LMap::update_timeAndPositionIssues(QString actualLaserID)
{
// First, if virtualLaserID is far from the movable root, move the movableRootID reference.
const QVec relPose = innerModel->transform(movableRootID, virtualLaserID);
float distFromReference = innerModel->transform(movableRootID, virtualLaserID).norm2();
float margin = 0.5*side-laserRange;
static bool first = true;
if (distFromReference >= margin or first)
{
first = false;
QVec relPosePixels = relPose.operator*(float(bins)/float(side));
relPosePixels.print("relPosePixels");
mapLaser = offsetImageWithPadding(mapLaser, relPosePixels(0), -relPosePixels(2));
mapRGBDs = offsetImageWithPadding(mapRGBDs, relPosePixels(0), -relPosePixels(2));
relPosePixels.print("relPosePixels");
QString mrID_parent = innerModel->getParentIdentifier(movableRootID);
QVec pFromMRIDp = innerModel->transform(mrID_parent, virtualLaserID);
innerModel->updateTransformValues(movableRootID, pFromMRIDp(0), pFromMRIDp(1), pFromMRIDp(2), 0,0,0);
printf("moving movableRootID %f\n", distFromReference);
}
// Decrease the obstacle certainty
const float forgetRateSubtractLaser = 7;
const float forgetSubtractLaser = float(forgetRateSubtractLaser*lastForgetSubtractLaser.elapsed())/1000.;
if (forgetSubtractLaser > 2)
{
lastForgetSubtractLaser = QTime::currentTime();
for (int x=0; x<bins; x++)
{
for (int z=0; z<bins; z++)
{
int64_t t = mapLaser.at<uchar>(z, x);
if (t > 128)
{
t -= forgetSubtractLaser;
if (t<128) t = 128;
mapLaser.at<uchar>(z, x) = t;
}
}
}
}
// Decrease the obstacle certainty
const float forgetRateSubtractRGBD = 5;
const float forgetSubtractRGBD = float(forgetRateSubtractRGBD*lastForgetSubtractRGBD.elapsed())/1000.;
if (forgetSubtractRGBD > 2)
{
lastForgetSubtractRGBD = QTime::currentTime();
for (int x=0; x<bins; x++)
{
for (int z=0; z<bins; z++)
{
int64_t t = mapRGBDs.at<uchar>(z, x);
if (t > 128)
{
t -= forgetSubtractRGBD;
if (t<128) t = 128;
mapRGBDs.at<uchar>(z, x) = t;
}
}
}
}
// Increase the obstacle certainty
const float forgetRateAdd = 7;
const float forgetAdd = float(forgetRateAdd*lastForgetAdd.elapsed())/1000.;
if (forgetAdd > 5)
{
lastForgetAdd = QTime::currentTime();
for (int x=0; x<bins; x++)
{
for (int z=0; z<bins; z++)
{
int64_t t;
t = mapLaser.at<uchar>(z, x);
if (t < 128)
{
t += forgetAdd;
if (t>128) t = 128;
mapLaser.at<uchar>(z, x) = t;
}
t = mapRGBDs.at<uchar>(z, x);
if (t < 128)
{
t += forgetAdd;
if (t>128) t = 128;
mapRGBDs.at<uchar>(z, x) = t;
}
}
}
}
}
void SpecificWorker::ballTouched(bool first)
{
QTime fff;
if (first) fff = QTime::currentTime();
// if (tocaButton->isChecked())
{
tocaButton->setText("tocando");
const int32_t ball = atoi(params["b"].value.c_str());
const int32_t robot = atoi(params["r"].value.c_str());
printf("\n\n----------\n");
try
{
// Get
const float tx = str2float(worldModel->getSymbol(ball)->getAttribute("tx"));
const float ty = str2float(worldModel->getSymbol(ball)->getAttribute("ty"));
const float tz = str2float(worldModel->getSymbol(ball)->getAttribute("tz"));
const QVec offset = QVec::vec3(0., 0., -100.);
const QVec targetRobot = QVec::vec3(tx, ty, tz) + offset;
const QVec actualWristPosition = QVec::vec3(
str2float(worldModel->getSymbol(robot)->attributes["rightwrist_tx"]),
str2float(worldModel->getSymbol(robot)->attributes["rightwrist_ty"]),
str2float(worldModel->getSymbol(robot)->attributes["rightwrist_tz"]));
actualWristPosition.print("actualWristPosition");
const QVec error = targetRobot-actualWristPosition;
// If the hand is far from the target, move the hand
int32_t extra = 0.015 * (fff.elapsed() - 2000);
if (extra<0) extra = 0;
if (extra>200) extra = 200;
int32_t umbral = 100. + extra;
if (error.norm2() > umbral)
{
targetRobot.print("targetRobot");
actualWristPosition.print("actualWristPosition");
error.print("error");
printf("ERROR: %f\n", error.norm2());
printf("UMBRAL: %d\n", umbral);
const QVec poseTr = innerModel->transform("world", targetRobot, "robot");
printf("move hand to T=(%.2f, %.2f, %.2f) \n", poseTr(0), poseTr(1), poseTr(2));
sendRightHandPose(poseTr, QVec::vec3(0,0,0), QVec::vec3(1,1,1), QVec::vec3(0,0,0));
}
// If the hand is close to the target, acknowledge the new state
else
{
printf("lo conseguimos!!!\n");
printf("lo conseguimos!!!\n");
printf("lo conseguimos!!!\n");
printf("lo conseguimos!!!\n");
printf("lo conseguimos!!!\n");
printf("lo conseguimos!!!\n");
resetGame();
}
}
catch(AGMModelException &e)
{
printf("I don't know ball %d\n", ball);
}
}
}