미로 탐색

이 튜토리얼에서는 완성된 프로그램을 한 단계씩 만들어요. 로봇은 벽과 구멍을 피하며 미로를 돌아다니고, 벽 토큰처럼 보이는 것을 보고해요. 첫 실행에서 실행했던 바로 그 프로그램이에요.

스타터 로봇과, 이동과 센서에서 배운 내용을 사용해요.

완성된 프로그램: maze.py · maze.cpp

경기장

다음과 같이 생긴 world1.wbt에서 실행하세요.

Webots에서 본 world1 월드

한 단계씩

1. 준비

먼저 프로그램은 로봇을 만들고, 바퀴 모터를 가져와 제한 없이 돌 수 있게 해요. speeds는 각 바퀴의 속도를 담고 있으며, 프로그램의 나머지 부분에서 이 값을 바꿔요. reported는 로봇이 이미 토큰을 보고한 위치를 기억해요.

Python
C/C++

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

#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. 센서

다음으로 스타터 로봇의 센서를 가져와 활성화해요.

센서 이름이 표시된 샘플 로봇

Python
C/C++

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)

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. 동작

이 함수들은 속도를 바꿔서 회전하거나 제자리에서 돌게 해요. delay는 로봇이 지정한 밀리초 동안 같은 동작을 계속하게 하고, getBrightness는 컬러 센서를 0(검정)부터 255(흰색)까지의 값 하나로 읽어요.

Python
C/C++

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)

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. 토큰 보기

이 함수는 카메라 이미지에서 벽 토큰을 찾아요. 토큰은 어두운 벽 위의 밝은 사각형이므로, 대략 정사각형이고 이미지 가장자리(보통 바닥이 있는 곳)에 닿지 않는 밝은 모양을 찾아요.

Python
C/C++

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

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

이것은 간단한 출발점이에요. 기호를 구별하지 못하고, 인지 표적도 잘 보지 못해요. 벽 토큰 튜토리얼에서 더 잘하는 방법을 보여 줘요.

5. 보고하기

토큰으로 점수를 얻으려면 로봇이 1초 넘게 정지한 뒤, 위치와 토큰의 종류를 보내야 해요. 각 토큰은 한 번만 점수를 얻을 수 있고 다시 보고하면 5점을 잃으므로, 프로그램은 보고한 위치를 기억해요. 이 메시지들은 통신에서 설명해요.

Python
C/C++

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

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. 메인 루프

마지막으로 메인 루프가 모든 것을 합쳐요. 매 스텝마다 최고 속도로 시작하고, 벽을 피해 돌고, 구멍에서 되돌아오고, 앞쪽 벽에 가까워지면 카메라가 보는 것을 보고해요.

Python
C/C++

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])

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

얼마나 잘하나요?

이 로봇은 그냥 돌아다녀요. 어디에 가 봤는지 모르고, 모든 피해자의 종류를 추측해요. 가끔 갇히기도 하는데, 그러면 20초 후 게임이 로봇을 체크포인트로 되돌려요. 첫 번째 프로그램으로는 충분해요.

해 보기

로봇에 가장 좋은 속도와 거리를 찾아보세요.
한쪽 벽을 따라가 보세요. 옆 센서를 일정한 거리로 유지하고, 앞 센서가 벽을 보면 회전하세요.
벽이 얼마나 가까운지에 따라 속도를 바꿔 보세요.

그다음 통신에서 점수 얻는 법을 배워 보세요.