int SpecificWorker::getMinTag(int flag){
int index = -1;
QVec targetRobot;
float min_distance = 10000;
if (flag == 0){ //If looking for a box
for(unsigned int i = 0; i < listaMarcas.size(); i++){
targetRobot = innermodel->transform("world",QVec::vec3(listaMarcas[i].tx, 0, listaMarcas[i].tz) , "rgbd");
if (targetRobot.norm2() < min_distance && listaMarcas[i].id > 9 && std::find(cajasRecogidas.begin(), cajasRecogidas.end(), listaMarcas[i].id) == cajasRecogidas.end()){
min_distance = targetRobot.norm2();
index = i;
currentBox = listaMarcas[i].id;
}
}
}
else{ //If looking for a wall tag
for(unsigned int i = 0; i < listaMarcas.size(); i++){
targetRobot = innermodel->transform("world",QVec::vec3(listaMarcas[i].tx, 0, listaMarcas[i].tz) , "rgbd");
if (targetRobot.norm2() < min_distance && listaMarcas[i].id < 9){
min_distance = targetRobot.norm2();
index = i;
}
}
}
return index;
}
bool SpecificWorker::hayCamino()
{
QVec t = inner->transform("robot", ctarget.target, "world");
float alpha =atan2(t.x(), t.z() );
float d = t.norm2();
float x, z;
//int i = 50;
for ( uint i = 0; i<ldata.size(); i++ ) {
if ( ldata[i].angle <= alpha ) {
if ( ldata[i].dist < d ) {
qDebug() <<"NO hay camino";
return false;
} else {
ctarget.activeSub=false;
qDebug() <<"hay camino";
return true;
}
}
}
qDebug() <<"NO ve la marca";
state = State::TURN;
return false;
}
void SpecificWorker::irASubTarget()
{
qDebug() << __FUNCTION__<<"ir a subTarget";
QVec t = inner->transform ( "robot", ctarget.subTarget, "world" );
float alpha =atan2 ( t.x(), t.z() );
float r= 0.4*alpha;
float d = t.norm2();
if ( d<100 ) {
ctarget.activeSub=false;
differentialrobot_proxy->setSpeedBase ( 0,0 );
sleep ( 1 );
} else {
if ( fabs ( r ) > 0.2 ) {
d = 0;
}
if ( d>300 ) {
d=300;
}
try {
differentialrobot_proxy->setSpeedBase ( d,r );
} catch ( const Ice::Exception &ex ) {
std::cout << ex << std::endl;
}
}
}
inline void LMap::fromImageToVirtualLaser(float xi, float zi, int laserBins, float &dist, int32_t &bin)
{
QVec mapCoordInLaser = fromImageToReference(xi, zi, virtualLaserID);
float angle = atan2(mapCoordInLaser(0), mapCoordInLaser(2));
dist = mapCoordInLaser.norm2();
bin = angle2bin(angle, laserBins);
}
bool SpecificWorker::heLlegado()
{
QVec t = inner->transform("robot", ctarget.target, "world");
// qDebug()<< ctarget.target;
float d = t.norm2();
qDebug()<< "distancia: "<<d;
if ( d < 100 )
{
return true;
}else
{
return false;
}
}
void SpecificWorker::irATarget()
{
QVec t = inner->transform ( "robot", ctarget.target, "world" );
qDebug() << __FUNCTION__<< ctarget.target;
qDebug() << __FUNCTION__<< t;
float alpha =atan2 ( t.x(), t.z() );
float r= 0.3*alpha;
float d = 0.3*t.norm2();
qDebug() << "velocidad " << d;
if ( fabs ( r ) > 0.2 ) {
d = 0;
}
if ( d>300 ) {
d=300;
}
try {
differentialrobot_proxy->setSpeedBase ( d,r );
} catch ( const Ice::Exception &ex ) {
std::cout << ex << std::endl;
}
}
bool SpecificWorker::checkFreeWay( const QVec &targetInRobot)
{
qDebug() << __FUNCTION__ ;
//First turn the robot to point towards the new target
if (alignToTarget( targetInRobot ) != true )
{
stopAction();
qFatal("Could not align the robot");
}
QList<QVec> lPoints;
//points on the corners of thw square
lPoints.append( QVec::vec3(ROBOT_RADIUS,0,ROBOT_RADIUS));
lPoints.append( QVec::vec3(ROBOT_RADIUS,0,-ROBOT_RADIUS));
lPoints.append( QVec::vec3(-ROBOT_RADIUS,0,-ROBOT_RADIUS));
lPoints.append( QVec::vec3(-ROBOT_RADIUS,0,ROBOT_RADIUS));
//points on the sides
lPoints.append( QVec::vec3(ROBOT_RADIUS,0,0));
lPoints.append( QVec::vec3(0,0,-ROBOT_RADIUS));
lPoints.append( QVec::vec3(-ROBOT_RADIUS,0,0));
lPoints.append( QVec::vec3(0,0, ROBOT_RADIUS));
float dist = targetInRobot.norm2();
float alpha =atan2(targetInRobot.x(), targetInRobot.z() );
float step = ceil(dist/ (ROBOT_SIZE/3));
QVec tNorm = targetInRobot.normalize();
QVec r;
inner->updateTransformValues ("vbox",0, 0, 0, 0, 0, 0, "robot");
for(size_t i = 0; i<ldata.size(); i++)
{
if(ldata[i].angle <= alpha)
{
for(float landa=400; landa<=dist; landa+=step)
{
r = tNorm * landa;
inner->updateTransformValues ("vbox",r.x(), r.y(), r.z(), 0, alpha, 0, "robot");
foreach(QVec p, lPoints)
{
QVec pointInRobot = inner->transform("robot", p, "vbox");
float dPR = pointInRobot.norm2();
float alphaPR = atan2(pointInRobot.x(), pointInRobot.z());
for(auto ld : ldata)
if(ld.angle <= alphaPR)
{
if( ld.dist>0 and ld.dist < LASER_MAX and ld.dist >= dPR)
{
//qDebug()<<__FUNCTION__<< "Hay camino libre al target" << ldata[i].dist << d;
break;
}
else
{
qDebug() << "collision of point(R) " << inner->transform("robot", p, "vbox");
return false;
}
}
}
}
qDebug()<<__FUNCTION__<< "Hay camino libre al target. Laser distance:" << ldata[i].dist << ". Target distance:" << dist;
break; //We found the laser ray aligned with the target.
}
}
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;
}
float ElasticBand::computeForces(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
if (road.size() < 3)
return 0;
// To avoid moving the rotation element attached to the last
int lastP;
if (road.last().hasRotation)
lastP = road.size() - 2;
else
lastP = road.size() - 1;
// Go through all points in the road
float totalChange = 0.f;
for (int i = 1; i < lastP; i++)
{
if (road[i].isVisible == false)
break;
WayPoint &w0 = road[i - 1];
WayPoint &w1 = road[i];
WayPoint &w2 = road[i + 1];
// Atraction force caused by the trajectory stiffnes, trying to straighten itself. It is computed as a measure of local curvature
QVec atractionForce(3);
float n = (w0.pos - w1.pos).norm2() / ((w0.pos - w1.pos).norm2() + w1.initialDistanceToNext);
atractionForce = (w2.pos - w0.pos) * n - (w1.pos - w0.pos);
//Compute derivative of force field and store values in w1.bMinuxX .... and w1.minDist. Also variations wrt former epochs
computeDistanceField(innermodel, w1, laserData, FORCE_DISTANCE_LIMIT);
QVec repulsionForce = QVec::zeros(3);
QVec jacobian(3);
// space interval to compute the derivative. Related to to robot's size
float h = DELTA_H;
if ((w1.minDistHasChanged == true) /*and (w1.minDist < 250)*/ )
{
jacobian = QVec::vec3(w1.bMinusX - w1.bPlusX,
0,
w1.bMinusY - w1.bPlusY) * (T) (1.f / (2.f * h));
// repulsion force is computed in the direction of maximun laser-point distance variation and scaled so it is 0 is beyond FORCE_DISTANCE_LIMIT and FORCE_DISTANCE_LIMIT if w1.minDist.
repulsionForce = jacobian * (FORCE_DISTANCE_LIMIT - w1.minDist);
}
float alpha = -0.5; //Negative values between -0.1 and -1. The bigger in magnitude, the stiffer the road becomes
float beta = 0.55; //Posibite values between 0.1 and 1 The bigger in magnitude, more separation from obstacles
QVec change = (atractionForce * alpha) + (repulsionForce * beta);
if (std::isnan(change.x()) or std::isnan(change.y()) or std::isnan(change.z()))
{
road.print();
qDebug() << atractionForce << repulsionForce;
qFatal("change");
}
//Now we remove the tangencial component of the force to avoid recirculation of band points
//QVec pp = road.getTangentToCurrentPoint().getPerpendicularVector();
//QVec nChange = pp * (pp * change);
w1.pos = w1.pos - change;
totalChange = totalChange + change.norm2();
}
return totalChange;
}
void SpecificWorker::crearSubTarget()
{
qDebug() << __FUNCTION__ << "creando subTarget";
float dt;
QVec t = inner->transform ( "rgbd", ctarget.target, "world" );
float d = t.norm2();
float alpha =atan2 ( t.x(), t.z() );
uint i,j;
//const int R =400;
for ( i = 5; i<ldata.size()-5; i++ ) {
if ( ldata[i].angle < alpha ) {
if ( d>ldata[i].dist ) {
dt=ldata[i].dist;
break;
}
}
}
qDebug() << __FUNCTION__<<i;
qDebug() << __FUNCTION__<<ldata[i].dist<<ldata[i].angle;
for ( j = i; j<ldata.size()-5; j++ ) {
qDebug() << __FUNCTION__<<dt<< dt+ ( dt*0.2 ) <<ldata[j].dist << ldata[j].angle;
if ( ldata[j].dist> ( dt+ ( dt*0.2 ) ) and ldata[j].angle < 0 ) {
ctarget.subTarget=inner->transform ( "world", QVec::vec3 ( ldata[j].dist *sin ( ldata[j].angle )-2000,0, ldata[j].dist *cos ( ldata[j].angle ) ), "laser" );
ctarget.activeSub = true;
break;
}
}
qDebug() << __FUNCTION__<< "Subtargeet" << QVec::vec3 ( ldata[j].dist *sin ( ldata[j].angle ),0, ldata[j].dist *cos ( ldata[j].angle ) );
/*
//Search the first increasing step from the center to the right
uint i,j;
const int R =600;
for(i=ldata.size()/2; i>5; i--)
{
if( (ldata[i].dist - ldata[i-1].dist) < -R )
{
if(i<=7)
{
i=0;
break;
}
uint k=i-2;
while( (k >= 0) and (fabs( ldata[k].dist*sin(ldata[k].angle - ldata[i-1].angle)) < R ))
{ k--; }
i=k;
break;
}
}
for(j=ldata.size()/2-5; j<ldata.size()-1; j++)
{
if( (ldata[j].dist - ldata[j+1].dist) < -R )
{
if(j>ldata.size()-3)
{
j=ldata.size()-1;
break;
}
uint k=j+2;
while( (k < ldata.size()) and (fabs( ldata[k].dist*sin(ldata[k].angle - ldata[j+1].angle)) < R ))
{ k++; }
j=k;
break;
}
}
QVec sI = inner->transform("world", QVec::vec3(ldata[j].dist *sin(ldata[j].angle),0, ldata[j].dist *cos(ldata[j].angle)), "laser");
QVec sD = inner->transform("world", QVec::vec3(ldata[i].dist *sin(ldata[i].angle),0, ldata[i].dist *cos(ldata[i].angle)), "laser");
if( (sI-ctarget.target).norm2() > (sD-ctarget.target).norm2() )
ctarget.subtarget=sD;
else
ctarget.subtarget=sI;
ctarget.activeSub=true;
// for(i=5; i<ldata.size()-5;i++)
// {
// if(ldata[i-1].dist - ldata[i].dist > 400)
// {
// ctarget.subtarget=inner->transform("world", QVec::vec3(ldata[i].dist *sin(ldata[i].angle),0, ldata[i].dist *cos(ldata[i].angle)), "laser");
// ctarget.activeSub=true;
// break;
// }
//
// if(ldata[i+1].dist - ldata[i].dist > 400)
// {
//
// ctarget.subtarget=inner->transform("world", QVec::vec3(ldata[i].dist *sin(ldata[i].angle),0, ldata[i].dist *cos(ldata[i].angle)), "laser");
// ctarget.activeSub=true;
// break;
// }
//
// }
qDebug()<<ctarget.subtarget;
// qDebug()<<"distancia subtarget"<<ldata[i].dist;
// differentialrobot_proxy->setSpeedBase(0,0.5);
//exit(-1);*/
}
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);
}
}
}