grasp_adjust.h

Go to the documentation of this file.

/*
 * <name of package>
 * Copyright (c) 2010, Willow Garage, Inc.
 * All rights reserved.
 *
 * Software License Agreement (BSD License)
 *
 *
 * 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.
 *     * Neither the name of Willow Garage nor the names of its
 *       contributors may 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 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_ADJUST_H_
#define _GRASP_ADJUST_H_


#include <ros/ros.h>

#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

#include <boost/bind.hpp>

#include "pcl/io/pcd_io.h"
#include "pcl/point_types.h"


#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>



#include <dynamic_reconfigure/server.h>
#include "pr2_grasp_adjust/EstimateConfig.h"
#include "pr2_grasp_adjust/DebugConfig.h"
#include "pr2_grasp_adjust/GraspAdjust.h"


#include <iostream>
#include <fstream>
#include <queue>

#include "gripper_model.h"
#include "helpers.h"



typedef pcl::PointXYZRGBNormal PointT;


class GraspAdjust{

public:

    pcl::PointCloud<PointT> last_cloud_, cloud_in_, cloud_process_, cloud_out_;

    ros::NodeHandle nh_, nh_pvt_;
    //ros::Subscriber sub_cloud_;
    ros::Publisher *pub_cloud_, *pub_marker_, *pub_marker_array_, *pub_cloud_roi_;

    tf::TransformListener *listener_;
    tf::TransformBroadcaster *broadcaster_;

    ros::Time now_;

    GripperModel starting_gripper_;
    tf::Vector3 centroid_;

    bool got_first_cloud_;

    FeatureWeights weights_;

    // ***** Dynamic reconfigure stuff *****
    typedef pr2_grasp_adjust::EstimateConfig Config;
    Config config_;

    typedef pr2_grasp_adjust::DebugConfig Debug;
    Debug debug_;

    // ***** Helpers for training ******
    bool training_;
    std::ofstream train_file_;

    // This saves time (I think?) because we can copy rather than re-initialize each new gripper. I could be wrong.
    const GripperModel DEFAULT_GRIPPER;

    GraspAdjust();

    struct params{
        params(double X, double Y, double Z, double r, double p, double y): X(X), Y(Y), Z(Z), r(r), p(p), y(y) {}
        double X, Y, Z, r, p, y;
    };

    int findGrasps(const geometry_msgs::PoseStamped &grasp_pose_in,
                   std::vector<geometry_msgs::PoseStamped> *adjusted_poses,
                   std::vector<double> *pose_scores,
                   int search_mode = 0,
                   std::string common_frame = std::string("base_footprint"));

    int findGrasps(const geometry_msgs::PoseStamped &grasp_pose_in,
                   std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> *ranked_poses,
                   int search_mode = 0,
                   std::string common_frame = std::string("base_footprint"));

//    /*!
//    * \brief The function that performs the whole grasp adjustment.
//    *
//    * This description is displayed lower in the doxygen as an extended description along with
//    * the above brief description.
//    * \param grasp_pose The starting pose for evaluation.
//    * \param adjusted_poses A vector for returning the candidate poses.
//    * \param search_mode Selects between a local or global search or single pose (which is for debugging).
//    * \param common_frame The frame to which everything is transformed (base_footprint is probably the best).
//    */
//    int findGrasps(const geometry_msgs::PoseStamped &grasp_pose_in,
//                   std::vector<geometry_msgs::PoseStamped> *adjusted_poses,
//                   int search_mode = 0,
//                   std::string common_frame = std::string("base_footprint"));

    void setInputCloud(const pcl::PointCloud<PointT> &cloud);


private:

    bool doGradientDescent(GripperModel &seed, int steps);




};

#endif