C++ QVec::x方法代码示例

 void SpecificWorker::getMarcas()
 {
 	QVec robotInWorld = innermodel->transform("world","robot");
	listaMarcas = getapriltags_proxy->checkMarcas();

	
 	int tag;
 	if (!boxPicked){
 		tag = getMinTag(0);
 		if (tag != -1){ //If box found
 			QVec targetRobot = innermodel->transform("world",QVec::vec3(listaMarcas[tag].tx, 0, listaMarcas[tag].tz) , "rgbd");
 			T.insertTag(currentBox, targetRobot.x(), targetRobot.z());
 		}
 
 	}
 	else{
 		tag = getTag(currentTag);
 		if ( tag != -1){
 				QVec targetRobot = innermodel->transform("world",QVec::vec3(listaMarcas[tag].tx, 0, listaMarcas[tag].tz) , "rgbd");
 				T.insertTag(listaMarcas[tag].id, targetRobot.x(), targetRobot.z());
 		}
 
 	}
 	
 
 
 	
}

/**
 * @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 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;
 }

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;
        }
    }
}

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::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();
}

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;
}

//bool PlannerOMPL::computePath(const QVec& target, InnerModel* inner)
bool PlannerOMPL::computePath(const QVec& origin, const QVec &target, int maxTime)
{
	//Planning proper
	if (simpleSetUp == NULL)
		return false;
	
	simpleSetUp->clear();
	
	ob::ScopedState<> start(simpleSetUp->getStateSpace());
	start[0] = origin.x();	start[1] = origin.y(); start[2] = origin.z();
	ob::ScopedState<> goal(simpleSetUp->getStateSpace());
	goal[0] = target.x(); goal[1] = target.y(); goal[2] = target.z();
	simpleSetUp->setStartAndGoalStates(start, goal);
	
	simpleSetUp->getProblemDefinition()->print(std::cout);
	
	currentPath.clear();
	
	ob::PlannerStatus solved = simpleSetUp->solve(maxTime);

	if (solved)
	{
		std::cout << __FILE__ << __FUNCTION__ << "RRT, found solution with " << simpleSetUp->getSolutionPath().getStateCount() << " waypoints" << std::endl;;
	
		//if (simpleSetUp->haveSolutionPath())	
	//	simpleSetUp->simplifySolution();
		og::PathGeometric &p = simpleSetUp->getSolutionPath();
 //		simpleSetUp->getPathSimplifier()->simplify(p,5);//
//		std::cout << __FILE__ << __FUNCTION__ << "Solution after simplify: " << p. getStateCount() << ". Path length: " << p.length() << std::endl;
//		p.print(std::cout);

 		simpleSetUp->getPathSimplifier()->smoothBSpline(p);
		p.interpolate();
		
		for (std::size_t i = 0; i < p.getStateCount(); ++i)
		{
			currentPath.append( QVec::vec3( p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[0], 
											p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[1], 
											p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[2]));
		}
		return true;
	}
	else
		return false;
}

std::tuple<bool, QString> Sampler::checkRobotValidStateAtTarget(const QVec &targetPos, const QVec &targetRot) const 
{
 	QMutexLocker ml(&mutex);
	QString diagnosis;
	
	//First we move the robot in our copy of innermodel to its current coordinates
	innerModelSampler->updateTransformValues("robot", targetPos.x(), targetPos.y(), targetPos.z(), targetRot.x(), targetRot.y(), targetRot.z());

	///////////////////////
	//// Check if the target is a point inside known space and outside forbidden regions
	///////////////////////
	if ( outerRegion.contains( QPointF(targetPos.x(), targetPos.z()) ) == false  )
	{
		diagnosis += "OuterRegion " + QVariant(outerRegion).toString() + "does not contain the point";
		return std::make_tuple(false, diagnosis);
	}
	foreach( QRectF r, innerRegions)
		if( r.contains( QPointF(targetPos.x(), targetPos.z())) == true )
		{
			diagnosis += "InnerRegion " + QVariant(r).toString() + "contains the point";
			return std::make_tuple(false, diagnosis);
		}
	
	///////////////////////
	//// Check if the robot at the target collides with any know object
	///////////////////////	
	for ( auto &in : robotNodes )
		for ( auto &out : restNodes )
		{
			if ( innerModelSampler->collide( in, out))
			{
				//qDebug() << __FUNCTION__ << "collision de " << in << " con " << out;
				diagnosis += "Collision of robot's mesh '" + in + "' with '" + out + "' at robot position " 
											+ QString::number(targetPos.x()) + ", " + QString::number(targetPos.z());
				return std::make_tuple(false, diagnosis);
			}
		}
	return std::make_tuple(true, diagnosis);
}

/**
 * @brief Searches in the graph closest points to origin and target
 * 
 * @param origin ...
 * @param target ...
 * @param originVertex to return selected vertex
 * @param targetVertex ...
 * @return void
 */
void PlannerPRM::searchClosestPoints(const QVec& origin, const QVec& target, Vertex& originVertex, Vertex& targetVertex)
{
	qDebug() << __FUNCTION__ << "Searching from " << origin << "and " << target;

	//prepare the query
	Eigen::MatrixXi indices;
	Eigen::MatrixXf distsTo;
	Eigen::MatrixXf query(3,2);
	indices.resize(1, query.cols());
	distsTo.resize(1, query.cols());
	query(0,0) = origin.x();query(1,0) = origin.y();query(2,0) = origin.z();
	query(0,1) = target.x();query(1,1) = target.y();query(2,1) = target.z();

	nabo->knn(query, indices, distsTo, 1);

	originVertex = vertexMap.value(indices(0,0));
	targetVertex = vertexMap.value(indices(0,1));

 	qDebug() << __FUNCTION__ << "Closest point to origin is at" << data(0,indices(0,0)) << data(1,indices(0,0)) << data(2,indices(0,0)) << " and corresponds to " << graph[originVertex].pose;
 	qDebug() << __FUNCTION__ << "Closest point to target is at" << data(0,indices(0,1)) << data(1,indices(0,1)) << data(2,indices(0,1)) << " and corresponds to " << graph[targetVertex].pose;

}

/** 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 &current, CurrentTarget &currentT, 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;
}

/**
* \brief Default constructor
*/
SpecificWorker::SpecificWorker(MapPrx& mprx) : GenericWorker(mprx)
{
	inner = new InnerModel("/home/salabeta/Robotica2015/[email protected]/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();
}

///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 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 );
	}
}

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;
    }
  
}