位置センサー

位置センサー（GPS）は、ロボットの位置を メートル 単位で返します。スターターロボットでは gps という名前です。壁のトークンを報告するには位置センサーが必要で、計画的に探索するときにもとても役立ちます。

getValues() は [x, y, z] を返します。X と Z が水平方向で、Y が垂直方向なので、迷路内の位置には X と Z を使います。

水平面の X 軸と Z 軸、垂直方向の Y 軸

Python
C/C++

from controller import Robot

robot = Robot()
timeStep = int(robot.getBasicTimeStep())

gps = robot.getDevice("gps")
gps.enable(timeStep)

while robot.step(timeStep) != -1:
    x, y, z = gps.getValues()  # In meters. X and Z are horizontal, Y is vertical.
    print("x:", round(x, 3), "z:", round(z, 3))

#include <iostream>
#include <webots/Robot.hpp>
#include <webots/GPS.hpp>

using namespace webots;
using namespace std;

int main() {
  Robot *robot = new Robot();
  int timeStep = (int)robot->getBasicTimeStep();

  GPS *gps = robot->getGPS("gps");
  gps->enable(timeStep);

  while (robot->step(timeStep) != -1) {
    const double *position = gps->getValues(); // In meters. X and Z are horizontal, Y is vertical.
    cout << "x: " << position[0] << " z: " << position[2] << endl;
  }

  delete robot;
  return 0;
}

GPS には数ミリメートルのノイズがあるので、ロボットが止まっていても値は少しずつ変わります。位置が完全に一致するかどうかで比較してはいけません。

例: スタートからの距離

このプログラムは、ロボットがスタート地点からタイル半分の距離に達するまで前進します。

Python
C/C++

from controller import Robot
import math

robot = Robot()
timeStep = int(robot.getBasicTimeStep())

gps = robot.getDevice("gps")
gps.enable(timeStep)
leftWheel = robot.getDevice("left wheel motor")
rightWheel = robot.getDevice("right wheel motor")
leftWheel.setPosition(float("inf"))
rightWheel.setPosition(float("inf"))

robot.step(timeStep)  # Step once, so that the GPS has a value
startX, _, startZ = gps.getValues()

leftWheel.setVelocity(3.0)
rightWheel.setVelocity(3.0)

while robot.step(timeStep) != -1:
    x, _, z = gps.getValues()
    distance = math.sqrt((x - startX) ** 2 + (z - startZ) ** 2)
    print("Distance from the start:", round(distance, 3), "m")
    if distance > 0.06:  # Half a tile
        leftWheel.setVelocity(0)
        rightWheel.setVelocity(0)
        break

#include <iostream>
#include <cmath>
#include <webots/Robot.hpp>
#include <webots/GPS.hpp>
#include <webots/Motor.hpp>

using namespace webots;
using namespace std;

int main() {
  Robot *robot = new Robot();
  int timeStep = (int)robot->getBasicTimeStep();

  GPS *gps = robot->getGPS("gps");
  gps->enable(timeStep);
  Motor *leftWheel = robot->getMotor("left wheel motor");
  Motor *rightWheel = robot->getMotor("right wheel motor");
  leftWheel->setPosition(INFINITY);
  rightWheel->setPosition(INFINITY);

  robot->step(timeStep); // Step once, so that the GPS has a value
  double startX = gps->getValues()[0], startZ = gps->getValues()[2];

  leftWheel->setVelocity(3.0);
  rightWheel->setVelocity(3.0);

  while (robot->step(timeStep) != -1) {
    double x = gps->getValues()[0], z = gps->getValues()[2];
    double distance = sqrt(pow(x - startX, 2) + pow(z - startZ, 2));
    cout << "Distance from the start: " << distance << " m" << endl;
    if (distance > 0.06) { // Half a tile
      leftWheel->setVelocity(0);
      rightWheel->setVelocity(0);
      break;
    }
  }

  delete robot;
  return 0;
}