Labirentte yol bul

Bu eğitimde adım adım eksiksiz bir program yazacaksın. Robot, duvarlardan ve çukurlardan kaçınarak labirentte dolaşır ve duvar işaretine benzeyen her şeyi bildirir. Bu, İlk çalıştırman sayfasında çalıştırdığın programdır.

Program, başlangıç robotunu ve Hareket ile Sensörler sayfalarında öğrendiklerini kullanır.

Programın tamamı: maze.py · maze.cpp

Saha

Programı şuna benzeyen world1.wbt üzerinde çalıştır:

Adım adım

1. Hazırlık

Program önce robotu oluşturur, tekerlek motorlarını alır ve motorların sınırsız dönmesine izin verir. speeds her tekerleğin hızını tutar; programın geri kalanı bu değerleri değiştirir. reported, robotun daha önce nerelerde işaret bildirdiğini hatırlar.

Python

from controller import Robot
import struct
import numpy as np
import cv2 as cv

robot = Robot()
timeStep = 32         # Milliseconds between steps
maxVelocity = 6.28    # The maximum speed of the wheels, in radians per second

leftWheel = robot.getDevice("left wheel motor")
rightWheel = robot.getDevice("right wheel motor")
leftWheel.setPosition(float("inf"))   # Let the wheels turn without limit
rightWheel.setPosition(float("inf"))

speeds = [maxVelocity, maxVelocity]   # [left wheel speed, right wheel speed]
reported = []                         # Positions where a token has already been reported

C/C++

#include <array>
#include <cmath>
#include <cstring>
#include <vector>
#include <opencv2/opencv.hpp>
#include <webots/Robot.hpp>
#include <webots/Motor.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/Camera.hpp>
#include <webots/Emitter.hpp>
#include <webots/GPS.hpp>

using namespace webots;
using namespace std;

Robot *robot = new Robot();
const int timeStep = 32;          // Milliseconds between steps
const double maxVelocity = 6.28;  // The maximum speed of the wheels, in radians per second

Motor *leftWheel = robot->getMotor("left wheel motor");
Motor *rightWheel = robot->getMotor("right wheel motor");

double speeds[2] = {maxVelocity, maxVelocity}; // {left wheel speed, right wheel speed}
vector<array<int, 2>> reported;                // Positions where a token has already been reported

2. Sensörler

Sonra başlangıç robotunun sensörlerini alır ve etkinleştirir:

Python

leftDist = robot.getDevice("leftDist")
frontDist = robot.getDevice("frontDist")
rightDist = robot.getDevice("rightDist")
camera = robot.getDevice("camera")
colorSensor = robot.getDevice("color")
gps = robot.getDevice("gps")
emitter = robot.getDevice("emitter")  # The emitter does not need to be enabled

for sensor in [leftDist, frontDist, rightDist, camera, colorSensor, gps]:
    sensor.enable(timeStep)

C/C++

DistanceSensor *leftDist = robot->getDistanceSensor("leftDist");
DistanceSensor *frontDist = robot->getDistanceSensor("frontDist");
DistanceSensor *rightDist = robot->getDistanceSensor("rightDist");
Camera *camera = robot->getCamera("camera");
Camera *colorSensor = robot->getCamera("color");
GPS *gps = robot->getGPS("gps");
Emitter *emitter = robot->getEmitter("emitter"); // The emitter does not need to be enabled

void enableSensors() {
  leftDist->enable(timeStep);
  frontDist->enable(timeStep);
  rightDist->enable(timeStep);
  camera->enable(timeStep);
  colorSensor->enable(timeStep);
  gps->enable(timeStep);
}

3. Hareketler

Bu fonksiyonlar, robotun dönüş yapması ve olduğu yerde dönmesi için hızları değiştirir. delay, robotun belirli bir milisaniye boyunca aynı şeyi yapmaya devam etmesini sağlar; getBrightness ise renk sensörünü 0 (siyah) ile 255 (beyaz) arasında tek bir değer olarak okur.

Python

def turnRight():
    speeds[0] = 0.6 * maxVelocity
    speeds[1] = -0.2 * maxVelocity

def turnLeft():
    speeds[0] = -0.2 * maxVelocity
    speeds[1] = 0.6 * maxVelocity

def spin():
    speeds[0] = 0.6 * maxVelocity
    speeds[1] = -0.6 * maxVelocity

def delay(ms):
    start = robot.getTime()
    while robot.step(timeStep) != -1:
        if (robot.getTime() - start) * 1000 > ms:
            break

def getBrightness():
    image = colorSensor.getImage()
    return colorSensor.imageGetGray(image, colorSensor.getWidth(), 0, 0)  # 0 (black) to 255 (white)

C/C++

void turnRight() {
  speeds[0] = 0.6 * maxVelocity;
  speeds[1] = -0.2 * maxVelocity;
}

void turnLeft() {
  speeds[0] = -0.2 * maxVelocity;
  speeds[1] = 0.6 * maxVelocity;
}

void spin() {
  speeds[0] = 0.6 * maxVelocity;
  speeds[1] = -0.6 * maxVelocity;
}

void delay(int ms) {
  double start = robot->getTime();
  while (robot->step(timeStep) != -1) {
    if ((robot->getTime() - start) * 1000 > ms)
      break;
  }
}

int getBrightness() {
  const unsigned char *image = colorSensor->getImage();
  return Camera::imageGetGray(image, colorSensor->getWidth(), 0, 0); // 0 (black) to 255 (white)
}

4. Bir işareti görme

Bu fonksiyon, kamera görüntüsünde duvar işaretlerini arar. İşaretler, koyu duvarların üzerindeki parlak karelerdir. Bu yüzden fonksiyon, kabaca kare biçiminde olan ve görüntünün kenarına (genellikle zeminin göründüğü yere) değmeyen parlak şekiller arar.

Python

def seesToken(image):
    height, width = camera.getHeight(), camera.getWidth()
    image = np.frombuffer(image, np.uint8).reshape((height, width, 4))  # BGRA image for OpenCV
    gray = cv.cvtColor(image, cv.COLOR_BGRA2GRAY)
    _, bright = cv.threshold(gray, 200, 255, cv.THRESH_BINARY)          # Bright parts in white
    contours, _ = cv.findContours(bright, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)
    for contour in contours:
        x, y, w, h = cv.boundingRect(contour)
        if x == 0 or y == 0 or x + w == width or y + h == height:
            continue  # Shapes touching the edge of the image are usually the floor
        if cv.contourArea(contour) > 40 and 0.65 < w / h < 1.5:
            return True  # A bright, roughly square shape on a wall
    return False

C/C++

bool seesToken(const unsigned char *image) {
  int height = camera->getHeight(), width = camera->getWidth();
  cv::Mat frame(height, width, CV_8UC4, (void *)image); // BGRA image for OpenCV
  cv::Mat gray, bright;
  cv::cvtColor(frame, gray, cv::COLOR_BGRA2GRAY);
  cv::threshold(gray, bright, 200, 255, cv::THRESH_BINARY); // Bright parts in white
  vector<vector<cv::Point>> contours;
  cv::findContours(bright, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
  for (const auto &contour : contours) {
    cv::Rect box = cv::boundingRect(contour);
    if (box.x == 0 || box.y == 0 || box.x + box.width == width || box.y + box.height == height)
      continue; // Shapes touching the edge of the image are usually the floor
    double ratio = (double)box.width / box.height;
    if (cv::contourArea(contour) > 40 && ratio > 0.65 && ratio < 1.5)
      return true; // A bright, roughly square shape on a wall
  }
  return false;
}

Bu basit bir başlangıçtır: sembolleri ayırt edemez ve bilişsel hedefleri iyi göremez. Duvar işaretleri eğitimi, nasıl daha iyisini yapabileceğini gösterir.

5. Bildirme

Bir işaretten puan almak için robot 1 saniyeden uzun süre durmalı, sonra işaretin konumunu ve türünü göndermelidir. Her işaretten yalnızca bir kez puan alınabilir ve aynı işareti tekrar bildirmek 5 puana mal olur; bu yüzden program, nerelerde bildirim yaptığını hatırlar. Bu mesajlar İletişim sayfasında açıklanır.

Python

def report(tokenType):
    leftWheel.setVelocity(0)  # Stop for more than 1 second
    rightWheel.setVelocity(0)
    delay(1300)
    x = int(gps.getValues()[0] * 100)  # Meters to centimeters
    z = int(gps.getValues()[2] * 100)
    emitter.send(struct.pack("i i c", x, z, tokenType.encode()))
    robot.step(timeStep)
    reported.append((x, z))

def alreadyReported():
    # Reporting the same token again is a misidentification
    x = gps.getValues()[0] * 100
    z = gps.getValues()[2] * 100
    return any(abs(x - rx) < 12 and abs(z - rz) < 12 for rx, rz in reported)  # Closer than 1 tile

C/C++

void report(char tokenType) {
  leftWheel->setVelocity(0); // Stop for more than 1 second
  rightWheel->setVelocity(0);
  delay(1300);
  int x = (int)(gps->getValues()[0] * 100); // Meters to centimeters
  int z = (int)(gps->getValues()[2] * 100);
  char message[9];
  memcpy(&message[0], &x, 4);
  memcpy(&message[4], &z, 4);
  message[8] = tokenType;
  emitter->send(message, 9);
  robot->step(timeStep);
  reported.push_back({x, z});
}

bool alreadyReported() {
  // Reporting the same token again is a misidentification
  double x = gps->getValues()[0] * 100, z = gps->getValues()[2] * 100;
  for (const auto &position : reported)
    if (fabs(x - position[0]) < 12 && fabs(z - position[1]) < 12) // Closer than 1 tile
      return true;
  return false;
}

6. Ana döngü

Son olarak ana döngü her şeyi bir araya getirir. Her adımda tam hızla başlar, duvarlardan uzaklaşmak için döner, çukurlardan geri döner ve önünde yakın bir duvar olduğunda kameranın gördüğünü bildirir.

Python

while robot.step(timeStep) != -1:
    speeds[0] = maxVelocity
    speeds[1] = maxVelocity

    if leftDist.getValue() < 0.05:   # Wall on the left: turn right
        turnRight()
    if rightDist.getValue() < 0.05:  # Wall on the right: turn left
        turnLeft()
    if frontDist.getValue() < 0.05:  # Wall in front: spin
        spin()

    if getBrightness() < 80:         # Black floor: turn away from the hole
        spin()
        leftWheel.setVelocity(speeds[0])
        rightWheel.setVelocity(speeds[1])
        delay(600)

    # Close to a wall, and something on it: report it
    if frontDist.getValue() < 0.07 and not alreadyReported() and seesToken(camera.getImage()):
        report("H")  # The type cannot be told yet, so guess "H"

    leftWheel.setVelocity(speeds[0])
    rightWheel.setVelocity(speeds[1])

C/C++

int main() {
  enableSensors();
  leftWheel->setPosition(INFINITY); // Let the wheels turn without limit
  rightWheel->setPosition(INFINITY);

  while (robot->step(timeStep) != -1) {
    speeds[0] = maxVelocity;
    speeds[1] = maxVelocity;

    if (leftDist->getValue() < 0.05) // Wall on the left: turn right
      turnRight();
    if (rightDist->getValue() < 0.05) // Wall on the right: turn left
      turnLeft();
    if (frontDist->getValue() < 0.05) // Wall in front: spin
      spin();

    if (getBrightness() < 80) { // Black floor: turn away from the hole
      spin();
      leftWheel->setVelocity(speeds[0]);
      rightWheel->setVelocity(speeds[1]);
      delay(600);
    }

    // Close to a wall, and something on it: report it
    if (frontDist->getValue() < 0.07 && !alreadyReported() && seesToken(camera->getImage()))
      report('H'); // The type cannot be told yet, so guess 'H'

    leftWheel->setVelocity(speeds[0]);
    rightWheel->setVelocity(speeds[1]);
  }

  delete robot;
  return 0;
}

Ne kadar başarılı?

Bu robot rastgele dolaşır: nerelerde bulunduğunu bilmez ve her kazazedenin türünü tahmin eder. Bazen sıkışır ve oyun onu 20 saniye sonra bir kontrol noktasına geri taşır. İlk program için bu gayet iyi.

Dene

• Robotun için en iyi hızları ve mesafeleri bul.
• Bir taraftaki duvarı izle: yan sensörü sabit bir mesafede tut ve ön sensör bir duvar gördüğünde dön.
• Hızı duvarların ne kadar yakın olduğuna göre değiştir.

Sonra İletişim sayfasında puan kazanmayı öğren.