$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 #ifndef BAYESIAN_GRASP_PLANNER_H 00036 #define BAYESIAN_GRASP_PLANNER_H 00037 00038 #include <boost/shared_ptr.hpp> 00039 #include <object_manipulator/tools/grasp_marker_publisher.h> 00040 #include <object_manipulation_msgs/GraspableObject.h> 00041 #include <object_manipulation_msgs/Grasp.h> 00042 #include "bayesian_grasp_planner/bayesian_grasp_planner_tools.h" 00043 #include <household_objects_database/objects_database.h> 00044 00045 namespace bayesian_grasp_planner 00046 { 00047 00048 //class GraspWM; 00049 class ObjectDetector; 00050 class GraspEvaluatorProb; 00051 00052 class BayesianGraspPlanner 00053 { 00054 private: 00055 00056 ros::NodeHandle nh_; 00057 00058 household_objects_database::ObjectsDatabasePtr database_; 00059 00061 object_manipulator::GraspMarkerPublisher debug_preprune_grasp_marker_publisher_; 00062 00064 object_manipulator::GraspMarkerPublisher debug_postprune_grasp_marker_publisher_; 00065 00067 object_manipulator::GraspMarkerPublisher grasp_marker_publisher_; 00068 00073 void visualizeGrasps(const std::vector<GraspWM> &grasps, 00074 object_manipulator::GraspMarkerPublisher *grasp_marker_pub, bool color_by_rank); 00075 00076 00077 void createMutuallyExclusiveObjectRepresentations(const object_manipulation_msgs::GraspableObject &original_object, 00078 std::vector<object_manipulation_msgs::GraspableObject> &me_objects ); 00079 00080 void createDatabaseObjectDetectors(const std::vector<object_manipulation_msgs::GraspableObject> &objects, 00081 std::vector< boost::shared_ptr<ObjectDetector> >&detectors); 00082 00083 void appendMetadataToTestGrasps(std::vector<object_manipulation_msgs::Grasp> &input_list, 00084 std::vector<GraspWM> &output_list, 00085 const std::string frame_id); 00086 00087 void clusterGrasps(std::vector<GraspWM> &input_list, 00088 std::vector<GraspWM> &cluster_rep_list); 00089 00090 void pruneGraspList(std::vector<GraspWM> &grasps, const double threshold); 00091 00092 public: 00093 BayesianGraspPlanner(household_objects_database::ObjectsDatabasePtr database) : 00094 nh_("~"),database_(database), 00095 debug_preprune_grasp_marker_publisher_("debug_preprune_grasp_list","",5.0), 00096 debug_postprune_grasp_marker_publisher_("debug_postprune_grasp_list","",5.0), 00097 grasp_marker_publisher_("grasp_marker_publisher","",5.0) 00098 {} 00099 00100 void bayesianInference(std::vector<GraspWM> &grasps, 00101 const std::vector<object_manipulation_msgs::GraspableObject> &objects, 00102 const std::vector< boost::shared_ptr<ObjectDetector> > &object_detectors, 00103 const std::vector< boost::shared_ptr<GraspEvaluatorProb> > &prob_evaluators); 00104 00105 void plan(const std::string &arm_name, 00106 const object_manipulation_msgs::GraspableObject &graspable_object, 00107 std::vector<object_manipulation_msgs::Grasp> &final_grasp_list); 00108 00109 }; 00110 typedef boost::shared_ptr<BayesianGraspPlanner> BayesianGraspPlannerPtr; 00111 00112 } //namespace 00113 00114 #endif