structure_extraction_node.cpp

Go to the documentation of this file.

//##################
//#### includes ####
//#include <algorithm>

// ROS includes
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>

// ros message includes
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

//internal includes
#include <cob_3d_mapping_msgs/ShapeArray.h>
#include <cob_3d_mapping_semantics/structure_extraction.h>
#include <cob_3d_mapping_semantics/structure_extraction_nodeConfig.h>
#include <cob_3d_mapping_common/ros_msg_conversions.h>

using namespace cob_3d_mapping;

class StructureExtractionNode
{
public:

  // Constructor
  StructureExtractionNode () :
    target_frame_id_ ("/map"),
    remove_floor_("false"),
    colorize_("false")
  {
    config_server_.setCallback(boost::bind(&StructureExtractionNode::dynReconfCallback, this, _1, _2));

    sa_sub_ = n_.subscribe ("shape_array", 10, &StructureExtractionNode::callbackShapeArray, this);
    sa_pub_ = n_.advertise<cob_3d_mapping_msgs::ShapeArray> ("shape_array_pub", 1);
  }

  // Destructor
  ~StructureExtractionNode ()
  {
  }

  void
  dynReconfCallback(cob_3d_mapping_semantics::structure_extraction_nodeConfig &config, uint32_t level)
  {
    ROS_INFO("[table_extraction]: received new parameters");
    target_frame_id_ = config.target_frame_id;
    se_.setFloorHeight (config.floor_height);
    se_.setCeilingHeight (config.ceiling_height);
    remove_floor_ = config.remove_floor;
    colorize_ = config.colorize;
  }

  void
  callbackShapeArray (const cob_3d_mapping_msgs::ShapeArray::ConstPtr sa_ptr)
  {
    if(sa_ptr->header.frame_id != target_frame_id_)
    {
      ROS_ERROR("Frame IDs do not match, aborting...");
      return;
    }
    cob_3d_mapping_msgs::ShapeArray sa_out;
    sa_out.header = sa_ptr->header;
    sa_out.header.frame_id = target_frame_id_ ;
    for (unsigned int i = 0; i < sa_ptr->shapes.size (); i++)
    {
      Polygon::Ptr poly_ptr (new Polygon());
      fromROSMsg(sa_ptr->shapes[i], *poly_ptr);
      se_.setInputPolygon(poly_ptr);
      unsigned int label;
      se_.classify(label);
      if(colorize_)
      {
        switch (label)
        {
          case 1:
            poly_ptr->color_[0] = 1;
            poly_ptr->color_[1] = 1;
            poly_ptr->color_[2] = 1;
            poly_ptr->color_[3] = 0.5;
            break;
          case 2:
            poly_ptr->color_[0] = 0.5;
            poly_ptr->color_[1] = 0.5;
            poly_ptr->color_[2] = 0.5;
            poly_ptr->color_[3] = 1;
            break;
          case 3:
            poly_ptr->color_[0] = 0;
            poly_ptr->color_[1] = 0;
            poly_ptr->color_[2] = 1;
            poly_ptr->color_[3] = 1;
            break;
        }
      }
      cob_3d_mapping_msgs::Shape s;
      s.header = sa_ptr->header;
      s.header.frame_id = target_frame_id_;
      toROSMsg(*poly_ptr,s);
      if(label == 2 && remove_floor_)
        {}//std::cout << "removing floor" << std::endl;
      else
        sa_out.shapes.push_back (s);
    }
    sa_pub_.publish (sa_out);
  }


  ros::NodeHandle n_;

protected:
  ros::Subscriber sa_sub_;
  ros::Publisher sa_pub_;

  dynamic_reconfigure::Server<cob_3d_mapping_semantics::structure_extraction_nodeConfig> config_server_;

  StructureExtraction se_;

  std::string target_frame_id_;
  bool remove_floor_;
  bool colorize_;

};

int
main (int argc, char** argv)
{
  ros::init (argc, argv, "structure_extraction_node");

  StructureExtractionNode sem_exn_node;
  ros::spin ();
}