fast_plane_detector_node.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2011, Jeannette Bohg and Mårten Björkman 
 * ({bohg,celle}@csc.kth.se) 
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *  1.Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  2.Redistributions in binary form must reproduce the above
 *    copyright notice, this list of conditions and the following
 *    disclaimer in the documentation and/or other materials provided
 *    with the distribution.  
 *  3.The name of Jeannette Bohg or Mårten Björkman may not be used to 
 *    endorse or promote products derived from this software without 
 *    specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <ros/ros.h>
#include <std_msgs/String.h>

#include <sensor_msgs/CameraInfo.h>
#include <stereo_msgs/DisparityImage.h>

#include <fast_plane_detection/Plane.h>

#include <visualization_msgs/Marker.h>


#include "fast_plane_detector/plane_detection.h"
#include "fast_plane_detector/plane_transform.h"

#include <image_transport/image_transport.h>

#include "fast_plane_detector/timercpu.h"
#include "fast_plane_detector/utils.h"

namespace fast_plane_detection {
  
  class FastPlaneDetector
  {
    
  private:
    
    void clearOldMarkers(std::string frame_id);
    
    void maskDisparityImage(const stereo_msgs::DisparityImage::ConstPtr&
                            disparity_image,
                            const sensor_msgs::Image &labels,
                            stereo_msgs::DisparityImage &masked_disparity);

    ros::NodeHandle root_nh_;
    ros::NodeHandle priv_nh_;
    
    ros::Subscriber camera_info_sub_;
    std::string camera_info_topic_;

    ros::Subscriber disparity_image_sub_;
    std::string disparity_image_topic_;

    int buffer_size_;
    
    stereo_msgs::DisparityImage::ConstPtr disparity_image_;

    sensor_msgs::CameraInfo::ConstPtr camera_info_;
        
    ros::Publisher plane_marker_pub_;

    ros::Publisher plane_msg_pub_;

    ros::Publisher disparity_image_pub_;

    image_transport::Publisher image_pub_;

    

    PlaneDetection* plane_detect_;

    bool find_table_;
  
    PlaneTransform* plane_transform_;

    double up_direction_;
    
    int inlier_threshold_;

    int num_markers_published_;
    int current_marker_id_;

  public:
    
    FastPlaneDetector( int buffer_size );
    ~FastPlaneDetector();
    
    //------------------ Callbacks -------------------

    void planeCallback(const stereo_msgs::DisparityImage::ConstPtr& 
                       disparity_image);

    //------------------ The Data --------------------

    Plane plane_;
    
  };
  
  
  FastPlaneDetector::FastPlaneDetector( int buffer_size)
    : root_nh_("")
    , priv_nh_("~")
    , buffer_size_(buffer_size)
    , num_markers_published_(0)
    , current_marker_id_(0)
  {
    
    // initialise operational flags
    priv_nh_.param<std::string>("camera_info_topic", 
                                camera_info_topic_, 
                                "/narrow_stereo_textured/left/camera_info");
    priv_nh_.param<std::string>("disparity_image_topic", 
                                disparity_image_topic_, 
                                "/narrow_stereo_textured/disparity");
    priv_nh_.param<bool>("find_table", 
                         find_table_, 
                         true);  
    priv_nh_.param<double>("up_direction", 
                           up_direction_, 
                           -1.0f);  
    priv_nh_.param<int>("inlier_threshold", 
                        inlier_threshold_, 
                        300);  

    // retrieve camera info just once and one sample disparity 
    // for parameter extraction
    while (!camera_info_ || ! disparity_image_){
      if( !camera_info_ ){
        ROS_INFO_STREAM("  Waiting for message on topic " 
                        << camera_info_topic_);
        camera_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>
          (camera_info_topic_, root_nh_, ros::Duration(0.5));
      }

      if( !disparity_image_){
        ROS_INFO_STREAM("  Waiting for message on topic " 
                        << disparity_image_topic_);
        disparity_image_ = ros::topic::waitForMessage<stereo_msgs::DisparityImage>
          (disparity_image_topic_, root_nh_, ros::Duration(1.0));
      }
    }
    
    // get parameters from disparity image
    float drange = disparity_image_->max_disparity - 
      disparity_image_->min_disparity;
    int w = disparity_image_->image.width;
    int h = disparity_image_->image.height;
    
    plane_detect_ = new PlaneDetection((int)drange, w, h, find_table_, 
                                       disparity_image_->image.header);

    plane_transform_ = new PlaneTransform( *camera_info_, 
                                           disparity_image_->T, 
                                           up_direction_);
    // subscribe to disparity images
    disparity_image_sub_ = root_nh_.subscribe(disparity_image_topic_, 
                                              buffer_size_, 
                                              &FastPlaneDetector::planeCallback,
                                              this);
    
    // publish plane markers
    plane_marker_pub_ = root_nh_.advertise<visualization_msgs::Marker>("plane_markers", 1); 

    // publish plane msg
    plane_msg_pub_ = root_nh_.advertise<Plane>("plane_msg", 1);

    // publish masked disparity images
    std::string topic = root_nh_.resolveName("/plane_detection/disparity");
    disparity_image_pub_ 
      = root_nh_.advertise<stereo_msgs::DisparityImage>(topic, 1);
    
    image_transport::ImageTransport it_(root_nh_);
    image_pub_ = it_.advertise("plane_out", 1);
    
  }

  FastPlaneDetector::~FastPlaneDetector()
  {}


  void FastPlaneDetector::planeCallback(const stereo_msgs::DisparityImage::ConstPtr& disparity_image) 
  {
    TimerCPU timer(2800);
    
    plane_detect_->Update(disparity_image);

    float alpha, beta, d;
    int min_x, max_x, min_y, max_y;
    plane_detect_->GetPlaneParameters( alpha, beta, d, 
                                       min_x, max_x, min_y, max_y);
    
    sensor_msgs::Image labels;
    plane_detect_->GetLabels(labels);

    stereo_msgs::DisparityImage disparity_masked;
    maskDisparityImage(disparity_image, labels, disparity_masked);
    
    int sum = 0;
    for(unsigned int i=0; i<labels.height; ++i)
      for(unsigned int j=0; j<labels.width; ++j){
        int adr = i*labels.width+j; 
        sum+=(labels.data[adr])/255;
      }
      
    
    image_pub_.publish(labels);
    disparity_image_pub_.publish(disparity_masked);
    

    if(sum<inlier_threshold_){
      ROS_WARN("Plane has only %d inliers", sum);
      return;
    }
    
    plane_transform_->setParameters( alpha, 
                                     beta, 
                                     d,
                                     min_x, max_x, 
                                     min_y, max_y, 
                                     disparity_image->header);
    
    if(!plane_transform_->getPlane(plane_)) {
      ROS_WARN("No Plane found");
      return; 
    } else {
      
      visualization_msgs::Marker planeMarker 
        = getPlaneMarker( plane_.top_left, 
                          plane_.top_right,
                          plane_.bottom_left, 
                          plane_.bottom_right);
      planeMarker.header = disparity_image->header;
      planeMarker.pose = plane_.pose.pose;
      planeMarker.ns = "tabletop_node";
      planeMarker.id = current_marker_id_++;
      plane_marker_pub_.publish(planeMarker);
      
      /*      ROS_INFO("Before clearing old");
      ROS_INFO("current_marker_id %d", current_marker_id_);
      ROS_INFO("num_markers_published %d", num_markers_published_);
      */
      clearOldMarkers(disparity_image->header.frame_id);

      /*
      ROS_INFO("After clearing old");
      ROS_INFO("current_marker_id %d", current_marker_id_);
      ROS_INFO("num_markers_published %d", num_markers_published_);
      */
    }

    ROS_INFO_STREAM("Time: "  << timer.read() << " ms");
    
  }
  
  void FastPlaneDetector::clearOldMarkers(std::string frame_id)
  {
    for (int id=current_marker_id_; id < num_markers_published_; id++)
      {

        ROS_WARN("Cleared old markers");

        visualization_msgs::Marker delete_marker;
        delete_marker.header.stamp = ros::Time::now();
        delete_marker.header.frame_id = frame_id;
        delete_marker.id = id;
        delete_marker.action = visualization_msgs::Marker::DELETE;
        delete_marker.ns = "tabletop_node";
        plane_marker_pub_.publish(delete_marker);
      }
    
    num_markers_published_ = current_marker_id_;
    current_marker_id_ = 0;
  }
 

  void 
  FastPlaneDetector::maskDisparityImage( const stereo_msgs::DisparityImage::ConstPtr
                                         &disparity_image,
                                         const sensor_msgs::Image &labels,
                                         stereo_msgs::DisparityImage &masked_disparity)
  {
    int height = labels.height;
    int width  = labels.width;
    
    masked_disparity.header = disparity_image->header;
    masked_disparity.image.header = disparity_image->image.header;
    masked_disparity.image.height = height;
    masked_disparity.image.width = width;
    masked_disparity.image.encoding = disparity_image->image.encoding;
    masked_disparity.image.is_bigendian = disparity_image->image.is_bigendian;
    masked_disparity.image.step = disparity_image->image.step;
    size_t size = masked_disparity.image.step * masked_disparity.image.height;
    masked_disparity.image.data.resize(size, 0);

    masked_disparity.f = disparity_image->f;
    masked_disparity.T = disparity_image->T;
    masked_disparity.min_disparity = disparity_image->min_disparity;
    masked_disparity.max_disparity = disparity_image->max_disparity;
    masked_disparity.delta_d = disparity_image->delta_d;

    float *dimd_in  = (float*)&(disparity_image->image.data[0]);
    uint8_t *labelsd = (uint8_t*)&(labels.data[0]);
    
    float *dimd_out = (float*)&(masked_disparity.image.data[0]);
    
    for (int y=0;y<height; y++) {
      for (int x=0;x<width; x++) {
        int adr = y*width+x;
        if(labelsd[adr]==0)
          dimd_out[adr] = dimd_in[adr];
        else dimd_out[adr] = -1.0f;
      }
    }
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "fast_plane_detection_node");
  
  int buffer_size_ = 1;
  fast_plane_detection::FastPlaneDetector node_(buffer_size_);
  ros::spin();
  
  return 0;
}