map_location_alg.cpp

Go to the documentation of this file.

#include "map_location_alg.h"

MapLocationAlgorithm::MapLocationAlgorithm(void)
{
  
 
  
  this->colorList.push_back(CV_RGB(240, 100,   0)); //orange  0
  this->colorList.push_back(CV_RGB(  0, 240,   0)); //green   1
  this->colorList.push_back(CV_RGB(  0,   0, 240)); //blue    2
  this->colorList.push_back(CV_RGB(  0, 240, 240)); //cyan    3
  this->colorList.push_back(CV_RGB(240,   0, 240)); //pink    4
  this->colorList.push_back(CV_RGB(240, 240,   0)); //yellow  5
  this->colorList.push_back(CV_RGB(  0,   0,   0)); //black   6
  this->colorList.push_back(CV_RGB(240, 240, 240)); //white   7

  this->textList.push_back("Your current location");
  this->textList.push_back("Asked destination"); 
  this->textList.push_back("Another helpful robot");
  this->textList.push_back("Transport vehicle");
  this->textList.push_back("Interesting place");
  this->textList.push_back("Extra information");
  
  this->initialize();
  t=0;
}

MapLocationAlgorithm::~MapLocationAlgorithm(void)
{
  cv::destroyWindow(WINDOW);
}

void MapLocationAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // save the current configuration
  this->path =  new_cfg.path;
  this->config_=new_cfg;
  
  this->unlock();
}

// MapLocationAlgorithm Public API
void MapLocationAlgorithm::initialize()
{

}

void MapLocationAlgorithm::mapIteration()
{
  //ROS_INFO("MapLocationAlgorithm::mapIteration");
  this->initialize();
  this->image = this->mapImage.clone();
  if(this->locations.size()!=0)
  {
    this->loadCaption();
    this->loadLocations();
    t=t+10;
  }
  //this->showImage(); //deprecated
}

void MapLocationAlgorithm::logoIteration()
{
  //ROS_INFO("MapLocationAlgorithm::logoIteration");
  cv::Mat img1 = this->logoImage;
  cv::Mat img2 = this->logo2Image;
  cv::Mat img3(cv::Size(img2.cols, img2.rows+img2.rows), CV_8UC3, CV_RGB(255, 255, 255));

  //img1.copyTo(img3(cv::Rect(0, 0        , img1.cols, img1.rows)));
  img2.copyTo(img3(cv::Rect(0, 0, img2.cols, img2.rows)));
  img2.copyTo(img3(cv::Rect(0, img2.rows, img2.cols, img2.rows)));
  
  this->image = img3;
}

cv::Mat MapLocationAlgorithm::loadImage(std::string imagePath, int mode)
{
  //Load cv image from file
  cv::Mat loadedImage = cv::imread(imagePath, mode); 
    //mode
    // 0 CV_LOAD_IMAGE_GRAYSCALE 
    // 1 CV_LOAD_IMAGE_COLOR 
    //-1 CV_LOAD_IMAGE_UNCHANGED
  if(! loadedImage.data )
    ROS_INFO("Could not open or find the image: %s", imagePath.c_str());
  else
    ROS_INFO("Loaded image: %s", imagePath.c_str());
  return loadedImage;
}

void MapLocationAlgorithm::loadCaption()
{
  int height = 400;
  int margin = 20;
  
  int width  = height*3;
  int startx = margin;
  int starty = this->image.rows - height - margin;
  
  //Background caption rectangle
  cv::Point pt1(startx, starty);
  cv::Point pt2(startx+width, starty+height);
  cv::rectangle(this->image, pt1, pt2, colorList[7], CV_FILLED, CV_AA, 0);
  cv::rectangle(this->image, pt1, pt2, colorList[6], 2,         CV_AA, 0);

  //Font and text 
  int    fontFace      = CV_FONT_HERSHEY_SIMPLEX;
  double fontScale     = height / (this->locations.size()*22); //22 height of CV_FONT_HERSHEY_SIMPLEX at scale 1
  float  fontShare     = 0.0; //no italic
  int    fontThickness = 3.0;
  int    fontLineType  = CV_AA;
  
  int blankSpace    = 100;
  int verticalSpace = 30;
  
  CvFont font;
  cv::Size textSize;
  this->initText(&textSize, &font, fontFace, fontScale, fontShare, fontThickness, fontLineType, currentText);

  //Make text fit in height
  while( signed(this->locations.size())*textSize.height + (signed(this->locations.size())+1)*verticalSpace > height)
  {
    this->initText(&textSize, &font, fontFace, fontScale, fontShare, fontThickness, fontLineType, currentText);
    fontScale*=0.9;
    if(fontScale<0.5)
      break;
  }
  //Make text fit in width
  while(textSize.width + blankSpace > width)
  {
    this->initText(&textSize, &font, fontFace, fontScale, fontShare, fontThickness, fontLineType, currentText);
    fontScale*=0.9;
    if(fontScale<0.5)
      break;
  }
  //Enlarge text to fill caption width and height
  while((( this->locations.size()*textSize.height*1.1 + (this->locations.size()+1)*verticalSpace) < height ) && ( ( textSize.width*1.1 + blankSpace) < width ))
  {
    this->initText(&textSize, &font, fontFace, fontScale, fontShare, fontThickness, fontLineType, currentText);
    fontScale*=1.1;
    if(fontScale>4)
      break;
  }
  //Draw text and circles
  IplImage dst_img = this->image;
  for(unsigned int i=0; i<this->locations.size(); i++)
  {
    cvPutText(&dst_img, this->currentText[i].c_str(), cv::Point(pt1.x + blankSpace, pt1.y + (i+1)*verticalSpace + (i+1)*textSize.height),   &font, currentColor[i]);
    this->drawCircle( startx + blankSpace/2, starty + (i+1)*verticalSpace + (i+(1./2.))*textSize.height      , 30, currentColor[i]);
  }
}

void MapLocationAlgorithm::loadLocations()
{
  for(unsigned int i=0; i<locations.size(); i++)
  {
    int x  = locations[i][0];
    int y  = locations[i][1];
    int r  = locations[i][2];
    //int id = locations[i][3];
    this->drawCircle(x,y,r,currentColor[i]);
    //ROS_INFO("Drawing location: %i, %i, %i, %i", x, y, r, id);
  }
}

void MapLocationAlgorithm::initText(cv::Size * textSize, CvFont * font, int const fontFace, double const fontScale, float const fontShare, int const fontThickness, int const fontLineType, std::vector<std::string> const currentText)
{
  textSize->width=0;
  textSize->height=0;
  cvInitFont(font, fontFace , fontScale, fontScale, fontShare, fontThickness, fontLineType);
  for(unsigned int i=0; i<locations.size(); i++)
  {
    cv::Size textSize2 = cv::getTextSize(currentText[i].c_str(), fontFace, fontScale, fontThickness, 0);
    textSize->width  = std::max(textSize->width, textSize2.width);
    textSize->height = std::max(textSize->height, textSize2.height);
  }
}

void MapLocationAlgorithm::drawCircle(int x, int y, int r, CvScalar color)
{
  int img_x = x; //*this->image.cols;
  int img_y = y; //*this->image.rows;
  
  cv::circle(this->image, cv::Point(img_x, img_y), r, color, -1);
  cv::circle(this->image, cv::Point(img_x, img_y), r, CV_RGB(0,0,0), 2);
  
  if( ( img_x+r > this->image.cols ) || ( img_y+r > this->image.rows ) || (img_x-r < 0 ) || (img_y-r < 0) )
    ROS_INFO("Circle out of bounds");
}

void MapLocationAlgorithm::addLocation(float xpos, float ypos, int id)
{
  int x = 1*xpos;
  int y = 1*ypos;
  int r = 30;
  bool added=false;
  
  if(id<0)
  {
    for(unsigned int i=0; i<locations.size(); i++)
    {
      if(locations[i][3]==abs(id))
      {
        //ROS_INFO("MapLocationAlgorithm::addLocation: Deleted location: %i, %i, %i, %i", locations[i][0], locations[i][1], locations[i][2], locations[i][3]);
        locations.erase(locations.begin()+i);
        currentColor.erase(currentColor.begin()+i);
        currentText.erase(currentText.begin()+i);        
        return;
      }
    }
    return;
  }

  for(unsigned int i=0; i<locations.size(); i++)
  {
    if(id==locations[i][3])
    {
      locations[i][0]=x;
      locations[i][1]=y;
      locations[i][2]=r;
      locations[i][3]=id;
      //ROS_INFO("MapLocationAlgorithm::addLocation: Updated location: %i, %i, %i, %i", x, y, r, id);
      added=true;
    }
  }
  if(!added)
  {
    std::vector<int> location;
    location.push_back(x);
    location.push_back(y);
    location.push_back(r);
    location.push_back(id);
    locations.push_back(location);
    ROS_INFO("MapLocationAlgorithm::addLocation: Added location: %i, %i, %i, %i", x, y, r, id);
    currentColor.push_back(colorList[id]);
    currentText.push_back(textList[id]);
  }
}

void MapLocationAlgorithm::addLocationWithCode(int code)
{
  int x=0,y=0;
  
  switch(code)
  {
    case   0: x =  882; y = 1035; break; //biblioteca
    case   1: x =  666; y =  678; break; //plaza caminos
    case   2: x = 1420; y =  672; break; //plaza informatica
    case   3: x =  450; y = 1113; break; //nexus
    case   4: x = 1467; y = 1485; break; //nexus2
    case   5: x = 1350; y = 1074; break; //omega
    case   6: x = 1083; y =  618; break; //optica
    case   7: x = 1050; y = 1030; break; //polideportivo
    case   8: x = 2120; y = 1400; break; //rectorado
    case   9: x = 1044; y =  678; break; //plaza telecos
    case  10: x = 2150; y =  447; break; //vertex
    case 101: x =  575; y =  825; break; //a1...6
    case 102: x =  765; y =  825; break;
    case 103: x =  954; y =  825; break;
    case 104: x = 1140; y =  825; break;
    case 105: x = 1333; y =  825; break;
    case 106: x = 1518; y =  825; break;
    case 201: x =  507; y =  666; break; //b1...6
    case 202: x =  807; y =  666; break;
    case 203: x =  921; y =  666; break;
    case 204: x = 1185; y =  666; break;
    case 205: x = 1296; y =  666; break;
    case 206: x = 1560; y =  666; break;
    case 301: x =  441; y =  560; break; //c1...6
    case 302: x =  768; y =  560; break;
    case 303: x =  957; y =  560; break;
    case 304: x = 1149; y =  560; break;
    case 305: x = 1335; y =  560; break;
    case 306: x = 1521; y =  560; break;
    case 401: x =  420; y =  429; break; //d1...6
    case 402: x =  768; y =  429; break;
    case 403: x =  957; y =  429; break;
    case 404: x = 1149; y =  429; break;
    case 405: x = 1335; y =  429; break;
    case 406: x = 1521; y =  429; break;
    case  -1:
      addLocation(0  , 0,  -1);
      return;
      break;
    default:  x =    0; y =    0; break;
  }
  
  addLocation(x  , y,  1); //green
}

cv::Mat  MapLocationAlgorithm::getImage()
{
  return this->image;
}

void MapLocationAlgorithm::showImage()
{
  cv::namedWindow(WINDOW, CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO); // Create a window for display.
  cv::resizeWindow(WINDOW, 1024, 768);                             // Resize the window for display
  cv::imshow(WINDOW, this->image);                                 // Show our image inside it.
  cv::waitKey(0);
}

void MapLocationAlgorithm::loadFiles()
{
  //this->path="./";
  
  this->mapPath=this->path + "/map.jpg"; //map image dimensions 2814x2023
  //this->mapPath="/home/fherrero/Pictures/map.jpg"; //map image dimensions 2814x2023
  this->mapImage = this->loadImage(this->mapPath, 1);
  //this->logoPath="/home/fherrero/Pictures/iri.png";
  //this->logoImage = this->loadImage(this->logoPath, 1);
  //this->logo2Path="/home/fherrero/Pictures/upc.jpg";
  //this->logo2Image = this->loadImage(this->logo2Path, 1);
}