camera_people_detection_alg_node.cpp

Go to the documentation of this file.

#include "camera_people_detection_alg_node.h"

CameraPeopleDetectionAlgNode::CameraPeopleDetectionAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<CameraPeopleDetectionAlgorithm>(), 
  it_(public_node_handle_),
  tf_listener_(ros::Duration(30.f)),
  new_laser_event_id_("new_laser_event"),
  new_image_event_id_("new_image_event"),
  new_tracker_event_id_("new_tracker_event"),
  width_resize_(0.3f),
  height_resize_(0.3f),
  minthr_(-0.5)
{
  //init class attributes if necessary
  loop_rate_ = 10;//in [Hz]

  // [init publishers]
  markers_publisher_         = public_node_handle_.advertise<visualization_msgs::MarkerArray>("markers", 100);
  image_detection_publisher_ = public_node_handle_.advertise<iri_perception_msgs::detectionArray>("image_detection", 100);
  debug_img_publisher_       = it_.advertise("debug_image", 1);

  // [init subscribers]
  this->tracker_subscriber_ = this->public_node_handle_.subscribe("tracker", 100, &CameraPeopleDetectionAlgNode::tracker_callback, this);
  laser_subscriber_ = public_node_handle_.subscribe("scan", 100, &CameraPeopleDetectionAlgNode::laser_callback, this);
  people_detection_subscriber_ = public_node_handle_.subscribe("people_detection", 100, &CameraPeopleDetectionAlgNode::people_detection_callback, this);
  cam_sub_ = it_.subscribeCamera("image_in", 1, &CameraPeopleDetectionAlgNode::image_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]

  event_server_ = CEventServer::instance();
  event_server_->create_event(new_laser_event_id_);
  event_server_->create_event(new_image_event_id_);
  event_server_->create_event(new_tracker_event_id_);
}

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

void CameraPeopleDetectionAlgNode::fillMarkerArray(const iri_perception_msgs::detectionArray & fusion_poses_msg,
                                                   visualization_msgs::MarkerArray & markerArray_msg)
{
  static unsigned int lastSize=0;
  markerArray_msg.markers.resize(std::max(uint(fusion_poses_msg.detection.size()), lastSize));
  lastSize = fusion_poses_msg.detection.size();

  for(unsigned int ii=0; ii<fusion_poses_msg.detection.size(); ii++)
  {
    markerArray_msg.markers[ii].header   = fusion_poses_msg.header;
    markerArray_msg.markers[ii].id       = ii;
    markerArray_msg.markers[ii].pose     = fusion_poses_msg.detection[ii].pose.pose;
    markerArray_msg.markers[ii].type     = visualization_msgs::Marker::CYLINDER;
    markerArray_msg.markers[ii].action   = visualization_msgs::Marker::ADD;
    markerArray_msg.markers[ii].scale.x  = 0.5f;
    markerArray_msg.markers[ii].scale.y  = 0.5f;
    markerArray_msg.markers[ii].scale.z  = 1.0f;
    markerArray_msg.markers[ii].color.a  = 0.75f;

    // if laser and camera match detection
    if(fusion_poses_msg.detection[ii].probability > 0)
    {
      markerArray_msg.markers[ii].color.r  = 0.0f;
      markerArray_msg.markers[ii].color.g  = 1.0f;
      markerArray_msg.markers[ii].color.b  = 0.0f;
    }
    else
    {
      // if detection is out of camera viewpoint
      if(fusion_poses_msg.detection[ii].probability == 0)
      {
        markerArray_msg.markers[ii].color.r  = 1.0f;
        markerArray_msg.markers[ii].color.g  = 1.0f;
        markerArray_msg.markers[ii].color.b  = 0.0f;
      }
      // if laser and camera match detection too close to camera
      else
      {
        markerArray_msg.markers[ii].color.r  = 0.0f;
        markerArray_msg.markers[ii].color.g  = 0.0f;
        markerArray_msg.markers[ii].color.b  = 1.0f;
      }
    }
    markerArray_msg.markers[ii].lifetime = ros::Duration(1.0f);;
  }

  //Delete extra previous markers;
  for(unsigned int i=fusion_poses_msg.detection.size(); i<markerArray_msg.markers.size(); i++)
  {
    markerArray_msg.markers[i].action = visualization_msgs::Marker::DELETE;
  }
}

void CameraPeopleDetectionAlgNode::filterLaserDetections(iri_perception_msgs::detectionArray & fusion_poses_msg,
                                                         cv_bridge::CvImage  & img_msg)
{
  image_mutex_.enter();
  people_detection_mutex_.enter();
    std::vector<detection> vLaserPeople(laser_poses_msg_.detection.size(),detection());
    //std::vector<detection> vTrackPeople(tracker_msg_.peopleSet.size(),detection());

    //resize original image size and detections
    cv::Mat img_resized;
    cv::resize(cv_ptr_->image, img_resized, cv::Size(), width_resize_, height_resize_, cv::INTER_LINEAR);
    
    // compute transform from laser frame to camera frame
    // and create detections vector
    tf::StampedTransform laser_to_camera_transf;
    try
    {
      //ros::Time acquisition_time = laser_poses_msg_.header.stamp;
      ros::Time now = ros::Time::now();
      ros::Duration timeout(1.0f);
      
          ROS_INFO("Entra_waittf");
      // wait for transform from laser to camera 
      bool tf_exists = tf_listener_.waitForTransform(cam_model_.tfFrame(), laser_poses_msg_.header.frame_id,
                                                     now, timeout);
                                                     //acquisition_time, timeout);

      if(tf_exists)
      {
        tf_listener_.lookupTransform(cam_model_.tfFrame(), laser_poses_msg_.header.frame_id,
                                     now, laser_to_camera_transf);
                                     //acquisition_time, laser_to_camera_transf);
                ROS_INFO("Entra_ldec");
        // create vector with laser detections transformed into image plane
        // vLaserPeople.resize(laser_poses_msg_.detection.size());
        for(unsigned int ii=0; ii<laser_poses_msg_.detection.size(); ii++)
        {
          //transform points from laser frame to image coordinates
          tf::Point laser_point(laser_poses_msg_.detection[ii].pose.pose.position.x,
                                laser_poses_msg_.detection[ii].pose.pose.position.y,
                                laser_poses_msg_.detection[ii].pose.pose.position.z);
          tf::Point cam_point(laser_to_camera_transf * laser_point);
          cv::Point3d cv_cam_point(cam_point.x(), cam_point.y(), cam_point.z());
          cv::Point2d image_point = cam_model_.project3dToPixel(cv_cam_point);
          
          // fill detections struct
          float x     = laser_poses_msg_.detection[ii].pose.pose.position.x;
          float y     = laser_poses_msg_.detection[ii].pose.pose.position.y;
          float depth = sqrt(x*x + y*y);

          vLaserPeople[ii].id    = ii;
          vLaserPeople[ii].im_x  = floor(image_point.x * width_resize_);
          vLaserPeople[ii].im_y  = floor(image_point.y * height_resize_);
          vLaserPeople[ii].depth = depth;
          vLaserPeople[ii].score = laser_poses_msg_.detection[ii].probability;
        }
                ROS_INFO("Sale_ldec");

        /*tracker_mutex_.enter();
                ROS_INFO("Entra_tdec"); 
                ROS_WARN("tracker_msg_.peopleSet.size=%d ",tracker_msg_.peopleSet.size());
        for(unsigned int ii=0; ii<tracker_msg_.peopleSet.size(); ii++)
        {
          tf::Point tracker_point(tracker_msg_.peopleSet[ii].x,
                                  tracker_msg_.peopleSet[ii].y,
                                  0.f);
          tf::Point cam_point(laser_to_camera_transf * tracker_point);
          cv::Point3d cv_cam_point(cam_point.x(), cam_point.y(), cam_point.z());
          cv::Point2d image_point = cam_model_.project3dToPixel(cv_cam_point);
          
          // fill detections struct
          float x     = tracker_msg_.peopleSet[ii].x;
          float y     = tracker_msg_.peopleSet[ii].y;
          float depth = sqrt(x*x + y*y);

          vTrackPeople[ii].id    = ii;
          vTrackPeople[ii].im_x  = floor(image_point.x * width_resize_);
          vTrackPeople[ii].im_y  = floor(image_point.y * height_resize_);
          vTrackPeople[ii].depth = depth;
          vTrackPeople[ii].score = tracker_msg_.peopleSet[ii].probability;
          ROS_WARN("vTrackPeople[%d].im_x=%d vTrackPeople[ii].im_y=%d",ii,vTrackPeople[ii].im_x,vTrackPeople[ii].im_y);
        }
                ROS_INFO("Sale_tdec");
        tracker_mutex_.exit();*/      
      }
      else
      {
          ROS_INFO("CameraPeopleDetectionAlgNode::No transform: %s-->%s", cam_model_.tfFrame().c_str(), laser_poses_msg_.header.frame_id.c_str());
      }
          ROS_INFO("Sale_waittf");
    }
    catch (tf::TransformException& ex)
    {
      ROS_WARN("TF exception:\n%s", ex.what());
    }

    // filter laser detections
    std::vector<detection> vOutputPeople;
    std::vector<cv::Rect> found;
    std::vector<double> foundWeights;    

    // execute camera people detection algorithm
        ROS_INFO("Entra_val_detec");
    cam_detector_.validate_detections(img_resized, vLaserPeople, vOutputPeople, found, foundWeights, minthr_);
    //cam_detector_.validate_detections(img_resized, vLaserPeople, vTrackPeople, vOutputPeople, found, foundWeights, minthr_);
        ROS_INFO("Sale_val_detec");

    // debug output image
    cv::Mat outIm;

    // draw all laser points on image
    laser_mutex_.enter();
      std::vector<detection> vAllScans(scan_.ranges.size(),detection());
      for(unsigned int ii=0; ii<scan_.ranges.size(); ii++)
      {
        float scan_x = scan_.ranges[ii]*cos(scan_.angle_min+scan_.angle_increment*ii);
        float scan_y = scan_.ranges[ii]*sin(scan_.angle_min+scan_.angle_increment*ii);
        float scan_z = 0.f;

        //transform points from laser frame to image coordinates
        tf::Point laser_point(scan_x, scan_y, scan_z);
        tf::Point cam_point(laser_to_camera_transf * laser_point);
        cv::Point3d cv_cam_point(cam_point.x(), cam_point.y(), cam_point.z());
        cv::Point2d image_point = cam_model_.project3dToPixel(cv_cam_point);
        
        // fill detections struct
        vAllScans[ii].id    = -1;
        vAllScans[ii].im_x  = floor(image_point.x * width_resize_);
        vAllScans[ii].im_y  = floor(image_point.y * height_resize_);
        vAllScans[ii].depth = sqrt(scan_x*scan_x + scan_y*scan_y);
        vAllScans[ii].score = -2;
      }
          ROS_INFO("Entra_draw");
      cam_detector_.drawalllaserdetections(img_resized, outIm, vAllScans);
          ROS_INFO("Sale_draw");    
        laser_mutex_.exit();

    // draw detections on image
   cam_detector_.drawvalImagelaserdetections(outIm, outIm, vLaserPeople, vOutputPeople, found, minthr_); // only show
 //   cam_detector_.drawvalImagelaserdetectionsFile(outIm, outIm, vLaserPeople, vOutputPeople, found, minthr_); // show and save in a File
//    cam_detector_.drawallinfodetections(outIm, outIm, vLaserPeople, vOutputPeople, found, foundWeights); // show all info from detcs
    
    img_msg.header   = cv_ptr_->header;
    img_msg.encoding = cv_ptr_->encoding;
    img_msg.image    = outIm;
   
    // fill new vector to publish
    fusion_poses_msg.detection.clear();
    fusion_poses_msg.detection.resize(vOutputPeople.size());

    fusion_poses_msg.header = laser_poses_msg_.header;

        ROS_WARN("vOutputPeople.size=%lu ",vOutputPeople.size());
    for(unsigned int ii=0; ii<vOutputPeople.size(); ii++)
    {
          ROS_WARN("vOutputPeople[%d].id=%d laser_poses_msg_.detection.size()=%lu",ii,vOutputPeople[ii].id,laser_poses_msg_.detection.size());
          // TODO: posible problem laser does not contain all the poses from vOutputPeople (when is with tracking)
          fusion_poses_msg.detection[ii] = laser_poses_msg_.detection[vOutputPeople[ii].id]; 
      fusion_poses_msg.detection[ii].probability = vOutputPeople[ii].score;
          
    }

  people_detection_mutex_.exit();
  image_mutex_.exit();

}

void CameraPeopleDetectionAlgNode::mainNodeThread(void)
{
  static std::list<std::string> events;
  events.push_back(new_laser_event_id_);
  events.push_back(new_image_event_id_);
  events.push_back(new_tracker_event_id_);

  // [fill msg structures]

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

  // [publish messages]

  //unsigned int event_id = event_server_->wait_first(events);
  //event_server_->wait_all(events);
  if(event_server_->event_is_set(new_laser_event_id_) && 
     event_server_->event_is_set(new_image_event_id_) &&
     event_server_->event_is_set(new_tracker_event_id_) )
  {
    event_server_->reset_event(new_laser_event_id_);
    event_server_->reset_event(new_image_event_id_);
    event_server_->reset_event(new_tracker_event_id_);

        ROS_INFO("Entra_Camera_People");  
    // filter laser detections with image detections
    iri_perception_msgs::detectionArray fusion_poses_msg;
    cv_bridge::CvImage img_msg;
    filterLaserDetections(fusion_poses_msg, img_msg);
        ROS_INFO("Sale_Camera_People");  

        ROS_INFO("Entra_copydect");  
    // copy image detections to marker array for rviz visualization
    visualization_msgs::MarkerArray markerArray_msg;
    fillMarkerArray(fusion_poses_msg, markerArray_msg);
        ROS_INFO("Sale_copydect");  

        ROS_INFO("Entra_publish");  
    // publish all data
    image_detection_publisher_.publish(fusion_poses_msg);
    markers_publisher_.publish(markerArray_msg);
    debug_img_publisher_.publish(img_msg.toImageMsg());
        ROS_INFO("Sale_publish");  
  }

}

/*  [subscriber callbacks] */
void CameraPeopleDetectionAlgNode::tracker_callback(const iri_perception_msgs::peopleTrackingArray::ConstPtr& msg) 
{ 
  ROS_DEBUG("CameraPeopleDetectionAlgNode::tracker_callback: New Message Received"); 

  tracker_mutex_.enter(); 
    tracker_msg_.header = msg->header;
    tracker_msg_.peopleSet.clear();
    tracker_msg_.peopleSet.resize(msg->peopleSet.size());
    for(unsigned int ii=0; ii<msg->peopleSet.size(); ii++)
    {
      tracker_msg_.peopleSet[ii].targetId = msg->peopleSet[ii].targetId;
      tracker_msg_.peopleSet[ii].x = msg->peopleSet[ii].x;
      tracker_msg_.peopleSet[ii].y = msg->peopleSet[ii].y;
      tracker_msg_.peopleSet[ii].vx = msg->peopleSet[ii].vx;
      tracker_msg_.peopleSet[ii].vy = msg->peopleSet[ii].vy;
      tracker_msg_.peopleSet[ii].covariances = msg->peopleSet[ii].covariances;
    }
  tracker_mutex_.exit(); 
  
  if( !event_server_->event_is_set(new_tracker_event_id_) )
    event_server_->set_event(new_tracker_event_id_);
}

void CameraPeopleDetectionAlgNode::laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) 
{ 
  ROS_DEBUG("CameraPeopleDetectionAlgNode::laser_callback: New Message Received"); 

  laser_mutex_.enter(); 
    scan_.header          = msg->header;
    scan_.angle_min       = msg->angle_min;
    scan_.angle_max       = msg->angle_max;
    scan_.angle_increment = msg->angle_increment;
    scan_.time_increment  = msg->time_increment;
    scan_.scan_time       = msg->scan_time;
    scan_.range_min       = msg->range_min;
    scan_.range_max       = msg->range_max;
    scan_.ranges          = msg->ranges;
    scan_.intensities     = msg->intensities;
  laser_mutex_.exit(); 
}
void CameraPeopleDetectionAlgNode::people_detection_callback(const iri_perception_msgs::detectionArray::ConstPtr& msg) 
{ 
  ROS_DEBUG("CameraPeopleDetectionAlgNode::people_detection_callback: New Message Received"); 

  people_detection_mutex_.enter();
    laser_poses_msg_.detection.clear(); 
    laser_poses_msg_.header     = msg->header;
    laser_poses_msg_.detection  = msg->detection;
  people_detection_mutex_.exit(); 

  if( !event_server_->event_is_set(new_laser_event_id_) )
    event_server_->set_event(new_laser_event_id_);
}

void CameraPeopleDetectionAlgNode::image_callback(const sensor_msgs::ImageConstPtr& image_msg, 
                                                  const sensor_msgs::CameraInfoConstPtr& info_msg)
{
  ROS_DEBUG("CameraPeopleDetectionAlgNode::image_callback: New Message Received"); 

  image_mutex_.enter();
    try
    {
      cv_ptr_ = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    
    // update camera model
    cam_model_.fromCameraInfo(info_msg);
  image_mutex_.exit();

  if( !event_server_->event_is_set(new_image_event_id_) )
    event_server_->set_event(new_image_event_id_);
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void CameraPeopleDetectionAlgNode::node_config_update(Config &config, uint32_t level)
{
  alg_.lock();
  image_mutex_.enter();
    width_resize_  = config.width_resize;
    height_resize_ = config.height_resize;
    minthr_ = config.min_threshold;
    cam_detector_.setDectThr(config.thr_dect);
  image_mutex_.exit();
  alg_.unlock();
}

void CameraPeopleDetectionAlgNode::addNodeDiagnostics(void)
{
}

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