C++ Pose::getY方法代码示例

void Java::slotGetWayPoints(const Pose start, const Pose end)
{
	QMutexLocker locker(mutex);

	logger->Java
	(
		"Java::slotGetWayPoints(): now processing waypoint request from %s to %s.",
		qPrintable(start.toString()),
		qPrintable(end.toString())
	);

	QList<QPointF> wayPoints;

	jintArray array = (jintArray)env->CallStaticObjectMethod(
		robletPath,
		robletPathMethodGetPath,
		(int)((config->getRobotRadius() + 0.15) * 1000),	// add 15cm safety to pathplanning
		(int)(start.getX()*1000),
		(int)(start.getY()*1000),
		(int)(end.getX()*1000),
		(int)(end.getY()*1000));

	if(env->ExceptionOccurred() != 0)
	{
		env->ExceptionDescribe();
		logger->Java("Java::slotGetWayPoints(): getPath() threw an exception, is genPath running? Emitting empty waypoint list.");
		emit wayPointsReady(wayPoints);
		return;
	}

	jboolean isCopy = JNI_FALSE;
	jint* elements = env->GetIntArrayElements(array, &isCopy);
	if(!elements) abort("Java::slotGetWayPoints(): could not get access to waypoint result array.");

	int arrayLength = env->GetArrayLength(array);

	// extra check, should already have been caught by the Exception check above.
	if(arrayLength == 0)
	{
		logger->Java("Java::slotGetWayPoints(): received an empty waypoint list. Is start/end too close to an obstacle?");
		emit wayPointsReady(wayPoints);
		return;
	}

	// convert returned data into a list of QPointFs.
	for(int i = 0; i < arrayLength; i+=2)
	{
			//logger->Java("Java::slotGetWayPoints(): list has %d elements, now accessing x ([%d]=%d) and y ([%d]=%d).", arrayLength, i, elements[i], i+1, elements[i+1]);
		wayPoints.append(QPointF(elements[i+0] / 1000.0, elements[i+1] / 1000.0));
	}

	env->ReleaseIntArrayElements(array, elements, JNI_ABORT);


	logger->Java("Java::slotGetWayPoints(): received %d waypoints (including start and end).", wayPoints.size());

	optimizeWayPoints(wayPoints);

	emit wayPointsReady(wayPoints);
}

string MCDMFunction::getEncodedKey(Pose& p, int value)
{
    string key;
    //value = 0 : encode everything
    //value = 1 : encode x,y,orientation, take first 
    //value = 2 : encode x,y,orientation, take multiple time
    if(value == 0){
	key =  to_string(p.getX()) + "/" + to_string( p.getY()) + "/" + to_string( p.getOrientation()) + "/"  + to_string(p.getRange()) + "/" + to_string(p.getFOV());
    }else if(value == 1){
	key = to_string(p.getX()) + "/" + to_string( p.getY()) + "/" + to_string( p.getOrientation()) + "/" + "1";
    } else if (value ==2){
	key = to_string(p.getX()) + "/" + to_string( p.getY()) + "/" + to_string( p.getOrientation()) + "/" + "0";
    }
    return key;
}

u_int32_t RoadSection::evaluation(const Pose& p)
{
    // kind elliptic distance function... TODO: improve this function!
    _position_type distance = Point(p.getX(), p.getY() *2).abs();

    if (distance > 0.3)
        return 0;

    return 0.4 - distance;
}

/**
 *	Function used to draw all objects in a vector of object pointers that are related to renderobjects
 *	@param _objects is a vector of Object pointers that contains a list of all objects to draw
 *	@param _renderobjects is a vector of RenderObject that contain the direction of how to draw each object
 */
void RenderEngine::drawobjects(vector<Object*>* _objects, vector<RenderObject> _renderobjects)
{
	vector<RenderObject>::iterator renderobjectsiter;
	vector<RenderObject>::iterator renderobjectsend;

	list<Point>::iterator piter;
	int objectID;
	Point3D point1;
	Point3D point2;
	Pose position;
	for(int i = 0;i < _objects->size(); i++)
	{
		objectID = (*_objects)[i]->getObjectID();
		position = (*_objects)[i]->getPose();
		renderobjectsiter = _renderobjects.begin();
		renderobjectsend = _renderobjects.end();
		while(renderobjectsiter != renderobjectsend)
		{
			if(renderobjectsiter->getObjectID() == objectID)
			{
				piter = renderobjectsiter->getPointsBegin();
				while(piter != renderobjectsiter->getPointsEnd())
				{
					point1.setXYZ(piter->getX() + position.getX(), 
						piter->getY() - position.getY(),
						piter->getZ() - position.getZ());
					piter++;
					point2.setXYZ(piter->getX() + position.getX(), 
						piter->getY() - position.getY(),
						piter->getZ() - position.getZ());
					window->DrawLineInSpace(point1,point2);
					piter++;
				}
			}
			renderobjectsiter++;
			window->DrawLineOnScreen(Point2D(-1,0),Point2D(1,0));
			window->DrawLineOnScreen(Point2D(-.001,0),Point2D(.001,0));
			window->DrawLineOnScreen(Point2D(0.00,-.001),Point2D(0.00,.001));
		}
	}
}

DiscreteState StateDiscretizer::discretizeState(Pose pose, Velocity vel, int time_step)
{
  DiscreteState state_i;

  //find cell position
  state_i.in_map = getCellPosition(pose.getX(), pose.getY(), state_i.x_i,
                                   state_i.y_i, state_i.grid_cell);

  //find the nearest discrete orientation
  state_i.angle_i = getDiscreteOrientation(pose.getTheta());

  //find the discrete representation of the velocity
  getDiscreteVelocity(vel, state_i.vel_x_i, state_i.vel_w_i);

  state_i.time_step = time_step;

  return state_i;
}

void RobotMove::GoToPose(Pose newPose){
	xDestination = newPose.getX();
	yDestination = newPose.getY();
	//Vector worldCoordinateVector(xDestination + (*position).GetXPos(), yDestination + (*position).GetYPos());
	//Vector localCoordinateVector(worldCoordinateVector.getDistance(), dtor(worldCoordinateVector.getAngle()) + (*position).GetYaw(), false);
	//xDestination = localCoordinateVector.getXIntensity();//x + (*position).GetXPos();
	//yDestination = localCoordinateVector.getYIntensity();//y + (*position).GetYPos();

	SensorScan sensors(&(*laser), &(*position), &(*worldView), sensorFunction, 1000.0);
	double distance = closenessCutOff;
	while (distance >= closenessCutOff) {				//cout << "Setup";
		(*robot).Read();			
		sensors.ReadSensors();
		Vector sensorVector = sensors.VectorSum();
		sensorVector.invertVector();
		//sensorVector.debugIntensity();
		Vector avoidVector(sensorVector.getDistance(), dtor(sensorVector.getAngle()) + (*position).GetYaw() , false);
		
		Vector driveVector((*position).GetXPos(), (*position).GetYPos(), xDestination, yDestination);
		distance = driveVector.getDistance();
		TargetFunction(driveVector);
		
		//TargetFunction(driveVector);
		//cout << endl << "AvoidVector       : ";
		//avoidVector.debug();avoidVector.debugIntensity();
		//cout << endl << "Drive Vector      : ";
		//cout << closenessCutOff << endl;
		//driveVector.debug();
		//driveVector.debugIntensity();
		//cout << endl;
		driveVector.add(avoidVector);
		//cout << endl << "Resulting Vector  : " << endl;
		//driveVector.debug();driveVector.debugIntensity();
		DriveToVector(driveVector, &(*position));
	}
	Vector directionVector(.1, newPose.getAngle(), true);
	//while (abs((*position).GetYaw() - dtor(newPose.getAngle())) > closenessCutOff) {
	//	DriveToVector(directionVector, &(*position));
	//}
	(*position).SetSpeed(0,0);
	

}

pair<Pose,double> MCDMFunction::selectNewPose(EvaluationRecords *evaluationRecords)
{    
    Pose newTarget;
    double value = 0;
    unordered_map<string,double> evaluation = evaluationRecords->getEvaluations();
    for(unordered_map<string,double>::iterator it = evaluation.begin(); it != evaluation.end(); it++){
	string tmp = (*it).first;
	Pose p = evaluationRecords->getPoseFromEncoding(tmp);
	if(value <= (*it).second){
		newTarget = p;
		value = (*it).second;
	    }//else continue;
    }
    pair<Pose,double> result = make_pair(newTarget,value);
    
    // i switch x and y to allow debugging graphically looking the image
    cout << "New target : " << "x = "<<newTarget.getY() <<", y = "<< newTarget.getX() << ", orientation = " 
	    <<newTarget.getOrientation() << ", Evaluation: "<< value << endl;
    return result;
}

void Robot::calcAction(){
	vector<Pose> poses;
	vector<Pose> grSim2ompl;

	for(int i = 0 ; i < robots->size() ; i++){
		poses.push_back(robots->at(i).getActPose());
		grSim2ompl.push_back(common::grSim2OMPL(robots->at(i).getActPose()));
		grSim2ompl.at(i).show();
	}

	potentialField.setRobots(poses);

	/*pathPlanning.setRobots(grSim2ompl);

	offpath = pathPlanning.solvePath(id, common::grSim2OMPL(*ball));

	for(int i = 0 ; i < offpath.poses.size() ; i++){
		offpath.poses.at(i) = common::OMPL2grSim(offpath.poses.at(i));
	}

	if(id == 0)
		offpath.show();*/

	Pose targetPosition = potentialField.calcResult(id, *ball, true);

	targetPosition.setX(actPose.getX() + targetPosition.getX());
	targetPosition.setY(actPose.getY() + targetPosition.getY());

	command = pid.calcCommand(actPose, targetPosition);

	// if(have to plan a new path)
	//		Plan();
	//
	// if(distance of robot to pose_i < some_value)
	//		i++;
	//
	// Pose potential = potentialField(robot to pose_i);
	//
	// Command cmd = pid.calcCommand(act pose to potential);
}

// Input : ./mcdm_online_exploration_ros ./../Maps/map_RiccardoFreiburg_1m2.pgm 100 75 5 0 15 180 0.95 0.12
// resolution x y orientation range centralAngle precision threshold
int main ( int argc, char **argv )
{
  auto startMCDM = chrono::high_resolution_clock::now();
  ifstream infile;
  infile.open ( argv[1] );  // the path to the map
  double resolution = atof ( argv[2] );  // the resolution of the map
  double imgresolution = atof ( argv[10] );  // the resolution to use for the planningGrid and RFIDGrid
  dummy::Map map = dummy::Map ( infile,resolution, imgresolution );
  RFIDGridmap myGrid(argv[1], resolution, imgresolution, false);
  cout << "Map dimension: " << map.getNumGridCols() << " : "<<  map.getNumGridRows() << endl;
  int gridToPathGridScale = map.getGridToPathGridScale();
  // i switched x and y because the map's orientation inside and outside programs are different
  long initX = static_cast<long>( atoi ( argv[4] ) *imgresolution );  // initial X-position of the robot in map frame
  long initY = static_cast<long>( atoi ( argv[3] ) *imgresolution );  // initial Y-position of the robot in map frame
  std::cout << "initX: " << initX << " initY: " << initY << std::endl;
  int initOrientation = atoi ( argv[5] );  // initial orientation of the robot in map frame
  double initFov = atoi ( argv[7] );  // initial FOV of the robot sensor
  initFov = initFov * PI /180;
  int initRange = atoi ( argv[6] );
  double precision = atof ( argv[8] );
  double threshold = atof ( argv[9] );
  //x,y,orientation,range,FOV

  Pose initialPose = Pose ( initX,initY,initOrientation,initRange,initFov );
  Pose invertedInitial = createFromInitialPose ( initX,initY,initOrientation,180,initRange,initFov );
  Pose eastInitial = createFromInitialPose ( initX,initY,initOrientation,90,initRange,initFov );
  Pose westInitial = createFromInitialPose ( initX,initY,initOrientation,270,initRange,initFov );
  Pose target = initialPose;
  Pose previous = initialPose;
  long numConfiguration = 1;
  vector<pair<string,list<Pose>>> graph2;
  NewRay ray;
  ray.setGridToPathGridScale ( gridToPathGridScale );
  MCDMFunction function;
  long sensedCells = 0;
  long newSensedCells =0;
  long totalFreeCells = map.getTotalFreeCells();
  int count = 0;
  double travelledDistance = 0;
  int numOfTurning = 0;
  unordered_map<string,int> visitedCell;
  vector<string>history;
  history.push_back ( function.getEncodedKey ( target,1 ) );
  EvaluationRecords record;
  //amount of time the robot should do nothing for scanning the environment ( final value expressed in second)
  unsigned int microseconds = 5 * 1000 * 1000 ;
  list<Pose> unexploredFrontiers;
  list<Pose> tabuList;
  tabuList.push_back(target);
  list<Pose> nearCandidates;
  bool btMode = false;
  double totalAngle = 0;
  Astar astar;
  double totalScanTime = 0;
  bool act = true;
  int encodedKeyValue = 0;

  // RFID
  double absTagX =  std::stod(argv[12]); // m.
  double absTagY =  std::stod(argv[11]); // m.
  double freq = std::stod(argv[13]); // Hertzs
  double txtPower= std::stod(argv[14]); // dBs
  std::pair<int, int> relTagCoord;

  do
  {
    // If we are doing "forward" navigation towards cells never visited before
    if ( btMode == false )
    {
      long x = target.getX();
      long y = target.getY();
      int orientation = target.getOrientation();
      int range = target.getRange();
      double FOV = target.getFOV();
      string actualPose = function.getEncodedKey ( target,0 );
      map.setCurrentPose ( target );
      string encoding = to_string ( target.getX() ) + to_string ( target.getY() );
      visitedCell.emplace ( encoding,0 );
      // Get the sensing time required for scanning
      target.setScanAngles ( ray.getSensingTime ( map,x,y,orientation,FOV,range ) );
      // Perform a scanning operation
      newSensedCells = sensedCells + ray.performSensingOperation ( map,x,y,orientation,FOV,range, target.getScanAngles().first, target.getScanAngles().second );
      // Calculate the scanning angle
      double scanAngle = target.getScanAngles().second - target.getScanAngles().first;
      // Update the overall scanning time
      totalScanTime += calculateScanTime ( scanAngle*180/PI );
      // Calculare the relative RFID tag position to the robot position
      relTagCoord = map.getRelativeTagCoord(absTagX, absTagY, target.getX(), target.getY());
      // Calculate the received power and phase
      double rxPower = received_power_friis(relTagCoord.first, relTagCoord.second, freq, txtPower);
      double phase = phaseDifference(relTagCoord.first, relTagCoord.second, freq);
      // Update the path planning and RFID map
      map.updatePathPlanningGrid ( x, y, range, rxPower - SENSITIVITY);
      myGrid.addEllipse(rxPower - SENSITIVITY, map.getNumGridCols() - target.getX(),  target.getY(), target.getOrientation(), -0.5, 7.0);
      // Search for new candidate position
      ray.findCandidatePositions ( map,x,y,orientation,FOV,range );
      vector<pair<long,long> >candidatePosition = ray.getCandidatePositions();
      ray.emptyCandidatePositions();
//.........这里部分代码省略.........

string EvaluationRecords::getEncodedKey(Pose& p)
{
    string key =  to_string(p.getX()) + "/" + to_string( p.getY()) + "/" + to_string( (int)p.getOrientation()) + "/" + to_string(p.getRange()) +"/" + to_string(p.getFOV());
    return key;
}