カラーセンサー

カラーセンサーは床を見て、穴、沼地、通路があることをロボットに知らせます。下を向いた 1 ピクセルだけのカメラです。スターターロボットでは color という名前で、車輪が穴に落ちる前に穴が見えるよう、ロボットの前方に付いています。

Python

from controller import Robot

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

colorSensor = robot.getDevice("color")
colorSensor.enable(timeStep)

while robot.step(timeStep) != -1:
    image = colorSensor.getImage()  # A single pixel
    r = colorSensor.imageGetRed(image, 1, 0, 0)
    g = colorSensor.imageGetGreen(image, 1, 0, 0)
    b = colorSensor.imageGetBlue(image, 1, 0, 0)
    print("r:", r, "g:", g, "b:", b)

C/C++

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

using namespace webots;
using namespace std;

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

  Camera *colorSensor = robot->getCamera("color");
  colorSensor->enable(timeStep);

  while (robot->step(timeStep) != -1) {
    const unsigned char *image = colorSensor->getImage(); // A single pixel
    int r = Camera::imageGetRed(image, 1, 0, 0);
    int g = Camera::imageGetGreen(image, 1, 0, 0);
    int b = Camera::imageGetBlue(image, 1, 0, 0);
    cout << "r: " << r << " g: " << g << " b: " << b << endl;
  }

  delete robot;
  return 0;
}

床の色

次の値は、world2.wbt でスターターロボットを使って測ったものです。

床	赤	緑	青
通常の床	252	252	252
沼地	245	222	142
エリア 1 と 2 の間の通路（青）	91	91	255
エリア 2 と 3 の間の通路（紫）	193	91	251
エリア 1 と 4 の間の通路（緑）	48	255	48
エリア 3 と 4 の間の通路（赤）	255	91	91

注意

値はワールドによって変わります。world1.wbt では、通常の床は約 240 になります。正確な値ではなく範囲を使い、Erebus の player_controllers フォルダーにある calibrateColourSensor.py で確認しましょう。

穴のふちは黒いので、穴の直前で床が暗くなります。このセンサーにはチェックポイントが通常の床とほとんど同じに見えるので、自分のワールドのチェックポイントでキャリブレーションしてください。

例: 床を監視する

このプログラムは前進しながら、床が変わるたびにその床を表示し、穴を見つけたら止まります。

Python

from controller import Robot

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

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

def floorType(r, g, b):
    # Tune these ranges for your own robot and worlds
    brightness = (r + g + b) / 3
    if brightness < 80:
        return "hole"
    if r > 220 and g > 200 and b < 190:
        return "swamp"
    if b > 220 and g < 130:
        return "purple passage" if r > 150 else "blue passage"
    if r > 220 and g < 130 and b < 130:
        return "red passage"
    if g > 220 and r < 100 and b < 100:
        return "green passage"
    return "floor"

leftWheel.setVelocity(3.0)
rightWheel.setVelocity(3.0)
last = None

while robot.step(timeStep) != -1:
    image = colorSensor.getImage()
    floor = floorType(colorSensor.imageGetRed(image, 1, 0, 0),
                      colorSensor.imageGetGreen(image, 1, 0, 0),
                      colorSensor.imageGetBlue(image, 1, 0, 0))
    if floor != last:
        print("Floor:", floor)
        last = floor
    if floor == "hole":  # Stop before falling in
        leftWheel.setVelocity(0)
        rightWheel.setVelocity(0)
        break

C/C++

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

using namespace webots;
using namespace std;

// Tune these ranges for your own robot and worlds
string floorType(int r, int g, int b) {
  int brightness = (r + g + b) / 3;
  if (brightness < 80)
    return "hole";
  if (r > 220 && g > 200 && b < 190)
    return "swamp";
  if (b > 220 && g < 130)
    return r > 150 ? "purple passage" : "blue passage";
  if (r > 220 && g < 130 && b < 130)
    return "red passage";
  if (g > 220 && r < 100 && b < 100)
    return "green passage";
  return "floor";
}

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

  Camera *colorSensor = robot->getCamera("color");
  colorSensor->enable(timeStep);
  Motor *leftWheel = robot->getMotor("left wheel motor");
  Motor *rightWheel = robot->getMotor("right wheel motor");
  leftWheel->setPosition(INFINITY);
  rightWheel->setPosition(INFINITY);

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

  while (robot->step(timeStep) != -1) {
    const unsigned char *image = colorSensor->getImage();
    string floor = floorType(Camera::imageGetRed(image, 1, 0, 0), Camera::imageGetGreen(image, 1, 0, 0),
                             Camera::imageGetBlue(image, 1, 0, 0));
    if (floor != last) {
      cout << "Floor: " << floor << endl;
      last = floor;
    }
    if (floor == "hole") { // Stop before falling in
      leftWheel->setVelocity(0);
      rightWheel->setVelocity(0);
      break;
    }
  }

  delete robot;
  return 0;
}