screen_information_alg.cpp

Go to the documentation of this file.

#include "screen_information_alg.h"

ScreenInformationAlgorithm::ScreenInformationAlgorithm(void)
{
  this->code = 0;
  
  //default own (robot/person) location (orange circle in map)
  ownX=1050;
  ownY=900;
}

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

void ScreenInformationAlgorithm::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();
}

//initialize: creates window and loads images from files
void ScreenInformationAlgorithm::initialize()
{
  std::string mapPath  = this->path + "/images/maps";
  std::string logoPath = this->path + "/images/logos";

  this->createWindow(WINDOW);

  cv::Mat blankImage(cv::Size(2048, 1536), CV_8UC3, CV_RGB(255, 255, 255));
  cv::Mat blankImage2(cv::Size(2048, 1536), CV_8UC3, CV_RGB(252, 235, 204));
  cv::Mat mapImage  = this->loadImage(mapPath  + "/map.jpg",CV_LOAD_IMAGE_COLOR);
  cv::Mat logoImage = this->loadImage(logoPath + "/iri2.png",CV_LOAD_IMAGE_COLOR);
  
  //Enlarge logo
  cv::Mat logoImageResize;
  cv::resize(logoImage, logoImageResize, cv::Size(), 3, 3, CV_INTER_LINEAR );
  logoImage=logoImageResize;

  // littleImage.copyTo(bigImage(cv::Rect(col0, row0, cols, rows)));

  //Put logo image in the center of the blank image
  cv::Mat logoBlankImage = blankImage.clone();
  logoImage.copyTo(logoBlankImage(cv::Rect(blankImage.cols/2-logoImage.cols/2, blankImage.rows/2-logoImage.rows/2, logoImage.cols, logoImage.rows)));
  
  
//   cv::Mat mapImageResize;
//   cv::resize(mapImage, mapImageResize, cv::Size(2048, 1536*mapImage.rows/2048), 0,0, CV_INTER_LINEAR);
//   mapImage=mapImageResize;
  //cv::Mat mapBlankImage 
  
  this->images.push_back(blankImage);     //0
  this->images.push_back(blankImage);     //1
  this->images.push_back(blankImage2);    //2
  this->images.push_back(mapImage);       //3
  this->images.push_back(logoBlankImage); //4
  
  this->texts.push_back("Stand-by");
  this->texts.push_back("Calculating ambient noise...");
  this->texts.push_back("Waiting for a question...");
  this->texts.push_back("Recording sound...");
  this->texts.push_back("Adquiring sound...");
  this->texts.push_back("Processing sound...");
  this->texts.push_back("Processing your question...");
  
  this->colors.push_back(CV_RGB(240, 100,   0)); //orange  0
  this->colors.push_back(CV_RGB(  0, 240,   0)); //green   1
  this->colors.push_back(CV_RGB(  0,   0, 240)); //blue    2
  this->colors.push_back(CV_RGB(  0, 240, 240)); //cyan    3
  this->colors.push_back(CV_RGB(240,   0, 240)); //pink    4
  this->colors.push_back(CV_RGB(240, 240,   0)); //yellow  5
  this->colors.push_back(CV_RGB(  0,   0,   0)); //black   6
  this->colors.push_back(CV_RGB(240, 240, 240)); //white   7
  
  
}

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

//createWindow: creates the window for display
void ScreenInformationAlgorithm::createWindow(const std::string& window)
{
  cv::namedWindow(window, CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO); // Create a window for display
  //cv::setWindowProperty(window, CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN); // Fullscreen mode
  cv::resizeWindow(window, 1024, 768); // Resize the window to desired resolution
}

//showImage: shows the image inside the window
void ScreenInformationAlgorithm::showImage(const std::string& window, cv::Mat image)
{
  //this->createWindow(window);
  cv::imshow(window, image); // Show our image inside the window.
  cv::waitKey(30);
}

//getImageIndex: depending on code, gives index of blank, blank2, or map image in images[]
int ScreenInformationAlgorithm::getImageIndex(int code)
{
  switch(code)
  {
    case 0:
    case 1:
    case 2:
    case 3:
    case 4:
    case 5:
    case 6:
      return 1; // satus image
      break;
    default:
      if(code<100) // normal answers
      {
        return 2; // text answer image        
      }
      else // location answers
      {
        return 3; // map answer image
      }
      break;
  }

}

//addText: adds text to image[0] depending on image index i and code
void ScreenInformationAlgorithm::addText(int i, int code)
{
  //Font and text 
  int    fontFace      = CV_FONT_HERSHEY_SIMPLEX;
  double fontScale     = 2.0; //22 height of CV_FONT_HERSHEY_SIMPLEX at scale 1
  float  fontShare     = 0.0; //no italic
  int    fontThickness = 2.0;
  int    fontLineType  = CV_AA;
 
  CvFont font;
  cv::Size textSize;
  cvInitFont(&font, fontFace , fontScale, fontScale, fontShare, fontThickness, fontLineType);

  //Adquisition status (show texts[code])
  if(code<10)
  {
    //ROS_INFO("ScreenInformationAlgorithm: this->code = %s", this->texts[code].c_str());
    putText(this->images[0], this->texts[code].c_str(), cv::Point(100, this->images[0].rows/2), fontFace, fontScale, colors[code], fontThickness, fontLineType);
  }
  //Giving answer (show this->answer)
  else if(code<100)
  {
    //Make answer text fit in the image (reducing fontScale until it does)
    cv::Size textSize = cv::getTextSize(this->texts[code].c_str(), fontFace, fontScale, fontThickness, 0);
    while((textSize.width > this->images[0].cols - 50) && (fontScale >1))
    {
      fontScale*=0.9;
      textSize = cv::getTextSize(this->texts[code].c_str(), fontFace, fontScale, fontThickness, 0);
    }
    
    //ROS_INFO("ScreenInformationAlgorithm: this->code = %s", this->answer.c_str());
    putText(this->images[0], this->answer.c_str(),        cv::Point(100, this->images[0].rows/2), fontFace, fontScale, colors[0], fontThickness, fontLineType);
  }
  //Giving location answer (show caption and locations)
  else
  {
    this->drawCircle(50,50,20,colors[0]);
    putText(this->images[0], "YOUR CURRENT LOCATION", cv::Point(100,  50+20), fontFace, fontScale, colors[0], fontThickness, fontLineType);
    this->drawCircle(50,100,20,colors[1]);
    putText(this->images[0], "DESTINATION",           cv::Point(100, 100+20), fontFace, fontScale, colors[1], fontThickness, fontLineType);
  }
}

//addOwnLocation: adds orange circle to map on input x y location
void ScreenInformationAlgorithm::addOwnLocation(int x, int y)
{
  this->drawCircle(x,y,30,colors[0]);
}

//addLocationWithCode: adds green circle to map on predefined x y location depending on code
void ScreenInformationAlgorithm::addLocationWithCode(int code)
{
  int x=0,y=0;
  
  switch(code)
  {
    case  100: x =  882; y = 1035; break; //biblioteca
    case  101: x =  666; y =  678; break; //plaza caminos
    case  102: x = 1420; y =  672; break; //plaza informatica
    case  103: x =  450; y = 1113; break; //nexus
    case  104: x = 1467; y = 1485; break; //nexus2
    case  105: x = 1350; y = 1074; break; //omega
    case  106: x = 1083; y =  618; break; //optica
    case  107: x = 1050; y = 1030; break; //polideportivo
    case  108: x = 2120; y = 1400; break; //rectorado
    case  109: x = 1044; y =  678; break; //plaza telecos
    case  110: x = 2150; y =  447; break; //vertex
    case 201: x =  575; y =  825; break; //a1...6
    case 202: x =  765; y =  825; break;
    case 203: x =  954; y =  825; break;
    case 204: x = 1140; y =  825; break;
    case 205: x = 1333; y =  825; break;
    case 206: x = 1518; y =  825; break;
    case 211: x =  507; y =  666; break; //b1...6
    case 212: x =  807; y =  666; break;
    case 213: x =  921; y =  666; break;
    case 214: x = 1185; y =  666; break;
    case 215: x = 1296; y =  666; break;
    case 216: x = 1560; y =  666; break;
    case 221: x =  441; y =  560; break; //c1...6
    case 222: x =  768; y =  560; break;
    case 223: x =  957; y =  560; break;
    case 224: x = 1149; y =  560; break;
    case 225: x = 1335; y =  560; break;
    case 226: x = 1521; y =  560; break;
    case 231: x =  420; y =  429; break; //d1...6
    case 232: x =  768; y =  429; break;
    case 233: x =  957; y =  429; break;
    case 234: x = 1149; y =  429; break;
    case 235: x = 1335; y =  429; break;
    case 236: x = 1521; y =  429; break;
    default:  x =    0; y =    0; break;
  }
  
  this->drawCircle(x,y,30,colors[1]);
}

//drawCircle: draws a circle on image[0] at input x y location and with input radius and color
void ScreenInformationAlgorithm::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->images[0], cv::Point(img_x, img_y), r, color, -1);
  cv::circle(this->images[0], cv::Point(img_x, img_y), r, CV_RGB(0,0,0), 2);
  
  if( ( img_x+r > this->images[0].cols ) || ( img_y+r > this->images[0].rows ) || (img_x-r < 0 ) || (img_y-r < 0) )
    ROS_INFO("Circle out of bounds");
}

// ScreenInformationAlgorithm Public API

//iteration::performs an iteration
void ScreenInformationAlgorithm::iteration()
{
  if(this->lastCode != this->code)
  {
    int i=getImageIndex(this->code); //1 = blank, 2=blank2, 3=map
    this->images[0] = this->images[i].clone(); //0 = image to modify and display
    this->addText(i,this->code);
    if(i==3) //map
    {
     //orange location (arbitrary chosen)
     this->addOwnLocation(ownX, ownY);
     //green location depending on code
     this->addLocationWithCode(this->code); 
    }
    //display image
    this->showImage(WINDOW, this->images[0]);
  }  
}

void ScreenInformationAlgorithm::setCode(int newCode)
{
  this->lastCode = this->code;
  this->code=newCode;
  //ROS_INFO("ScreenInformationAlgorithm: this->code set to = %i", this->code);
}

void ScreenInformationAlgorithm::setAnswer(std::string newAnswer)
{
  this->answer=newAnswer;
}