迷宮導航

在這個教學中，你會一步一步建立一個完整的程式。機器人會在迷宮中四處行駛，避開牆壁和坑洞，並回報任何看起來像牆上標記的東西。這就是你在首次執行中執行過的程式。

它使用入門機器人，以及你在移動和感測器中學到的內容。

完整的程式：maze.py · maze.cpp

場地

在 world1.wbt 上執行它，這個世界看起來像這樣：

一步一步來

1. 設定

首先，程式會建立機器人、取得輪子馬達，並讓它們不受限制地轉動。speeds 儲存每個輪子的速度，程式的其他部分會改變它。reported 會記住機器人已經在哪些位置回報過標記。

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. 感測器

接著，程式會取得並啟用入門機器人的感測器：

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. 動作

這些函式會改變速度來轉彎和原地旋轉。delay 讓機器人在指定的毫秒數內持續做同一件事，getBrightness 則把顏色感測器的讀數轉成一個從 0（黑色）到 255（白色）的值。

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. 看到標記

這個函式會在攝影機影像中尋找牆上的標記。標記是深色牆壁上的明亮方塊，所以它會尋找大致呈方形、而且沒有碰到影像邊緣（通常是地板所在的位置）的明亮形狀。

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

這只是一個簡單的開始：它無法分辨符號，也不太能看清認知目標。牆上的標記教學會說明如何做得更好。

5. 回報

要靠標記得分，機器人必須停止超過 1 秒，並傳送它的位置和標記的類型。每個標記只能得分一次，重複回報會扣 5 分，所以程式會記住它已經在哪裡回報過。通訊會說明這些訊息。

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. 主迴圈

最後，主迴圈把所有東西整合在一起。每一步，它都會以全速開始、避開牆壁轉向、遇到坑洞時掉頭，並在前方接近牆壁時回報攝影機看到的東西。

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

它的表現如何？

這台機器人只是四處亂走：它不知道自己去過哪裡，而且會猜測每個受害者的類型。它有時會卡住，比賽會在 20 秒後把它移回檢查點。作為第一個程式，這樣就很好了。

試試看

• 找出最適合你的機器人的速度和距離。
• 沿著一側的牆走：讓側面感測器保持固定的距離，並在前方感測器偵測到牆壁時轉彎。
• 依照與牆壁的距離改變速度。

接下來，在通訊中學習如何得分。