bayesian_grasp_planner: bayesian_grasp

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
bayesian_grasp_planner.h
