/**
* @brief A point of the road is visible if it is between the robot and the laser beam running through it, and if the previous point was visible
* All points in the road are updated
* @param road ...
* @param laserData ...
* @return bool
*/
bool ElasticBand::checkVisiblePoints(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
//Simplify laser polyline using Ramer-Douglas-Peucker algorithm
std::vector<Point> points, res;
QVec wd;
for (auto &ld : laserData)
{
//wd = innermodel->laserTo("world", "laser", ld.dist, ld.angle); //OPTIMIZE THIS FOR ALL CLASS METHODS
wd = innermodel->getNode<InnerModelLaser>("laser")->laserTo("world", ld.dist, ld.angle);
points.push_back(Point(wd.x(), wd.z()));
}
res = simPath.simplifyWithRDP(points, 70);
//qDebug() << __FUNCTION__ << "laser polygon after simp" << res.size();
// Create a QPolygon so we can check if robot outline falls inside
QPolygonF polygon;
for (auto &p: res)
polygon << QPointF(p.x, p.y);
// Move the robot along the road
int robot = road.getIndexOfNextPoint();
QVec memo = innermodel->transform6D("world", "robot");
for(int it = robot; it<road.size(); ++it)
{
road[it].isVisible = true;
innermodel->updateTransformValues("robot", road[it].pos.x(), road[it].pos.y(), road[it].pos.z(), 0, road[it].rot.y(), 0);
//get Robot transformation matrix
QMat m = innermodel->getTransformationMatrix("world", "robot");
// Transform all points at one to world RS
//m.print("m");
//pointsMat.print("pointsMat");
QMat newPoints = m * pointsMat;
//Check if they are inside the laser polygon
for (int i = 0; i < newPoints.nCols(); i++)
{
// qDebug() << __FUNCTION__ << "----------------------------------";
// qDebug() << __FUNCTION__ << QPointF(newPoints(0, i), newPoints(2, i));
// qDebug() << __FUNCTION__ << polygon;
if (polygon.containsPoint(QPointF(newPoints(0, i), newPoints(2, i)),Qt::OddEvenFill) == false)
{
road[it].isVisible = false;
//qFatal("fary");
break;
}
}
// if( road[it].isVisible == false)
// {
// for (int k = it; k < road.size(); ++k)
// road[k].isVisible = false;
// break;
// }
}
// Set the robot back to its original state
innermodel->updateTransformValues("robot", memo.x(), memo.y(), memo.z(), 0, memo.ry(), 0);
//road.print();
return true;
}
/**
* @brief Adds points to the band if two existing ones are too far apart (ROBOT_RADIUS)
*
* @param road ...
* @return void
*/
void ElasticBand::addPoints(WayPoints &road, const CurrentTarget ¤tTarget)
{
int offset = 1;
for (int i = 0; i < road.size() - offset; i++)
{
if (i > 0 and road[i].isVisible == false)
break;
WayPoint &w = road[i];
WayPoint &wNext = road[i + 1];
float dist = (w.pos - wNext.pos).norm2();
if (dist > ROAD_STEP_SEPARATION) //SHOULD GET FROM IM
{
float l = 0.9 * ROAD_STEP_SEPARATION / dist; //Crucial que el punto se ponga mas cerca que la condición de entrada
WayPoint wNew((w.pos * (1 - l)) + (wNext.pos * l));
road.insert(i + 1, wNew);
}
}
//ELIMINATED AS REQUESTED BY MANSO
//Move point before last to orient the robot. This works but only if the robots approaches from the lower quadrants
//The angle formed by this point and the last one has to be the same es specified in the target
//We solve this equations for (x,z)
// (x' -x)/(z'-z) = tg(a) = t
// sqr(x'-x) + sqr(z'-z) = sqr(r)
// z = z' - (r/(sqrt(t*t -1)))
// x = x' - r(sqrt(1-(1/t*t+1)))
// if( (currentTarget.hasRotation() == true) and (road.last().hasRotation == false) )
// {
// qDebug() << __FUNCTION__ << "computing rotation" << road.last().pos;
// float radius = 500;
// float ta = tan(currentTarget.getRotation().y());
// float xx = road.last().pos.x() - radius*sqrt(1.f - (1.f/(ta*ta+1)));
// float zz = road.last().pos.z() - (radius/sqrt(ta*ta+1));
// WayPoint wNew( QVec::vec3(xx,road.last().pos.y(),zz) );
// road.insert(road.end()-1,wNew);
// road.last().hasRotation = true;
// qDebug() << __FUNCTION__ << "after rotation" << wNew.pos << currentTarget.getRotation().y() << ta;
//
// }
//else
//qDebug() << road.last().hasRotation << road.last().pos << (road.end()-2)->pos << currentTarget.getRotation().y();
}
/**
* @brief Removes points from the band if two of them are too close, ROBOT_RADIUS/3.
*
* @param road ...
* @return void
*/
void ElasticBand::cleanPoints(WayPoints &road)
{
int i;
int offset = 2;
//if( road.last().hasRotation ) offset = 3; else offset = 2;
for (i = 1; i < road.size() -
offset; i++) // exlude 1 to avoid deleting the nextPoint and the last two to avoid deleting the target rotation
{
if (road[i].isVisible == false)
break;
WayPoint &w = road[i];
WayPoint &wNext = road[i + 1];
float dist = (w.pos - wNext.pos).norm2();
if (dist < ROAD_STEP_SEPARATION / 3.)
{
road.removeAt(i + 1);
}
}
}
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;
}
