Colour Sensor

The colour sensor looks at the floor, so it tells the robot about holes, swamps and passages. It is a camera of a single pixel, pointing down. On the starter robot it is named color, at the front of the robot, so that it sees a hole before the wheels reach it.

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

Floor colours

These values were measured with the starter robot on world2.wbt:

Floor	Red	Green	Blue
Normal	252	252	252
Swamp	245	222	142
Passage between areas 1 and 2 (blue)	91	91	255
Passage between areas 2 and 3 (purple)	193	91	251
Passage between areas 1 and 4 (green)	48	255	48
Passage between areas 3 and 4 (red)	255	91	91

Caution

Values change with the world: normal floor reads about 240 in world1.wbt. Use ranges instead of exact values, and check them with calibrateColourSensor.py in the player_controllers folder of Erebus.

Holes have a black edge, so the floor gets dark just before a hole. Checkpoints look almost like normal floor to this sensor, so calibrate on one in your world.

Example: watching the floor

This program drives forward, prints the floor each time it changes, and stops if it sees a hole:

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