movement_from_image.cpp

Go to the documentation of this file.

#include <iostream>
#include <string>

#include "sr_movements/movement_from_image.hpp"
#include <ros/ros.h>

namespace shadowrobot
{
  MovementFromImage::MovementFromImage(std::string image_path)
    : PartialMovement()
  {
    image_ = boost::shared_ptr<Magick::Image>( new Magick::Image() );
    image_->read(image_path);

    nb_cols_ = image_->columns();
    nb_rows_ = image_->rows();

    generate_movement_();
  }

  MovementFromImage::~MovementFromImage()
  {}


  void MovementFromImage::generate_movement_()
  {
    const Magick::PixelPacket* pixel_cache = image_->getConstPixels(0, 0, nb_cols_, nb_rows_);

    for (ssize_t col = 0; col < nb_cols_; ++col)
    {
      bool no_pixel = true;
      for (ssize_t row = 0; row < nb_rows_; ++row)
      {
        const Magick::PixelPacket* tmp_pixel = pixel_cache + row * nb_cols_ + col;
        if (tmp_pixel->red != 0xFFFF && tmp_pixel->green != 0xFFFF
           && tmp_pixel->blue != 0xFFFF)
        {
          no_pixel = false;
          steps.push_back(1.0 - static_cast<double>(row) / static_cast<double>(nb_rows_));
          break;
        }
      }
      if (no_pixel)
      {
        // not sending any targets for this point.
        steps.push_back(-1.0);
      }
    }
  }
}  // namespace shadowrobot

/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/