grasp_adjust.h
Go to the documentation of this file.
00001 /*
00002  * <name of package>
00003  * Copyright (c) 2010, Willow Garage, Inc.
00004  * All rights reserved.
00005  *
00006  * Software License Agreement (BSD License)
00007  *
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of Willow Garage nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
00032  */
00033 
00036 
00037 #ifndef _GRASP_ADJUST_H_
00038 #define _GRASP_ADJUST_H_
00039 
00040 
00041 #include <ros/ros.h>
00042 
00043 #include <tf/tf.h>
00044 #include <tf/transform_listener.h>
00045 #include <tf/transform_broadcaster.h>
00046 
00047 #include <boost/bind.hpp>
00048 
00049 #include "pcl/io/pcd_io.h"
00050 #include "pcl/point_types.h"
00051 
00052 
00053 #include <visualization_msgs/Marker.h>
00054 #include <visualization_msgs/MarkerArray.h>
00055 
00056 
00057 
00058 #include <dynamic_reconfigure/server.h>
00059 #include "pr2_grasp_adjust/EstimateConfig.h"
00060 #include "pr2_grasp_adjust/DebugConfig.h"
00061 #include "pr2_grasp_adjust/GraspAdjust.h"
00062 
00063 
00064 #include <iostream>
00065 #include <fstream>
00066 #include <queue>
00067 
00068 #include "gripper_model.h"
00069 #include "helpers.h"
00070 
00071 
00072 
00073 typedef pcl::PointXYZRGBNormal PointT;
00074 
00075 
00082 class GraspAdjust{
00083 
00084 public:
00085 
00086     pcl::PointCloud<PointT> last_cloud_, cloud_in_, cloud_process_, cloud_out_;
00087 
00088     ros::NodeHandle nh_, nh_pvt_;
00089     //ros::Subscriber sub_cloud_;
00090     ros::Publisher *pub_cloud_, *pub_marker_, *pub_marker_array_, *pub_cloud_roi_;
00091 
00092     tf::TransformListener *listener_;
00093     tf::TransformBroadcaster *broadcaster_;
00094 
00095     ros::Time now_;
00096 
00097     GripperModel starting_gripper_;
00098     tf::Vector3 centroid_;
00099 
00100     bool got_first_cloud_;
00101 
00102     FeatureWeights weights_;
00103 
00104     // ***** Dynamic reconfigure stuff *****
00105     typedef pr2_grasp_adjust::EstimateConfig Config;
00106     Config config_;
00107 
00108     typedef pr2_grasp_adjust::DebugConfig Debug;
00109     Debug debug_;
00110 
00111     // ***** Helpers for training ******
00112     bool training_;
00113     std::ofstream train_file_;
00114 
00115     // This saves time (I think?) because we can copy rather than re-initialize each new gripper. I could be wrong.
00116     const GripperModel DEFAULT_GRIPPER;
00117 
00118     GraspAdjust();
00119 
00120     struct params{
00121         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) {}
00122         double X, Y, Z, r, p, y;
00123     };
00124 
00135     int findGrasps(const geometry_msgs::PoseStamped &grasp_pose_in,
00136                    std::vector<geometry_msgs::PoseStamped> *adjusted_poses,
00137                    std::vector<double> *pose_scores,
00138                    int search_mode = 0,
00139                    std::string common_frame = std::string("base_footprint"));
00140 
00141     int findGrasps(const geometry_msgs::PoseStamped &grasp_pose_in,
00142                    std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> *ranked_poses,
00143                    int search_mode = 0,
00144                    std::string common_frame = std::string("base_footprint"));
00145 
00146 //    /*!
00147 //    * \brief The function that performs the whole grasp adjustment.
00148 //    *
00149 //    * This description is displayed lower in the doxygen as an extended description along with
00150 //    * the above brief description.
00151 //    * \param grasp_pose The starting pose for evaluation.
00152 //    * \param adjusted_poses A vector for returning the candidate poses.
00153 //    * \param search_mode Selects between a local or global search or single pose (which is for debugging).
00154 //    * \param common_frame The frame to which everything is transformed (base_footprint is probably the best).
00155 //    */
00156 //    int findGrasps(const geometry_msgs::PoseStamped &grasp_pose_in,
00157 //                   std::vector<geometry_msgs::PoseStamped> *adjusted_poses,
00158 //                   int search_mode = 0,
00159 //                   std::string common_frame = std::string("base_footprint"));
00160 
00161     void setInputCloud(const pcl::PointCloud<PointT> &cloud);
00162 
00163 
00164 private:
00165 
00174     bool doGradientDescent(GripperModel &seed, int steps);
00175 
00176 
00177 
00178 
00179 };
00180 
00181 #endif


pr2_grasp_adjust
Author(s): Adam Leeper
autogenerated on Fri Jan 3 2014 12:00:28