[Up]

Implementation of Automonous Drive

Michael Smirnoff, Alex Smirnoff

Method

The goal with the autonomous navigation is to make the robot approach the target and stop at some pre-defined distance from it. The target consists of two boxes colored in a distinct color (in our case - green). The adopted method of achieving this in autonomous mode consists of:

  1. Bringing the robot to the position at some distance from the target.
  2. Keep turning and using the camera to analyze if the target is within the view.
  3. When the target is within the view turn to face the target straight on.
  4. Keep moving toward the target till its image in camera view reaches the desired size.

Implementation

The navigation by camera is implemented in two files: drive.cc which implements routines for manual and autonomous drive and autonomous.cc which implements navigation by the camera.

Main Routine

The drive.cc file contains the routine to navigate using a keyboard and a function to start the autonomous mode by pressing the 'a' key.

The autonomous function consists of two parts:

  1. Moving a certain distance without using camera.
  2. Navigating by camera.

Below is the snipped of this function. 

// 
// Autonomous navigation
//
void autonomous(gazebo::transport::PublisherPtr robot) {
  std::cout << "START Autonomous\n\r";

  // Go blindly to position:
  velocity = BLIND_SPEED; angle = 0.0;
  Time::start(BLIND_TIME);
  drive(robot);
  while (!Time::timeout()) {
    Time::pause();
  }
  stop(robot);

  // 
  // Navigate by camera
  //
  static int max_attempts = 100;
  extern bool
    reached_target,
    camera_off;
  Time::start(VISUAL_TIME);
  while (!reached_target && !Time::timeout()) {
    int attempts = 0;
    camera_off = false;
    for (; camera_off == false && attempts<max_attempts; attempts++) {
      // It is assumed that the global variable
      // 'camera_off' is updated in the callback 
      // function 'navigate(...)'
      Time::pause();
    }
    if (attempts == max_attempts) {
      std::cout << "Camera failed after "<<attempts<<" attempts!\n\r";
      break;
    }
    camera_off = true;
    std::cout << "Time left: "<<Time::time_left()<<"\r"; 
    drive(robot);
  }
  stop(robot);
  //TODO: Here you do gear deposition
  //...
  std::cout << "\n\rEND Autonomous\n\r";
}

The drive function called by the above code sends two variables to the robot simulator: velocity and angle. These are global variables defined in drive.cc and continuously updated by the navigate function described below.

Navigation by Camera

File autonomous.cc implements the navigation by camera function, navigate, which is a callback function called every time the camera sends an image. The function consists of three parts:

  1. Keep turning till there are exactly two square objects of a distinct color in full view.
  2. Determine the geometrical center of the two objects and keep turning till this center is positioned at the desired location in the camera view.
  3. Determine the distance between the two objects and keep moving toward or away from the objects till this distance matches the desired distance.

The adjustable parameters are

  1. Color threshold to identify the objects.
  2. The desired position of the geometrical center of the combined objects in the camera view.
  3. The desired distance between the objects (i.e. size of the target).
  4. Distance of the "blind" part of the autonomous drive, which is provided by specifying the speed and time for the "blind" drive.
  5. Speed for the turning motion and for approaching the target while navigating by the camera.

Below is a code snippet from file autonomous.cc showing the navigate function.


extern double velocity, angle;

bool
  camera_off = true,
  reached_target = false;


//-----------------------------
namespace cv {
//
// Navigate by camera
//
void navigate(ConstImageStampedPtr &_img)
{

  if (camera_off) return;
  static int
    half_nbuf = 4, //2*nbuf = number of pixels for averaging 
    nbuf = 2*half_nbuf;

  // 
  // Acquire the image
  //
  int
    width = _img->image().width(),
    height = _img->image().height(),
    size = _img->image().data().length(),
    desired_target_position = width/2, // middle of the screen
    desired_target_size = DESIRED_TARGET_IMAGE_SIZE, // in pixels
    min_offset = nbuf,
    min_size_mismatch = half_nbuf,
    xmin = nbuf,
    xmax = width - nbuf,
    ymin = 0.2 * height,
    ymax = 0.8 * height;

  char *img = new char[size + 1];
  memcpy(img, _img->image().data().c_str(), size);
  Mat image(height, width, CV_8UC3, img);
  //
  // Analyze the image and navigate to the target
  //
  static float turning_angle = 8.0; // degrees
  int
    m1 = 0, x1a = 0, // average x-coordinates of object 1
    m2 = 0, x2a = 0; // average x-coordinates of object 2
  //
  // Read image by pixels
  //
  for (int y=ymin; y<ymax; y++) {
    int
      n1 = 0, x1 = -1,
      n2 = 0, x2 = -1,
      state = 0;
        // 0 - before object 1
        // 1 - in object 1
        // 2 - after object 1
        // 3 - in object 2
        // 4 - after object 2
    for (int x=xmin; x<xmax; x++) {
      float ColorIntensity[] = {0,0,0};
      // [0] = red, [1] = green, [2] = blue

      // To provide resistance from random noise 
      // apply a filter by simple averaging of color 
      // intensity over a pixel strip of length 'nbuf'
      for (int i=x-half_nbuf; i<x+half_nbuf; i++) {
        Vec3b intensity = image.at<Vec3b>(y, i);
        for (int j=0; j<3; j++)
          ColorIntensity[2-j] += (float)intensity.val[j];
      }

      // Compute the relative intensity of the target color
      // this avoid the effects of variable lighting and shades
      float target_light_intensity = 0.0;
      for (int j=0; j<3; j++) {
        float normalized_intensity = ColorIntensity[j]/nbuf;
        target_light_intensity += normalized_intensity;
        ColorIntensity[j] = normalized_intensity;
      }
      float target_color_contrast // relative color intensity
      = ColorIntensity[TARGET_COLOR] / target_light_intensity;
      if (target_color_contrast > TARGET_COLOR_CONTRAST) {
        // 
        // Determine if both objects are in full view
        // i.e. state==4 -> both objects are in full view
        switch (state) {
        case 0: // first time we saw this color during x-scan
          x1 = x; n1 = 1;
          state = 1;
          break;
        case 1: // we are inside object 1
          x1 += x; n1++;
          break;
        case 2: // we are past object 1
          x2 = x; n2 = 1;
          state = 3;
          break;
        case 3: // inside object 2
          x2 += x; n2++;
          break;
        case 4: // past object 2
          std::cerr<<"More than two objects found!\n\r";
          break;
        default:
          std::cerr<<"Wrong scan state on green RGB("
          <<ColorIntensity[2]<<','
          <<ColorIntensity[1]<<','
          <<ColorIntensity[0]<<"): "<<state<<"\n\r";
        }
      } else {
        switch (state) {
        // current pixel color is not the target color
        case 1: // inside obj 1
          state = 2;
          break;
        case 3: // inside obj 2
          state = 4;
          break;
        default:
          if (state > 4) {
            std::cerr<<"Wrong scan state on RGB("
          <<ColorIntensity[2]<<','
          <<ColorIntensity[1]<<','
          <<ColorIntensity[0]<<"): "<<state<<"\n\r";
          }
        }
      }
    }//endfor x
    //
    // Compute average x-locations
    // for the two objects
    //
    if (state == 4) {// both objects are visible
      // comute the average positions and number of hits
      if (n1 > 0) {
        x1a += (int) ((float)x1 / (float) n1);
        m1++; // number of hits over all y-scans
      }
      if (n2 > 0) {
        x2a += (int) ((float)x2 / (float) n2);
        m2++;
      }
    }
  }//endofr y

  if (m1 > 0 && m2 > 0) { // target acquired
    //
    // Navigate toward the target
    //
    x1a /= m1; // this completes the averaging
    x2a /= m2;
    int
      target_position = 0.5*(x1a + x2a), // center of both ojbects
      target_size = fabs((double)x1a - (double)x2a),
      target_offset = desired_target_position - target_position,
      size_mismatch = desired_target_size - target_size;

    //
    // Check if the observed center is in alignment with the 
    // desired position in camera view
    //
    if (fabs(target_offset) > min_offset) {
      // turn toward the target:
      angle = sign(target_offset) * turning_angle;
      velocity = 0.0;
    } else {  // the target is straight on -
      // make a step toward the target
      angle = 0.0;
      if (fabs(size_mismatch) > min_size_mismatch) {
        velocity = sign(size_mismatch) * VISUAL_SPEED;
      } else { // we are at the target - stop here.
        std::cout << "\n\rTarget Reached!\n\r";std::cout.flush();
        reached_target = true;
        angle = 0.0;
        velocity = 0.0;
      }
    }
  } else {// target is not on the image: keep turning
    angle = turning_angle;
    velocity = 0.0;
  }
  delete img;

  camera_off = true;

}
} // end namespace cv

There are several adjustable constants used in the code defined in robot.h file.

Files & References:

  1. Autonomous navigation simulation (mp4).
  2. Complete archive (zip).
[Up]