$search

tabletop_vfh_classifier.cpp

Go to the documentation of this file.

#include <string>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

#include <tf/transform_listener.h>
#include <vector>
#include "boost/filesystem/operations.hpp"
#include "boost/filesystem/path.hpp"
#include "geometry_msgs/Pose.h"

#include <household_objects_database_msgs/DatabaseModelPose.h>
#include <household_objects_database_msgs/DatabaseModelPoseList.h>
#include <household_objects_database_msgs/GetModelMesh.h>

#include "tabletop_vfh_cluster_detector/TabletopObjectRecognition.h"
#include <vfh_recognizer_db/vfh_recognition.h>

#include "tabletop_vfh_cluster_detector/marker_generator.h"

namespace tabletop_vfh_cluster_detector
{
  class TabletopVFHClassifier
  {
  private:
    ros::NodeHandle nh_;
    ros::NodeHandle priv_nh_;
    ros::ServiceServer object_recognition_srv_;
    ros::ServiceClient get_model_mesh_srv_;
    
    ros::Publisher marker_pub_;
    tf::TransformListener listener_;

    int num_markers_published_;
    int current_marker_id_;

    //VFH recognizer
    vfh_recognizer_db::VFHRecognizerDB<flann::HistIntersectionUnionDistance > vfh_recognizer;

    void publishFitMarkers(const std::vector<household_objects_database_msgs::DatabaseModelPoseList> &potential_models,
                           std::string frame_id);

    void clearOldMarkers(std::string frame_id);

    //------------------ Callbacks -------------------

    bool
    serviceCallback (TabletopObjectRecognition::Request &request, TabletopObjectRecognition::Response &response);

  public:
    TabletopVFHClassifier (ros::NodeHandle nh);

    ~TabletopVFHClassifier ()
    {
    }
  };

  TabletopVFHClassifier::TabletopVFHClassifier (ros::NodeHandle nh) :
    nh_ (nh), priv_nh_ ("~")
  {
    num_markers_published_ = 1;
    current_marker_id_ = 1;

    std::string get_model_mesh_srv_name;
    priv_nh_.param<std::string>("get_model_mesh_srv", get_model_mesh_srv_name, "get_model_mesh_srv");
    while ( !ros::service::waitForService(get_model_mesh_srv_name, ros::Duration(2.0)) && nh_.ok() ) 
    {
      ROS_INFO("Waiting for %s service to come up", get_model_mesh_srv_name.c_str());
    }
    if (!nh_.ok()) exit(0);
    get_model_mesh_srv_ = nh_.serviceClient<household_objects_database_msgs::GetModelMesh>
      (get_model_mesh_srv_name, true);

    marker_pub_ = nh_.advertise<visualization_msgs::Marker>(nh_.resolveName("markers_out"), 10);

    //advertise service and define service callback
    object_recognition_srv_ = nh_.advertiseService (nh_.resolveName ("object_recognition_srv"),
                                                    &TabletopVFHClassifier::serviceCallback, this);

    //initialize vfh_recognizer
    bf::path dpath = "/tmp/training_data";
    vfh_recognizer.cache_enabled_ = true;
    vfh_recognizer.initialize (false, dpath, 0, 41);
    vfh_recognizer.setApplyVoxelGrid(false);
    vfh_recognizer.setApplyICP(true);
    vfh_recognizer.setICPIterations(10);
  }

  void TabletopVFHClassifier::publishFitMarkers(
                        const std::vector<household_objects_database_msgs::DatabaseModelPoseList> &potential_models,
                        std::string frame_id)
  {
    for (size_t i=0; i<potential_models.size(); i++)
    {
      const std::vector<household_objects_database_msgs::DatabaseModelPose> models = potential_models[i].model_list;
      for (size_t j=0; j<models.size(); j++)
      {
        household_objects_database_msgs::GetModelMesh get_mesh;
        get_mesh.request.model_id = models[j].model_id;
        if ( !get_model_mesh_srv_.call(get_mesh) ||
             get_mesh.response.return_code.code != get_mesh.response.return_code.SUCCESS )
        {
          ROS_ERROR("tabletop_object_detector: failed to call database get mesh service for marker display");
        }
        else
        {
          double rank = ((double)j) / std::max( (int)(models.size())-1, 1 );
          visualization_msgs::Marker fitMarker =  MarkerGenerator::getFitMarker(get_mesh.response.mesh, rank);
          fitMarker.header.stamp = ros::Time::now();
          fitMarker.header.frame_id = frame_id;
          fitMarker.pose = models[j].pose.pose;
          fitMarker.ns = "tabletop_node_model_" + boost::lexical_cast<std::string>(j);
          fitMarker.id = current_marker_id_++;
          marker_pub_.publish(fitMarker);
        }  
      }
    }
  }

  void TabletopVFHClassifier::clearOldMarkers(std::string frame_id)
  {
    for (int id=current_marker_id_; id < num_markers_published_; id++)
    {
      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;
      //a hack, but we don't know what namespace the marker was in
      for (size_t j=0; j<10; j++)
      {
        delete_marker.ns = "tabletop_node_model_" + boost::lexical_cast<std::string>(j);
        marker_pub_.publish(delete_marker);
      }
    }
    num_markers_published_ = current_marker_id_;
    current_marker_id_ = 0;
  }


  bool
  TabletopVFHClassifier::serviceCallback (TabletopObjectRecognition::Request &request,
                                          TabletopObjectRecognition::Response &response)
  {
    //std::vector < boost::shared_ptr<household_objects_database::DatabaseView> > views;
    ROS_INFO("Incoming service call");
    //iterate over the clusters and call detection
    for (size_t i = 0; i < request.clusters.size (); ++i)
    {
      ROS_INFO("Processing model %d", i);
      int numModels = request.num_models;
      std::vector<int> scaled_model_ids;
      std::vector<float> confidences;
      std::vector < geometry_msgs::Pose > poses;

      std::cout << "NUM_MODELS:" << numModels << std::endl;
      numModels = 10;
      //transform cluster to sensor_msg/PointCloud2
      sensor_msgs::PointCloud2* pc2 = new sensor_msgs::PointCloud2;
      ROS_INFO("  Original point cloud has %d points in frame %s", request.clusters[i].points.size(),
               request.clusters[i].header.frame_id.c_str());
      sensor_msgs::convertPointCloudToPointCloud2(request.clusters[i], *pc2);
      ROS_INFO("  Converted point cloud has data size %d", (int)pc2->data.size());
      sensor_msgs::PointCloud2Ptr msg(pc2);
      vfh_recognizer.detect (msg, numModels, scaled_model_ids, poses, confidences, true);
      ROS_INFO("  Recognition complete. Found %d models", (int)scaled_model_ids.size());

      household_objects_database_msgs::DatabaseModelPoseList model_potential_fit_list;

       for (size_t j=0; j < request.num_models; j++) {
        //create the model fit result
        //std::cout << poses[i] << std::endl;
        household_objects_database_msgs::DatabaseModelPose pose_msg;
        pose_msg.model_id = scaled_model_ids[j];
        std::cout << "id:" << scaled_model_ids[j] << std::endl;
        pose_msg.pose.header = request.clusters[i].header;
        pose_msg.pose.pose = poses[j];
        pose_msg.confidence = confidences[j];
        //and push it in the list for this cluster
        model_potential_fit_list.model_list.push_back(pose_msg);
      }

      //add the models together with pose to the response
      response.models.push_back(model_potential_fit_list);

      response.cluster_model_indices.push_back(i);
      ROS_INFO("  Model %d complete", (int)i);
    }
    ROS_INFO("Recognition done. Publishing markers.");
    publishFitMarkers(response.models, request.table.pose.header.frame_id);
    clearOldMarkers(request.table.pose.header.frame_id);
    return true;
  }
} //namespace tabletop_vfh_cluster_detector

int
main (int argc, char **argv)
{
  ros::init (argc, argv, "tabletop_vfh_classifier");
  ros::NodeHandle nh;

  tabletop_vfh_cluster_detector::TabletopVFHClassifier node (nh);
  ros::spin ();
  return 0;
}

---

tabletop_vfh_cluster_detector
Author(s): Aitor Aldoma
autogenerated on Tue Mar 5 15:33:25 2013