grasp_localizer.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Andreas ten Pas
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * 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.
 *
 *  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 OWNER 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.
 */

#ifndef GRASP_LOCALIZER_H_
#define GRASP_LOCALIZER_H_

#include <eigen_conversions/eigen_msg.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <Eigen/Dense>

#include <vector>

#include <agile_grasp/CloudSized.h>
#include <agile_grasp/Grasp.h>
#include <agile_grasp/Grasps.h>
#include <agile_grasp/grasp_hypothesis.h>
#include <agile_grasp/handle.h>
#include <agile_grasp/localization.h>
#include <agile_grasp/rotating_hand.h>


typedef pcl::PointCloud<pcl::PointXYZRGBA> PointCloud;

class GraspLocalizer
{
public:

  struct Parameters
  {
    int num_threads_;
    int num_samples_;
    int num_clouds_;    
    Eigen::Matrix4d cam_tf_left_;
    Eigen::Matrix4d cam_tf_right_;
    Eigen::VectorXd workspace_;
    
    double finger_width_;
    double hand_outer_diameter_;
    double hand_depth_;
    double hand_height_;
    double init_bite_;
        
    // handle search parameters
    int min_inliers_;
    
    // visualization parameters
                int plotting_mode_;
                double marker_lifetime_;
  };
  
  GraspLocalizer(ros::NodeHandle& node, const std::string& cloud_topic, 
    const std::string& cloud_frame, int cloud_type, const std::string& svm_file_name,  
    const Parameters& params);
  
  ~GraspLocalizer() { delete localization_; }
  
  void localizeGrasps();

private:
        
  void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& msg);
  
  void cloud_sized_callback(const agile_grasp::CloudSized& msg);
  
  agile_grasp::Grasps createGraspsMsgFromHands(const std::vector<Handle>& handles);
  
  agile_grasp::Grasps createGraspsMsg(const std::vector<Handle>& handles);
  
  agile_grasp::Grasp createGraspMsg(const Handle& handle);
  
  agile_grasp::Grasps createGraspsMsg(const std::vector<GraspHypothesis>& hands);
  
  agile_grasp::Grasp createGraspMsg(const GraspHypothesis& hand);
  
  std::string svm_file_name_; 
  std::string cloud_frame_; 
  PointCloud::Ptr cloud_left_, cloud_right_; 
  ros::Subscriber cloud_sub_; 
  ros::Publisher grasps_pub_; 
  Localization* localization_; 
  std::vector<GraspHypothesis> hands_; 
  std::vector<GraspHypothesis> antipodal_hands_; 
  std::vector<Handle> handles_; 
  int num_clouds_received_; 
  int num_clouds_; 
  int size_left_; 
  int min_inliers_; 
  
        static const int POINT_CLOUD_2 = 0; 
        static const int CLOUD_SIZED = 1; 
};

#endif /* GRASP_LOCALIZER_H_ */