iri_score_map_mix_node.cpp

Go to the documentation of this file.

#include "iri_score_map_mix_node.h"

ScoreMapMixAlgNode::ScoreMapMixAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<ScoreMapMixAlgorithm>()
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]
    num_score_maps_ = 0;
    selection_algorithm_id_ = 0;
    weights_.resize(MAXMAPS);
    for(int i=0;i<MAXMAPS;i++)
      weights_[i] = 0;

  // [init publishers]
  this->score_results_publisher_ = this->public_node_handle_.advertise<iri_deformable_analysis::ScoreMapMixResults>("deformable_score_results", 10);
  /* Next 3 are just for logging and visualization purposes */
  this->grasping_point_pose_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("grasping_point_pose", 5);
  this->algorithm_id_publisher_ = this->public_node_handle_.advertise<std_msgs::UInt32>("score_mix_algorithm_id", 5);
  this->score_map_global_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("score_map_global", 10);


  // [init subscribers]
  this->score_map7_subscriber_ = this->public_node_handle_.subscribe("score_map7", 10, &ScoreMapMixAlgNode::score_map7_callback, this);
  this->score_map6_subscriber_ = this->public_node_handle_.subscribe("score_map6", 10, &ScoreMapMixAlgNode::score_map6_callback, this);
  this->score_map5_subscriber_ = this->public_node_handle_.subscribe("score_map5", 10, &ScoreMapMixAlgNode::score_map5_callback, this);
  this->score_map4_subscriber_ = this->public_node_handle_.subscribe("score_map4", 10, &ScoreMapMixAlgNode::score_map4_callback, this);
  this->score_map3_subscriber_ = this->public_node_handle_.subscribe("score_map3", 10, &ScoreMapMixAlgNode::score_map3_callback, this);
  this->score_map2_subscriber_ = this->public_node_handle_.subscribe("score_map2", 10, &ScoreMapMixAlgNode::score_map2_callback, this);
  this->score_map1_subscriber_ = this->public_node_handle_.subscribe("score_map1", 10, &ScoreMapMixAlgNode::score_map1_callback, this);
  this->score_map0_subscriber_ = this->public_node_handle_.subscribe("score_map0", 10, &ScoreMapMixAlgNode::score_map0_callback, this);
  
  // [init services]
  this->algorithm_id_server_ = this->public_node_handle_.advertiseService("set_score_mix_algorithm_id", &ScoreMapMixAlgNode::algorithm_idCallback, this);
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]

    ROS_INFO("Starting score_map_mix_node with %d num_score_maps_ and algorithm %d", num_score_maps_, selection_algorithm_id_);
}

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

void ScoreMapMixAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->PointCloud2_msg_.data = my_var;
  //this->PoseStamped_msg_.data = my_var;
  alg_.lock(); 
  UInt32_msg_.data = selection_algorithm_id_;
  //this->grasp_point_description_msg_.data = my_var;
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
  //score_map_global_publisher_.publish(score_map_global_msg_);
  grasping_point_pose_publisher_.publish(best_scored_pose_);
  algorithm_id_publisher_.publish(UInt32_msg_);
  alg_.unlock(); 

        
}

/*  [subscriber callbacks] */
void ScoreMapMixAlgNode::score_map7_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{ 
  ROS_DEBUG("ScoreMapMixAlgNode::score_map7_callback: New Message Received"); 

  if(num_score_maps_ <7){
    ROS_WARN("Received pointcloud 7 while I was expecting a max of %d score maps", num_score_maps_);
    return;
  }

  //use appropiate mutex to shared variables if necessary 
  this->alg_.lock(); 
  //this->score_map7_mutex_.enter(); 

  //std::cout << msg->data << std::endl; 
  pcl::fromROSMsg(*msg, input_score_maps_[7]);

  score_map_ready_[7] = true;

  if(all_clouds_ready())
    computeGraspPoints();
  
  //unlock previously blocked shared variables 
  this->alg_.unlock(); 
  //this->score_map7_mutex_.exit(); 
}

void ScoreMapMixAlgNode::score_map6_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{ 
  ROS_DEBUG("ScoreMapMixAlgNode::score_map6_callback: New Message Received"); 

  if(num_score_maps_ <7){
    ROS_WARN("Received pointcloud 6 but expecting a max of %d score maps", num_score_maps_);
    return;
  }

  this->alg_.lock(); 

  pcl::fromROSMsg(*msg, input_score_maps_[6]);

  score_map_ready_[6] = true;

  if(all_clouds_ready())
    computeGraspPoints();

  this->alg_.unlock(); 
}
void ScoreMapMixAlgNode::score_map5_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{ 
  ROS_DEBUG("ScoreMapMixAlgNode::score_map5_callback: New Message Received"); 

  if(num_score_maps_ <6){
    ROS_WARN("Received pointcloud 5 but expecting a max of %d score maps", num_score_maps_);
    return;
  }

  this->alg_.lock(); 

  pcl::fromROSMsg(*msg, input_score_maps_[5]);

  score_map_ready_[5] = true;

  if(all_clouds_ready())
    computeGraspPoints();

  this->alg_.unlock(); 
}

void ScoreMapMixAlgNode::score_map4_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{ 
  ROS_DEBUG("ScoreMapMixAlgNode::score_map4_callback: New Message Received"); 

  if(num_score_maps_ <5){
    ROS_WARN("Received pointcloud 4 but expecting a max of %d score maps", num_score_maps_);
    return;
  }

  this->alg_.lock(); 

  pcl::fromROSMsg(*msg, input_score_maps_[4]);

  score_map_ready_[4] = true;

  if(all_clouds_ready())
    computeGraspPoints();

  this->alg_.unlock(); 
}

void ScoreMapMixAlgNode::score_map3_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{ 
  ROS_DEBUG("ScoreMapMixAlgNode::score_map3_callback: New Message Received"); 

  if(num_score_maps_ <4){
    ROS_WARN("Received pointcloud 3 but expecting a max of %d score maps", num_score_maps_);
    return;
  }

  this->alg_.lock(); 

  pcl::fromROSMsg(*msg, input_score_maps_[3]);

  score_map_ready_[3] = true;

  if(all_clouds_ready())
    computeGraspPoints();

  this->alg_.unlock(); 
}

void ScoreMapMixAlgNode::score_map2_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{ 
  ROS_DEBUG("ScoreMapMixAlgNode::score_map2_callback: New Message Received"); 

  if(num_score_maps_ <3){
    ROS_WARN("Received pointcloud 2 but expecting a max of %d score maps", num_score_maps_);
    return;
  }

  this->alg_.lock(); 

  pcl::fromROSMsg(*msg, input_score_maps_[2]);

  score_map_ready_[2] = true;

  if(all_clouds_ready())
    computeGraspPoints();

  this->alg_.unlock(); 
}

void ScoreMapMixAlgNode::score_map1_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{ 
  ROS_DEBUG("ScoreMapMixAlgNode::score_map1_callback: New Message Received"); 
  this->alg_.lock(); 

  if(num_score_maps_ <2){
    ROS_WARN("Received pointcloud 1 but expecting a max of %d score maps", num_score_maps_);
    return;
  }

  pcl::fromROSMsg(*msg, input_score_maps_[1]);

  score_map_ready_[1] = true;

  if(all_clouds_ready())
    computeGraspPoints();

  this->alg_.unlock(); 
}

void ScoreMapMixAlgNode::score_map0_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{ 
  ROS_DEBUG("ScoreMapMixAlgNode::score_map0_callback: New Message Received"); 

  if(num_score_maps_ <1){
    ROS_WARN("Received pointcloud 0 but expecting a max of %d score maps", num_score_maps_);
    return;
  }
  this->alg_.lock(); 

  pcl::fromROSMsg(*msg, input_score_maps_[0]);

  score_map_ready_[0] = true;

  if(all_clouds_ready())
    computeGraspPoints();

  this->alg_.unlock(); 
}

/*  [service callbacks] */
bool 
ScoreMapMixAlgNode::algorithm_idCallback(iri_deformable_analysis::SetScoreMixAlgorithmID::Request &req, 
                                         iri_deformable_analysis::SetScoreMixAlgorithmID::Response &res)
{ 
  ROS_INFO("ScoreMapMixAlgNode::algorithm_idCallback: New Request Received!"); 

  //use appropiate mutex to shared variables if necessary 
  this->alg_.lock(); 
  //this->algorithm_id_mutex_.enter(); 

  selection_algorithm_id_ = req.algorithm_id;
  algorithmConfigurationTranslation(req.algorithm_id);

  res.success = true;
  //set the parameter server to globally know which algorithm we are using
  //public_node_handle_.setParam(public_node_hanle_.getNamespace() + "/info/algorithm_id", selection_algorithm_id_);

  //if(this->alg_.isRunning()) 
  //{ 
    //ROS_INFO("ScoreMapMixAlgNode::algorithm_idCallback: Processin New Request!"); 
    //do operations with req and output on res 
    //res.data2 = req.data1 + my_var; 
  //} 
  //else 
  //{ 
    //ROS_INFO("ScoreMapMixAlgNode::algorithm_idCallback: ERROR: alg is not on run mode yet."); 
  //} 

  //unlock previously blocked shared variables 
  this->alg_.unlock(); 
  //this->algorithm_id_mutex_.exit(); 

  return true; 
}

/*  [action callbacks] */

/*  [action requests] */

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

  ROS_INFO("Reconfiguring");
  num_score_maps_ = config.num_score_maps;
  if(selection_algorithm_id_ != config.selection_algorithm_id){
      algorithmConfigurationTranslation(config.selection_algorithm_id);
      selection_algorithm_id_ = config.selection_algorithm_id;
  }

  weights_[0] = config.weight_0; 
  weights_[1] = config.weight_1; 
  weights_[2] = config.weight_2; 
  weights_[3] = config.weight_3; 
  weights_[4] = config.weight_4; 
  weights_[5] = config.weight_5; 
  weights_[6] = config.weight_6; 
  weights_[7] = config.weight_7; 

  ROS_INFO("Reconfiguring map mix node with %d score maps and algorithm %d", num_score_maps_, selection_algorithm_id_);

    input_score_maps_.resize(num_score_maps_);
    score_map_ready_.resize(num_score_maps_);
    for(int i = 0; i < num_score_maps_; i++)
        score_map_ready_[i] = false;

  this->alg_.unlock();
}

void ScoreMapMixAlgNode::addNodeDiagnostics(void)
{
}

bool ScoreMapMixAlgNode::all_clouds_ready(){
    //function protected by mutex

    if(num_score_maps_ == 0)
        return false;

    for(int i=0; i<num_score_maps_; i++){
        if(!score_map_ready_[i]){
            ROS_WARN("%d clouds out of %d ready, waiting",i,num_score_maps_);
            return false;
        }
    }

    //reset the new data vector
    for(int i=0; i<num_score_maps_; i++)
        score_map_ready_[i] = false;

    ROS_INFO("all maps ready");
    return true;

}

void ScoreMapMixAlgNode::algorithmConfigurationTranslation(int alg_id){
    int i;
    for(i=0; i<num_score_maps_;i++)
        weights_[i] = 0;
    switch(alg_id){
        case FULL0:
            weights_[0] = 1;
            break;
        case FULL1:
            weights_[1] = 1;
            break;
        case FIFTY01:
            weights_[0] = 0.5;
            weights_[1] = 0.5;
            break;
        default:
            weights_[0] = 1;
            ROS_WARN("Algorithm id undefined");
    }

}

void ScoreMapMixAlgNode::computeScoreMap(){

    //the only thing not required to copy is the rgb!
    copyPointCloud(input_score_maps_[0], output_score_map_);

    pcl::PointCloud<pcl::PointXYZRGB>::iterator pt_iter_xyzrgb = output_score_map_.begin();

    pcl::PointXYZRGB point; 
    //TODO check pointcloud sizes and structuredness 
    for (unsigned int rr = 0; rr < output_score_map_.height; ++rr) {
      for (unsigned int cc = 0; cc < output_score_map_.width; ++cc, ++pt_iter_xyzrgb) {
        pt_iter_xyzrgb->rgb = 0; 
        for(unsigned int i = 0; i < input_score_maps_.size(); i++) {
            if(output_score_map_.height != input_score_maps_[i].height
               || output_score_map_.width != input_score_maps_[i].width){
                ROS_ERROR("score map size mismatch. Expected %d %d received %d %d",output_score_map_.height, output_score_map_.width, input_score_maps_[i].height, input_score_maps_[i].width);
                break;
            }
 
            point = input_score_maps_[i].at(cc,rr);
            if(!(isnan(point.x) || isnan(point.y) || isnan(point.z))) {
                //TODO is it possible to perform this kind of operation without going point by point?
                pt_iter_xyzrgb->rgb += weights_[i]*point.rgb; 
            } else {
                //TODO do something else to know the point does not have xyz coordinates
                pt_iter_xyzrgb->rgb = 0; 
            }
        }
      }
    } 
}

void ScoreMapMixAlgNode::getMax(pcl::PointCloud<pcl::PointXYZRGB> score_map, pcl::PointXYZRGB& grasp_point, int& pcl_row, int& pcl_col, float& max_score, float& min_score){
    
    pcl::PointCloud<pcl::PointXYZRGB>::iterator pt_iter_xyzrgb = score_map.begin();

    max_score = -std::numeric_limits<float>::infinity();
    min_score = std::numeric_limits<float>::infinity();
    pcl_row = -1;
    pcl_col = -1;
    for (unsigned int rr = 0; rr < score_map.height; ++rr) {
      for (unsigned int cc = 0; cc < score_map.width; ++cc, ++pt_iter_xyzrgb) {
        if(!(isnan(pt_iter_xyzrgb->x) && isnan(pt_iter_xyzrgb->y) && isnan(pt_iter_xyzrgb->z))) {
           if(max_score < pt_iter_xyzrgb->rgb){ 
               max_score = pt_iter_xyzrgb->rgb; 
               grasp_point.x = pt_iter_xyzrgb->x;
               grasp_point.y = pt_iter_xyzrgb->y;
               grasp_point.z = pt_iter_xyzrgb->z;
               grasp_point.rgb = pt_iter_xyzrgb->rgb;
               pcl_row = rr;
               pcl_col = cc;
//               ROS_INFO("Max at %d %d at position %f %f %f with score %f",pcl_row, pcl_col, grasp_point.x, grasp_point.y, grasp_point.z, grasp_point.rgb);
           }
           if(min_score > pt_iter_xyzrgb->rgb){ 
               min_score = pt_iter_xyzrgb->rgb; 
           }
        }
      }
    } 
}

void ScoreMapMixAlgNode::computeGraspPoints(){

    int pcl_row = -1, pcl_col = -1;
    float max_score, min_score;
    pcl::PointXYZRGB point;
    ROS_INFO("Normalize and compute global score map");
    for(int i=0;i<num_score_maps_;i++){
        normalizeScoreMap(input_score_maps_[i]);
    }
    computeScoreMap();
    ROS_INFO("Fetching max");
    getMax(output_score_map_, point, pcl_row, pcl_col, max_score, min_score);
    normalizeScoreMap(output_score_map_, max_score, min_score);

    pcl::toROSMsg(output_score_map_, score_map_global_msg_);
    score_map_global_publisher_.publish(score_map_global_msg_);

    if(pcl_row == -1 || pcl_col == -1){
        ROS_ERROR("Any suitable point has been found! Aborting.");
        //TODO deal with errors
        return;
    }

    ROS_INFO("Max at %d %d at position %f %f %f with score %f",pcl_row, pcl_col, point.x, point.y, point.z, point.rgb);

    // Build the pose: best_score_pose_ class member si use for visualization
    // purposes so it is built first and added to the returning messages.
    best_scored_pose_.header = output_score_map_.header;
    // Timestamp should be the message building time (now) so overriding
    best_scored_pose_.header.stamp = ros::Time::now();

    best_scored_pose_.pose = score_results_.best_scored_pose.pose;
    best_scored_pose_.pose.position.x = point.x;
    best_scored_pose_.pose.position.y = point.y;
    best_scored_pose_.pose.position.z = point.z;
    best_scored_pose_.pose.orientation.x = sqrt(2)/2;
    best_scored_pose_.pose.orientation.y = sqrt(2)/2;
    best_scored_pose_.pose.orientation.z = 0;
    best_scored_pose_.pose.orientation.w = 0;

    score_results_.best_scored_pose = best_scored_pose_;

    // Build the point
    score_results_.best_scored_point.x = point.x;
    score_results_.best_scored_point.y = point.y;
    score_results_.best_scored_point.z = point.z;

    score_results_publisher_.publish(score_results_);
}

void ScoreMapMixAlgNode::normalizeScoreMap(pcl::PointCloud<pcl::PointXYZRGB> score_map, double max_score, double min_score){

    pcl::PointCloud<pcl::PointXYZRGB>::iterator pt_iter_xyzrgb = score_map.begin();

    for (unsigned int rr = 0; rr < output_score_map_.height; ++rr) {
      for (unsigned int cc = 0; cc < output_score_map_.width; ++cc, ++pt_iter_xyzrgb) {
        if(!(isnan(pt_iter_xyzrgb->x) && isnan(pt_iter_xyzrgb->y) && isnan(pt_iter_xyzrgb->z))) {
            if(pt_iter_xyzrgb->rgb > max_score){
                pt_iter_xyzrgb->rgb = max_score;
            }else if(pt_iter_xyzrgb->rgb < min_score){
                pt_iter_xyzrgb->rgb = min_score;
            }else{
                pt_iter_xyzrgb->rgb = (pt_iter_xyzrgb->rgb - min_score)/(max_score-min_score); 
            }
        }
      }
    } 
}

void ScoreMapMixAlgNode::normalizeScoreMap(const pcl::PointCloud<pcl::PointXYZRGB> score_map){

    int pcl_row = -1, pcl_col = -1;
    float max_score, min_score;
    pcl::PointXYZRGB point;
    getMax(score_map, point, pcl_row, pcl_col, max_score, min_score);
    normalizeScoreMap(score_map, max_score, min_score);

}

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