laser_people_label_alg_node.cpp

Go to the documentation of this file.

#include "laser_people_label_alg_node.h"

LaserPeopleLabelAlgNode::LaserPeopleLabelAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<LaserPeopleLabelAlgorithm>()
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  this->areas_array_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("areas_array", 100);
  this->buttons_array_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("buttons_array", 100);
  this->scanLabel_publisher_ = this->public_node_handle_.advertise<sensor_msgs::LaserScan>("scanLabel", 100);

  // [init subscribers]
  this->goal_subscriber_ = this->public_node_handle_.subscribe("goal", 100, &LaserPeopleLabelAlgNode::goal_callback, this);

  // [init services]

  // [init clients]

  // [init action servers]

  // [init action clients]

  //define a private node handle if it does not exist already
  ros::NodeHandle private_node_handle("~");
  private_node_handle.param<std::string>("scanTopic", scanTopic, "scan");
  private_node_handle.param<std::string>("filePath", filePath, "~/iri-lab/ros/iri-ros-pkg/iri_perception/iri_laser_people_label/data/");

  debug=false;
  initialize();

}

LaserPeopleLabelAlgNode::~LaserPeopleLabelAlgNode(void)
{
  // [free dynamic memory]
}

void LaserPeopleLabelAlgNode::mainNodeThread(void)
{
  switch(mode)
  {
    case ending:
      alg_.myLaserPeopleLabel->saveLabelsFile(append);
      cout << "----------------------------------------------" << endl;
      cout << "               CTRL+C to exit                 " << endl;
      cout << "----------------------------------------------" << endl;
      ros::shutdown();
      break;

    case playing:
      nextScan();
      break;

    case standby:

      break;

    default:

      break;
  }

  // [fill msg structures]
  //this->MarkerArrayAreas_msg_.data = my_var;
  this->alg_.lock();
  this->LaserScan_msg_ = currentScan;
  this->LaserScan_msg_.intensities = intensities;
  this->alg_.unlock();

  // [fill srv structure and make request to the server]

  // [fill action structure and make request to the action server]

  // [publish messages]
  //this->areas_array_publisher_.publish(this->MarkerArrayAreas_msg_);

  this->scanLabel_publisher_.publish(this->LaserScan_msg_);

  fillButtons(this->MarkerArrayButtons_msg_);
  this->buttons_array_publisher_.publish(this->MarkerArrayButtons_msg_);
}

/*  [subscriber callbacks] */
void LaserPeopleLabelAlgNode::goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
  //ROS_INFO("LaserPeopleLabelAlgNode::goal_callback: New Message Received");

  this->goal_mutex_.enter();

  this->currentGoal.clear();
  this->currentGoal.push_back(msg->pose.position.x);
  this->currentGoal.push_back(msg->pose.position.y);

  getButton(currentGoal);
  doButton();
  this->goal_mutex_.exit();

}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

/* [class functions] */

// Inits startup variables, loads files and performs the first iteration
void LaserPeopleLabelAlgNode::initialize()
{
  if(debug)
    cout << "LaserPeopleLabelAlgNode::initialize()" << endl;
  //initialize variables
  currentIteration = 0;
  currentBagScan   = 0;
  skipScan         = false;
  modeArea         = 0;

  // defines paths to files and scan topic (to read in bagFile)
  //filePath = "/home/fherrero/iri-stuff/my-ros/iri_laser_people_label/data/";
  //filePath = "$(find iri_laser_people_label)/data/";
  alg_.myLaserPeopleLabel->setFilePath(filePath);
  bagFilePath = filePath + "bagFile.bag";
  labelsFilePath = filePath + "labels.txt";
  //scanTopic = "/tibi/front_laser/scan";
  //scanTopic = "scan";



  // checks if bagFile exists
  if(!doFileExist(bagFilePath))
  {
    cout << "--- Error: missing "<< bagFilePath << endl;
    exit(1);
  }

  // loads scans from bagFile
  loadBag();

  // checks if labels already exists, and asks to append data or not
  append = false;
  if(doFileExist(labelsFilePath))
  {
    while(1)
    {
      cout << "--- Do you want to APPEND information to existing data? [y/n]: ";
      string input;
      getline( cin, input );
      if(input=="y" || input=="Y" || input=="yes" || input=="YES" || input=="Yes")
      {
        append=1;
        string pointsFile = alg_.myLaserPeopleLabel->pointsFile;
        alg_.myLaserPeopleLabel->getNumberOfScans(pointsFile); //Get number of scans in data (needed for appending labels)
        cout << "--- Information will be added to existing data ("<<alg_.myLaserPeopleLabel->lastIter<<" iterations)" << endl;
        break;
      }
      else if(input=="n" || input=="N" || input=="no" || input=="NO" || input=="No"){
        append=0;
        if ( !remove(alg_.myLaserPeopleLabel->pointsFile.c_str()) )
        {
          cout<<"--- Deleted previous file: " << alg_.myLaserPeopleLabel->pointsFile << endl;
        }
        else
        {
          cout<<"--- Can't delete file: " << alg_.myLaserPeopleLabel->pointsFile << endl;
        }
        cout << "--- Existing files will be overwritten with new data" << endl;
        break;
      }
      else {
        cout << "--- Unrecognized answer. Try again" << endl;
      }
    }
  }

  // first iteration
  nextScan();
}

// Loads a bag into a vector of scans (std::vector<sensor_msgs::LaserScan> bagScans )
void LaserPeopleLabelAlgNode::loadBag(void)
{
  if(debug)
    cout << "LaserPeopleLabelAlgNode::loadBag()" << endl;
  cout << "--- Loading '" << scanTopic << "' topic from '"<< bagFilePath << "' file." << endl;
  bagScans.clear();
  rosbag::Bag bag(bagFilePath.c_str());
  rosbag::View view(bag, rosbag::TopicQuery(scanTopic));
  for(iter = view.begin(); iter != view.end(); ++iter)
  {
    rosbag::MessageInstance m = *iter;
    sensor_msgs::LaserScan::ConstPtr msgScan = m.instantiate<sensor_msgs::LaserScan>();
    this->alg_.lock();
    this->currentScan=*msgScan;
    this->currentScan.header.frame_id = "/laser";
    bagScans.push_back(currentScan);
    this->alg_.unlock();
  }
  bag.close();
  cout << "--- Loaded " << bagScans.size() << " scans." << endl;
  if(bagScans.size()==0)
  {
    cout << "--- Modify scanTopic param in launch according to bagFile in use" << endl;
    exit(1);
  }
}

// Returns if specified file exists
bool LaserPeopleLabelAlgNode::doFileExist(string filePath)
{
  ifstream ifile(filePath.c_str());
  if(ifile)
    cout << "--- Check: existing file " << filePath << endl;
  return ifile;
}

// Sets button_type value according to received goal
void LaserPeopleLabelAlgNode::getButton(vector<float> goal)
{
  float x = goal[0];
  float y = goal[1];
  float x1   = -2;
  float x2   = -3;
  float x3   = -4;
  float x4   = -5;

  if(modeArea!=0)
  {
    setArea();
    button_type = idle;
  }
  else
  {
    button_type = segment;
    if(x<x1 && x>x2)
    {
      if(y<2 && y>1)
        button_type = next;
      else if(y<1 && y>0)
        button_type = go;
      else if(y<0 && y>-1)
        button_type = skip;
      else if(y<-1 && y>-2)
        button_type = end;
    }
    else if(x<x2 && x>x3)
    {
      if(y<2 && y>1)
        button_type = Apos;
      else if(y<1 && y>0)
        button_type = Aneg;
      else if(y<0 && y>-1)
        button_type = Areset;
      //else if(y<-1 && y>-2)
        //button_type = empty;
    }
    else if(x<x3 && x>x4)
    {
      if(y<2 && y>1)
        button_type = play;
      else if(y<1 && y>0)
        button_type = stop;
      //else if(y<0 && y>-1)
        //button_type = empty;
      //else if(y<-1 && y>-2)
        //button_type = empty;
    }
    else
    {
      button_type = segment;
    }
  }

}

// Performs the actions associated to the pressed button
void LaserPeopleLabelAlgNode::doButton()
{
  skipScan = false;
  switch(button_type)
  {
    case next:
      nextScan();
      break;

    case go:
      skipScan=true;
      alg_.myLaserPeopleLabel->removeLabels(currentIteration);
      cout<<"--- Skipping scan "<< currentBagScan <<endl;
      goToScan();
      nextScan();
      break;

    case skip:
      skipScan=true;
      alg_.myLaserPeopleLabel->removeLabels(currentIteration);
      cout<<"--- Skipping scan "<< currentBagScan <<endl;
      nextScan();
      break;

    case end:
      mode = ending;
      break;

    case Apos:
      cout<<"--- Started adding area"<< endl;
      modeArea=1;
      areaVertex=0;
      break;

    case Aneg:
      cout<<"--- Started substracting area"<< endl;
      modeArea=-1;
      areaVertex=0;
      break;

    case Areset:
      cout<<"--- Reseting areas "<< endl;
      areaListPos.clear();
      areaListNeg.clear();
      fillAreas(this->MarkerArrayAreas_msg_);
      break;
    case play:
      mode = playing;
      break;

    case stop:
      mode = standby;
      break;

    case segment:
      alg_.myLaserPeopleLabel->labelSegment(currentGoal, currentIteration);
      intensities = alg_.myLaserPeopleLabel->getIntensities();
      break;

    case idle:
      //nothing
      break;

    default:
      //nothing
      break;

  }
}

// Loads the next scan of the bagFile (...)
void LaserPeopleLabelAlgNode::nextScan()
{
  if(debug)
    cout << "LaserPeopleLabelAlgNode::nextScan()" << endl;

  if(areaListPos.size()!=0 || areaListNeg.size()!=0)
  {
    cout<<"--- Labeling with areas" << endl;
    labelAreas();
  }

  // If reached end of bag, end labeling
  if(unsigned(currentBagScan+1)>bagScans.size())
  {
    mode=ending;
    return;
  }

  this->alg_.lock();
  //On start, save scan parameters on file
  if(currentIteration==0)
  {
    alg_.myLaserPeopleLabel->saveParamsFile(currentScan.angle_min, currentScan.angle_max, currentScan.angle_increment);
    cout << "--- Saved params.txt"<<endl;
  }

  //Save scan if no skipScan is set
  if(!skipScan && currentIteration!=0)
  {
    alg_.myLaserPeopleLabel->savePointsFile(currentScan.ranges, append);
  }

  // Next iteration
  if(!skipScan)
    currentIteration++;
  currentBagScan++;

  // Load next scan in currentScan

  currentScan = bagScans[currentBagScan-1];


  // labelIteration (ranges -> points -> segments)
  alg_.myLaserPeopleLabel->labelIteration(currentScan.ranges, currentScan.angle_min, currentScan.angle_max, currentScan.angle_increment);
  intensities = alg_.myLaserPeopleLabel->getIntensities();

  cout<<"--- Iter: "<< currentIteration+alg_.myLaserPeopleLabel->lastIter << "   Scan: " << currentBagScan <<"/"<< bagScans.size() << endl;
  this->alg_.unlock();

}

// GoToScan button, asks new scan input to jump to
void LaserPeopleLabelAlgNode::goToScan()
{
  unsigned int scan;
  while(1){
    cout << "--- Go to scan number: ";
    string input3;
    getline(cin,input3);
    istringstream iss( input3 );
    if( ( iss >> scan) && scan>0 && scan<=bagScans.size() ){
      iss>>scan;
      break;
    }
    else if( scan<=0 || scan>bagScans.size() ){
      cout<<"--- Scan must be between 1 and " << bagScans.size() << endl;
    }
    else if( !(iss>>scan) ) {
      cout<<"--- It must be an integer!"<<endl;
    }
  }
  currentBagScan=scan-1;
}

void LaserPeopleLabelAlgNode::setArea()
{
  cout << "--- --- modeArea: click to add vertex" << endl;
  area.push_back(currentGoal[0]);
  area.push_back(currentGoal[1]);
  areaVertex++;
  cout<<"--- --- Added area vertex "<< areaVertex <<"/2"<< endl;

  if(areaVertex==2)
  {
    if(modeArea==1)
      areaListPos.push_back(area);
    if(modeArea==-1)
      areaListNeg.push_back(area);
    modeArea=0;
    area.clear();
    cout << "--- Area finished" << endl;
    fillAreas(this->MarkerArrayAreas_msg_);
  }

}

void LaserPeopleLabelAlgNode::labelAreas()
{
  for(unsigned int i=0; i<areaListPos.size(); i++)
  {
    cout << "--- --- Using positive area " << i+1 << "/"<<areaListPos.size()<< endl;
    float xmax = max( areaListPos[i][0],areaListPos[i][2] );
    float xmin = min( areaListPos[i][0],areaListPos[i][2] );
    float ymax = max( areaListPos[i][1],areaListPos[i][3] );
    float ymin = min( areaListPos[i][1],areaListPos[i][3] );
    alg_.myLaserPeopleLabel->labelArea(xmax,xmin, ymax, ymin, 1.0, currentIteration);
  }

  for(unsigned int i=0; i<areaListNeg.size(); i++)
  {
    cout << "--- --- Using negative area " << i+1 << "/"<<areaListNeg.size()<< endl;
    float xmax = max( areaListNeg[i][0],areaListNeg[i][2] );
    float xmin = min( areaListNeg[i][0],areaListNeg[i][2] );
    float ymax = max( areaListNeg[i][1],areaListNeg[i][3] );
    float ymin = min( areaListNeg[i][1],areaListNeg[i][3] );
    alg_.myLaserPeopleLabel->labelArea(xmax, xmin, ymax, ymin, -1.0, currentIteration );
  }
}

// Defines the buttons (text and boxes) to show on rviz
void LaserPeopleLabelAlgNode::fillButtons(visualization_msgs::MarkerArray & buttons)
{
  string buttonText[] = {"Next",   "Go", "Skip",  "End",  "A+",  "A-",  "A0", "Play", "Stop"};
  float buttonR[]     = {    0 ,     0 ,     1 ,     1 ,    1 ,  0.4 ,    1 ,     0 ,     1 };
  float buttonG[]     = {    1 ,     1 ,     1 ,     0 ,    0 ,    1 ,  0.4 ,     1 ,     0 };
  float buttonB[]     = {    0 ,     1 ,     0 ,     0 ,    1 ,    0 ,    0 ,     0 ,     0 };
  float buttonX[]     = { -2.5 ,  -2.5 ,  -2.5 ,  -2.5 , -3.5 , -3.5 , -3.5 ,  -4.5 ,  -4.5 };
  float buttonY[]     = {  1.5 ,   0.5 ,  -0.5 ,  -1.5 ,  1.5 ,  0.5 , -0.5 ,   1.5 ,   0.5 };

  int buttonsNumber = sizeof(buttonR) / sizeof(float);
  buttons.markers.resize(2*buttonsNumber);

  for(int i=0; i<buttonsNumber; i++)
  {
    buttons.markers[i].header.frame_id = currentScan.header.frame_id;
    buttons.markers[i].id     = i;
    buttons.markers[i].type   = visualization_msgs::Marker::TEXT_VIEW_FACING;
    buttons.markers[i].action = visualization_msgs::Marker::ADD;

    buttons.markers[i].pose.position.x = buttonX[i];
    buttons.markers[i].pose.position.y = buttonY[i];
    buttons.markers[i].pose.position.z = 0;

    buttons.markers[i].scale.z = 0.5;

    buttons.markers[i].text= buttonText[i];
    buttons.markers[i].color.r = buttonR[i];
    buttons.markers[i].color.g = buttonG[i];
    buttons.markers[i].color.b = buttonB[i];
    buttons.markers[i].color.a = 1;
  }

  int j=0;
  for(int i=buttonsNumber; i<2*buttonsNumber; i++)
  {
    buttons.markers[i].header.frame_id = currentScan.header.frame_id;
    buttons.markers[i].id = i;
    buttons.markers[i].type = visualization_msgs::Marker::CUBE;
    buttons.markers[i].action = visualization_msgs::Marker::ADD;

    buttons.markers[i].pose.position.x = buttonX[j];
    buttons.markers[i].pose.position.y = buttonY[j];
    buttons.markers[i].pose.position.z = 0;

    buttons.markers[i].scale.x = 1;
    buttons.markers[i].scale.y = 1;
    buttons.markers[i].scale.z = 0.1;

    buttons.markers[i].color.r = buttonR[j];
    buttons.markers[i].color.g = buttonG[j];
    buttons.markers[i].color.b = buttonB[j];
    buttons.markers[i].color.a = 0.25;
    j++;
  }

}

void LaserPeopleLabelAlgNode::fillAreas(visualization_msgs::MarkerArray & areas)
{
  for(unsigned int i=0; i<areas.markers.size(); i++)
  {
    areas.markers[i].action = visualization_msgs::Marker::DELETE;
  }

  this->areas_array_publisher_.publish(this->MarkerArrayAreas_msg_);

  areas.markers.resize(areaListPos.size() + areaListNeg.size());
  //cout << "--- --- Drawing areas (+): " << areaListPos.size() << endl;

  for(unsigned int i=0; i<areaListPos.size(); i++)
  {
    areas.markers[i].header.frame_id = currentScan.header.frame_id;
    areas.markers[i].id = i;
    areas.markers[i].type = visualization_msgs::Marker::CUBE;
    areas.markers[i].action = visualization_msgs::Marker::ADD;

    areas.markers[i].pose.position.x = (areaListPos[i][0]+areaListPos[i][2])/2.0;
    areas.markers[i].pose.position.y = (areaListPos[i][1]+areaListPos[i][3])/2.0;
    areas.markers[i].pose.position.z = 0;

    areas.markers[i].scale.x = abs(areaListPos[i][0]-areaListPos[i][2]);
    areas.markers[i].scale.y = abs(areaListPos[i][1]-areaListPos[i][3]);
    areas.markers[i].scale.z = 0.1;

    //areas.markers[i].text=" ";
    areas.markers[i].color.r = 1.0;
    areas.markers[i].color.g = 0.0;
    areas.markers[i].color.b = 0.0;
    areas.markers[i].color.a = 0.25;
  }

  //cout << "--- --- Drawing areas (-): " << areaListNeg.size() << endl;

  for(unsigned int i=areaListPos.size(); i<(areaListPos.size()+areaListNeg.size()); i++)
  {
    int t=i-areaListPos.size();
    areas.markers[i].header.frame_id = currentScan.header.frame_id;
    areas.markers[i].id = i;
    areas.markers[i].type = visualization_msgs::Marker::CUBE;
    areas.markers[i].action = visualization_msgs::Marker::ADD;

    areas.markers[i].pose.position.x = (areaListNeg[t][0]+areaListNeg[t][2])/2.0;
    areas.markers[i].pose.position.y = (areaListNeg[t][1]+areaListNeg[t][3])/2.0;
    areas.markers[i].pose.position.z = 0;

    areas.markers[i].scale.x = abs(areaListNeg[t][0]-areaListNeg[t][2]);
    areas.markers[i].scale.y = abs(areaListNeg[t][1]-areaListNeg[t][3]);
    areas.markers[i].scale.z = 0.1;

    //areas.markers[i].text=" ";
    areas.markers[i].color.r = 0.5;
    areas.markers[i].color.g = 0.5;
    areas.markers[i].color.b = 0.5;
    areas.markers[i].color.a = 0.25;
  }

  this->areas_array_publisher_.publish(this->MarkerArrayAreas_msg_);
}

void LaserPeopleLabelAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();

  this->alg_.unlock();
}

void LaserPeopleLabelAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<LaserPeopleLabelAlgNode>(argc, argv, "laser_people_label_alg_node");
}