/**
* \brief Matrix to vector product operator; \f$ C = this * vector \f$
* @param A vector factor for operation
* @return QVec New vector result
*/
QVec RMat::QMat::operator *(const QVec & vector) const
{
Q_ASSERT ( cols == vector.size());
QMat aux = vector.toColumnMatrix();
QMat result = operator*(aux);
return result.toVector();
}
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;
}
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;
}
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;
}
}
}
/**
* \brief This method stored the motors's names, the initial position of the motors and the goal
* position of them. When all is prepared, it raises up the flag READY.
* @param newAnglesOfMotors an structure with the name of the motors and the value os them.
*/
void SpecificWorker::setJointPosition(const MotorAngleList &newAnglesOfMotors)
{
qDebug()<<"YEAH";
QMutexLocker ml(mutex);
if(INITIALIZED == true)
{
// 1) SACAMOS VALORES ACTUALES DE LOS MOTORES: (aprovechamos y sacamos motores disponibles)
QVec firstAngles;
for(auto motor : newAnglesOfMotors)
{
float angle = innerModel->getJoint(QString::fromStdString(motor.name))->getAngle();
firstAngles.push_back(angle);
selectedMotors<<QString::fromStdString(motor.name);
}
// 2) SACAMOS VALORES FINALES DE LOS MOTORES
QVec finalAngles;
for(auto motor : newAnglesOfMotors)
{
float angle = motor.angle;
finalAngles.push_back(angle);
}
jointValues.append(firstAngles);
jointValues.append(finalAngles);
COMPUTE_READY = true;
qDebug()<<"||------------------------------------------------";
qDebug()<<"|| setJointPosition: jointValues-->"<<jointValues;
qDebug()<<"||------------------------------------------------";
}
}
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
{
}
}
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);
}
void SpecificWorker::compute()
{
try {
differentialrobot_proxy->getBaseState ( bState );
ldata = laser_proxy->getLaserData();
inner->updateTransformValues ( "robot", bState.x, 0, bState.z, 0, bState.alpha, 0 );
float alpha;
QVec t;
switch ( state ) {
case State::INIT:
state = State::IDLE;
break;
case State::IDLE:
break;
case State::WORKING:
if ( heLlegado() ) {
qDebug() << __FUNCTION__<< "Arrived to target" << ctarget.target;
stopRobot();
state = State::FINISH;
} else if ( hayCamino() ) {
irATarget();
}
break;
case State::TURN:
qDebug() << "Buscando punto" << ctarget.target;
t = inner->transform ( "robot", ctarget.target, "world" );
alpha =atan2 ( t.x(), t.z() );
if ( alpha <= ldata.front().angle and alpha >= ldata. back().angle ) {
stopRobot();
state = State::WORKING;
} else
try {
differentialrobot_proxy->setSpeedBase ( 0, 0.4 );
} catch ( Ice::Exception &ex ) {
std::cout<<ex.what() <<std::endl;
};
break;
case State::FINISH:
sleep ( 2 );
undrawTarget ( "target" );
state = State::IDLE;
break;
}
} catch ( const Ice::Exception &e ) {
std::cout << "Error reading from Camera" << e << std::endl;
}
//histogram();
innerViewer->update();
osgView->autoResize();
osgView->frame();
}
/**
* \brief Computes the coefficients of the implicit line equation: y = mx + n passing by two points
* from two points (x1,y1) e (x2,y2) satisfying (y-y1)/(y2-y1) = (x-x1)/(x2-x1),
* that solving for y gives: y = x( (y2-y1)/(x2-x1) ) - x1( (y2-y1)/(x2-x1) ) + y1
* @param p1 containing x1,y1
* @param p2 containing x2,y2
* @return a QVec vector of 2 dimensions with m and n
*/
QVec RMat::QVec::line2DImplicitCoefsFrom2Points(const QVec & p1, const QVec & p2)
{
Q_ASSERT( p1.size() == 2 and p2.size() == 2 and p2(0)-p1(0) != 0 );
QVec res(2);
res(0) = (p2(1)-p1(1))/(p2(0)-p1(0)) ;
res(1) = p1(1) - p1(0) * ((p2(1)-p1(1))/(p2(0)-p1(0))) ;
return res;
}
/**
* \brief Construct a diagonal matrix
*
* Use vector values to create a new diagonal matrix
* @param v Vector to get diagonal values
* @return QMat new diagonal matrix
*/
QMat RMat::QMat::makeDiagonal ( const QVec &v )
{
QMat R ( v.size(),v.size(), (T)0 );
int f = v.size();
for ( int i=0; i<f; i++ )
R ( i,i ) = v ( i );
return R;
}
bool Sampler::checkRobotValidDirectionToTargetOneShot(const QVec & origin , const QVec & target) const
{
//qDebug() << __FUNCTION__ << "Checking between: " << origin << "and " << target;
const float MAX_LENGTH_ALONG_RAY = (target-origin).norm2();
QVec finalPoint;
float wRob=420, hRob=1600; //GET FROM INNERMODEL!!!
// if( MAX_LENGTH_ALONG_RAY < 50) //COMMENT THIS FOR NOW ::::::::::::::::::::...
// {
// qDebug() << __FUNCTION__ << "target y origin too close";
// return false;
// }
//Compute angle between origin-target line and world Zaxis
float alfa1 = QLine2D(target,origin).getAngleWithZAxis();
//qDebug() << "Angle with Z axis" << origin << target << alfa1;
// Update robot's position and align it with alfa1 so it looks at the TARGET point
innerModelSampler->updateTransformValues("robot", origin.x(), origin.y(), origin.z(), 0., alfa1, 0.);
// Compute rotation matrix between robot and world. Should be the same as alfa
QMat r1q = innerModelSampler->getRotationMatrixTo("world", "robot");
//qDebug()<< "alfa1" << alfa1 << r1q.extractAnglesR_min().y() << "robot" << innerModelSampler->transform("world","robot");
// Create a tall box for robot body with center at zero and sides:
boost::shared_ptr<fcl::Box> robotBox(new fcl::Box(wRob, hRob, wRob));
// Create a collision object
fcl::CollisionObject robotBoxCol(robotBox);
//Create and fcl rotation matrix to orient the box with the robot
const fcl::Matrix3f R1( r1q(0,0), r1q(0,1), r1q(0,2), r1q(1,0), r1q(1,1), r1q(1,2), r1q(2,0), r1q(2,1), r1q(2,2) );
//Check collision at maximum distance
float hitDistance = MAX_LENGTH_ALONG_RAY;
//Resize big box to enlarge it along the ray direction
robotBox->side = fcl::Vec3f(wRob, hRob, hitDistance);
//Compute the coord of the tip of a "nose" going away from the robot (Z dir) up to hitDistance/2
const QVec boxBack = innerModelSampler->transform("world", QVec::vec3(0, hRob/2, hitDistance/2), "robot");
//move the big box so it is aligned with the robot and placed along the nose
robotBoxCol.setTransform(R1, fcl::Vec3f(boxBack(0), boxBack(1), boxBack(2)));
//Check collision of the box with the world
for ( auto &it : restNodes)
{
if ( innerModelSampler->collide(it, &robotBoxCol))
{
//qDebug() << __FUNCTION__ << ": Robot collides with " << it;
return false;
}
}
return true;
}
QVec RMat::QMat::extractAnglesR_min() const
{
QVec r = extractAnglesR();
QVec v1 = r.subVector(0,2);
QVec v2 = r.subVector(3,5);
if (v1.norm2() < v2.norm2())
return v1;
return v2;
}
/**
* \brief Vector to matrix constructor
*
* Creates a row or column matrix from vector
* @param vector Vector to convert to matrix type
* @param rowVector if true, creates a row matrix from the vector, else creates a column matrix from it
*/
QMat::QMat(const QVec &vector, const bool rowVector)
{
cols = rowVector?vector.size():1;
rows = rowVector?1:vector.size();
data = new DataBuffer(cols*rows);
if (rowVector)
setRow(0, vector);
else
setCol(0, vector);
}
/**
* \brief Computes the coefficients of the explicit line equation: Ax+By+C=0 passing by two points
* from two points (x1,x2) e (y1,y2) satisfying (x-x1)/v1 = (y-y1)/v2, being v1 and v2 the direction vectors of the line
* that after some algebra gives: A=v2, B= -v1, C= v1y1-v2x2
* @param p1 containing x1,y1
* @param p2 containing x2,y2
* @return a QVec vector of 3 dimensions with A, B and C
*/
QVec RMat::QVec::line2DExplicitCoefsFrom2Points(const QVec & p1, const QVec & p2)
{
Q_ASSERT( p1.size() == 2 and p2.size() == 2);
QVec res(3);
//A
res(0) = p2(1)-p1(1) ;
//B
res(1) = -(p2(0)-p1(0)) ;
//C
res(2) = -res(1)*p1(1) - res(0)*p1(0);
return res;
}
/**
* @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 Multiplies a n-column vector times a m-row vector to generate a nxm matrix
* @param vector vector to play the rol of row vector
* @return resulting QMat matrix
*/
QMat QVec::externProduct(const QVec & vector) const
{
Q_ASSERT(size() == vector.size());
const int s = size();
QMat C ( size(), vector.size() );
#ifdef COMPILE_IPP
ippmMul_mm_32f ( getReadData(), s * sizeof ( T ), sizeof ( T ), s , s , vector.getReadData(), vector.size() * sizeof ( T ), sizeof ( T ), vector.size() , vector.size() , C.getWriteData(), vector.size() * sizeof ( T ), sizeof ( T ) );
#else
for(int r=0;r<s;r++)
for(int c=0;c<vector.size();c++)
C(r,c) = this->operator()(r) * vector(c);
#endif
return C;
}
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;
}
}
RMat::T RMat::QVec::distanceTo2DLine( const QVec & line ) const
{
Q_ASSERT_X( size() == 2 and line.size() == 3, "QVec::distanceTo2DLine", "incorrect size of parameters");
return fabs(line(0)*operator()(0) + line(1)*operator()(1) + line(2)) / sqrt(line(0)*line(0) + line(1)*line(1));
}
/** REVISARRRR
* @brief Sends the robot bakcwards on a straight line until targetT is reached.
*
* @param innerModel ...
* @param target position in World Reference System
* @return bool
*/
bool SpecificWorker::goBackwardsCommand(InnerModel *innerModel, CurrentTarget ¤t, CurrentTarget ¤tT, TrajectoryState &state, WayPoints &myRoad)
{
const float MAX_ADV_SPEED = 400.f;
const float MAX_POSITIONING_ERROR = 40; //mm
static float errorAnt = std::numeric_limits<float>::max();
///////////////////
//CHECK PARAMETERS
///////////////////
QVec target = current.getTranslation();
if (target.size() < 3 or std::isnan(target.x()) or std::isnan(target.y()) or std::isnan(target.z()))
{
qDebug() << __FUNCTION__ << "Returning. Invalid target";
RoboCompTrajectoryRobot2D::RoboCompException ex;
ex.text = "Returning. Invalid target";
throw ex;
return false;
}
state.setState("BACKWARDS");
QVec rPose = innerModel->transform("world", "robot");
float error = (rPose - target).norm2();
bool errorIncreasing = false;
if (error > errorAnt)
errorIncreasing = true;
qDebug() << __FUNCTION__ << "doing backwards" << error;
if ((error < MAX_POSITIONING_ERROR) or (errorIncreasing == true)) //TASK IS FINISHED
{
controller->stopTheRobot(omnirobot_proxy);
myRoad.requiresReplanning = true;
currentT.setWithoutPlan(true);
currentT.setState(CurrentTarget::State::GOTO);
errorAnt = std::numeric_limits<float>::max();
}
else
{
float vadv = -0.5 * error; //Proportional controller
if (vadv < -MAX_ADV_SPEED) vadv = -MAX_ADV_SPEED;
try
{ omnirobot_proxy->setSpeedBase(0, vadv, 0); }
catch (const Ice::Exception &ex)
{ std::cout << ex << std::endl; }
}
errorAnt = error;
return true;
}
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
{
}
}
/**
* \brief Default constructor
*/
SpecificWorker::SpecificWorker(MapPrx& mprx) : GenericWorker(mprx)
{
inner = new InnerModel("/home/salabeta/Robotica2015/RoCKIn@home/world/apartment.xml");
//Set odometry for initial robot TargetPose
try {
differentialrobot_proxy->getBaseState ( bState );
qDebug() << __FUNCTION__<< bState.x << bState.z << bState.alpha;
try {
inner->transform ( "world",QVec::zeros ( 6 ),"initialRobotPose" );
if ( bState.x == 0 and bState.z == 0 ) { //RCIS just initiated. We change robot odometry to the initialRobotPose
QVec rpos = inner->transform ( "world", QVec::zeros ( 6 ),"robot" );
RoboCompDifferentialRobot::TBaseState bs;
bs.x=rpos.x();
bs.z=rpos.z();
bs.alpha=rpos.ry();
differentialrobot_proxy->setOdometer ( bs );
qDebug() << "Robot odometry set to" << rpos;
} else {
inner->updateTransformValues ( "initialRobotPose", 0,0,0,0,0,0 );
}
} catch ( std::exception &ex ) {
std::cout<<ex.what() <<std::endl;
};
} catch ( Ice::Exception &ex ) {
std::cout<<ex.what() <<std::endl;
};
qDebug() << __FUNCTION__<< bState.x << bState.z << bState.alpha;
graphicsView->setScene ( &scene );
graphicsView->show();
graphicsView->scale ( 3,3 );
//Innermodelviewer
osgView = new OsgView ( this );
osgGA::TrackballManipulator *tb = new osgGA::TrackballManipulator;
osg::Vec3d eye ( osg::Vec3 ( 4000.,4000.,-1000. ) );
osg::Vec3d center ( osg::Vec3 ( 0.,0.,-0. ) );
osg::Vec3d up ( osg::Vec3 ( 0.,1.,0. ) );
tb->setHomePosition ( eye, center, up, true );
tb->setByMatrix ( osg::Matrixf::lookAt ( eye,center,up ) );
osgView->setCameraManipulator ( tb );
innerViewer = new InnerModelViewer ( inner, "root", osgView->getRootGroup(), true );
show();
}
/**
* \brief Dot product of operand with current vector
* @param vector
* @return dot product vector
*/
T QVec::dotProduct(const QVec &vector) const
{
Q_ASSERT(size() == vector.size());
double accum = 0;
for (int i = 0; i < size(); i++)
accum += at(i) * vector[i];
return accum;
}
/**
* \brief Element to element product
* @param vector
* @return product vector
*/
QVec QVec::pointProduct(const QVec & vector) const
{
Q_ASSERT(size() == vector.size());
QVec result = *this;
for (int i = 0; i < size(); i++)
result[i] *= vector[i];
return result;
}
/**
* \brief operator that subtracts a vector from the current one
* @param vector
* @return the difference vector
*/
QVec QVec::operator -(const QVec & vector) const
{
Q_ASSERT( size() == vector.size() );
QVec result(size());
for (int i = 0; i < size(); i++)
result[i] = at(i) - vector[i];
return result;
}
void RMat::QMat::setRow(const int row, QVec vector)
{
Q_ASSERT(row < nRows());
Q_ASSERT(nCols() == vector.size());
const int cols = nCols();
for (int col= 0; col < cols; col++)
operator()(row,col) = vector[col];
}
///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;
}
/**
* @brief Moves a virtual copy of the robot along the road checking for enough free space around it
*
* @param innermodel ...
* @param road ...
* @param laserData ...
* @param robotRadius ...
* @return bool
*/
bool ElasticBand::checkCollisionAlongRoad(InnerModel *innermodel, const RoboCompLaser::TLaserData& laserData, WayPoints &road, WayPoints::const_iterator robot,
WayPoints::const_iterator target, float robotRadius)
{
//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
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
QVec memo = innermodel->transform6D("world","robot");
bool free = false;
for( WayPoints::const_iterator it = robot; it != target; ++it)
{
if( it->isVisible == false)
break;
// compute orientation of the robot at the point
innermodel->updateTransformValues("robot", it->pos.x(), it->pos.y(), it->pos.z(), 0, it->rot.y(), 0);
//get Robot transformation matrix
QMat m = innermodel->getTransformationMatrix("world", "robot");
// Transform all points at one
qDebug() << __FUNCTION__ << "hello2";
m.print("m");
pointsMat.print("pointsMat");
QMat newPoints = m * pointsMat;
qDebug() << __FUNCTION__ << "hello3";
//Check if they are inside the laser polygon
for( int i=0; i<newPoints.nRows(); i++)
if( polygon.containsPoint(QPointF(pointsMat(i,0)/pointsMat(i,3), pointsMat(i,2)/pointsMat(i,3)), Qt::OddEvenFill ) == false)
{
free = false;
break;
}
free = true;
}
qDebug() << __FUNCTION__ << "hello";
// Set the robot back to its original state
innermodel->updateTransformValues("robot", memo.x(), memo.y(), memo.z(), 0, memo.ry(), 0);
return free ? true : false;
}
//Do it with inject
void RMat::QMat::setCol(const int col, QVec vector)
{
Q_ASSERT(col < nCols());
Q_ASSERT(nRows() == vector.size());
const int rows = nRows();
for (int row= 0; row < rows; row++)
operator()(row,col) = vector[row];
}
void RCDraw::draw3DRoiOnFloor(const QVec & center, const QMat & cov, const QColor & col, bool fill, int id)
{
//extract submatrix X;Z for 2D floor porjection
QMat cov2D(2,2);
/*cov2D(0,0) = cov(0,0);
cov2D(0,1) = cov(0,2);
cov2D(1,0) = cov(2,0);
cov2D(1,1) = cov(2,2);
*///cov2D.print("cov2D");
cov2D = cov;
QVec vals(2);
QMat vecs = cov2D.eigenValsVectors ( vals );
float sigma1 = vals(0);
float sigma2 = vals(1);
float phi;
if (sigma1 > sigma2)
phi = atan2(vecs(1,0),vecs(0,0));
else
phi = atan2(vecs(1,1),vecs(0,1));
float ang = phi*180/M_PI + (M_PI/2);
qDebug() << "ang " << phi;
/* if (phi<0)
ang = phi*180/M_PI + 360;
else
ang = phi*180/M_PI;*/
if ( isnan(sigma1)==false and isinf(sigma1)==false and isnan(sigma2)==false and isinf(sigma2)==false)
{
TEllipse e;
e.center.setX( center.x());
e.center.setY( center.z());
e.rx = sigma1;
e.ry = sigma2;
e.color = col;
e.id = id;
e.fill = fill;
e.ang = ang;
ellipseQueue.enqueue ( e );
}
}
/**
* \brief Compares two vector for equal values element to element
* @param vector
* @return true if both are equal, false if not
*/
bool RMat::QVec::equals(const QVec & vector ) const
{
if (size() != vector.size())
return false;
for (int i = 0; i < size(); i++)
if (at(i) != vector[i])
return false;
return true;
}