regression_grasp_evaluator_node.cpp
Go to the documentation of this file.
00001 #include <utility>
00002 #include <algorithm>
00003 #include <string>
00004 #include <functional>
00005 
00006 #include <time.h>
00007 
00008 #include <boost/foreach.hpp>
00009 #include <boost/lexical_cast.hpp>
00010 
00011 #include <boost/shared_ptr.hpp>
00012 using boost::shared_ptr;
00013 
00014 #include <ros/ros.h>
00015 
00016 #include <object_manipulation_msgs/Grasp.h>
00017 #include <object_manipulation_msgs/GraspableObject.h>
00018 #include <object_manipulation_msgs/GraspPlanning.h>
00019 #include <object_manipulation_msgs/GraspPlanningAction.h>
00020 #include <object_manipulation_msgs/GraspPlanningGoal.h>
00021 #include <object_manipulation_msgs/GraspPlanningResult.h>
00022 
00023 #include <household_objects_database/objects_database.h>
00024 using household_objects_database::ObjectsDatabasePtr;
00025 
00026 #include "bayesian_grasp_planner/grasp_evaluator.h"
00027 #include "bayesian_grasp_planner/grasp_generator.h"
00028 #include "bayesian_grasp_planner/bayesian_grasp_planner_tools.h"
00029 
00030 namespace bayesian_grasp_planner {
00031 
00032 class RegressionGraspEvaluatorNode
00033 {
00034 private:
00035 
00036   ros::NodeHandle priv_nh_;
00037   ros::NodeHandle root_nh_;
00038 
00040   ros::ServiceServer grasp_eval_srv_;
00041 
00042   ObjectsDatabasePtr database_;
00043 
00044   //bool show_colored_grasps_;
00045 
00047   bool initializeDatabase();
00048 
00050   bool graspEvalCB(object_manipulation_msgs::GraspPlanning::Request &request,
00051                    object_manipulation_msgs::GraspPlanning::Response &response);
00052 
00053   void appendMetadataToTestGrasps(std::vector<object_manipulation_msgs::Grasp> &input_list,
00054                                                                   std::vector<GraspWM> &output_list,
00055                                                                   const std::string frame_id);
00056 
00057 public:
00058   RegressionGraspEvaluatorNode();
00059   virtual ~RegressionGraspEvaluatorNode() {};
00060 };
00061 
00062 RegressionGraspEvaluatorNode::RegressionGraspEvaluatorNode():
00063     priv_nh_("~"),
00064     root_nh_("")
00065 {
00066   if ( !initializeDatabase() ) ros::shutdown();
00067 
00068   grasp_eval_srv_ = root_nh_.advertiseService("evaluate_grasp_regression",
00069                                               &RegressionGraspEvaluatorNode::graspEvalCB, this);
00070 
00071   ROS_INFO("Regression grasp evaluator service ready");
00072 }
00073 
00074 
00075 bool RegressionGraspEvaluatorNode::initializeDatabase()
00076 {
00077   //try to open a connection to the model database
00078   std::string database_host, database_port, database_user, database_pass, database_name;
00079   root_nh_.param<std::string> ("household_objects_database/database_host", database_host, "wgs36");
00080   //the param server decides by itself that this is an int, so we have to get it as one
00081   int port_int;
00082   root_nh_.param<int> ("household_objects_database/database_port", port_int, 5432);
00083   //convert it to a string
00084   std::stringstream ss;
00085   ss << port_int;
00086   database_port = ss.str();
00087   root_nh_.param<std::string> ("household_objects_database/database_user", database_user, "willow");
00088   root_nh_.param<std::string> ("household_objects_database/database_pass", database_pass, "willow");
00089   root_nh_.param<std::string> ("household_objects_database/database_name", database_name, "household_objects");
00090   database_.reset(new household_objects_database::ObjectsDatabase(database_host, database_port, database_user,
00091                                                               database_pass, database_name));
00092   if (!database_->isConnected())
00093   {
00094     ROS_ERROR("Failed to open model database on host %s, port %s, user %s with password %s, database %s",
00095               database_host.c_str(), database_port.c_str(), database_user.c_str(), database_pass.c_str(),
00096               database_name.c_str());
00097     database_.reset(); //Reset the shared ptr
00098     return false;
00099   }
00100   return true;
00101 }
00102 
00103 
00104 bool RegressionGraspEvaluatorNode::graspEvalCB(object_manipulation_msgs::GraspPlanning::Request &request,
00105                                                                                    object_manipulation_msgs::GraspPlanning::Response &response)
00106 {
00107    //create the probability distributions for graspit raw results
00108    boost::shared_ptr<ProbabilityDistribution> graspit_success_distribution(
00109                                                  new GaussianProbabilityDistribution(.825, .125, 0.0, 1.0, false) );
00110    boost::shared_ptr<ProbabilityDistribution> graspit_failure_distribution(
00111                                                  new GaussianProbabilityDistribution(.627, .376, 0.0, 1.0, false) );
00112 
00113    //one for each database model/pose detected
00114    std::vector<object_manipulation_msgs::GraspableObject> objects;
00115    for (size_t i = 0; i < request.target.potential_models.size(); i++)
00116    {
00117         object_manipulation_msgs::GraspableObject new_object;
00118         new_object.potential_models.push_back(request.target.potential_models[i]);
00119         new_object.reference_frame_id = request.target.reference_frame_id;
00120         objects.push_back(new_object);
00121    }
00122 
00123    //create the graspit-based regression evaluator
00124    boost::shared_ptr<MultiplexEvaluator> graspit_multiplexer_regression_eval( new MultiplexEvaluator );
00125 
00126    //for each database object
00127    BOOST_FOREACH(const household_objects_database_msgs::DatabaseModelPose &model, request.target.potential_models)
00128    {
00129      //create a grasp generator with all grasps
00130      boost::shared_ptr<GraspGenerator> grasp_gen( new GraspGeneratorDatabaseRetriever(database_, model, request.arm_name, false) );
00131      grasp_gen->generateGrasps();
00132 
00133      //create a regression evaluator based on it
00134      boost::shared_ptr<RawGraspEvaluator> regression_eval( new RawGraspEvaluatorWithRegression(grasp_gen, true) );
00135 
00136      //put that in our overall multiplexer graspit-regression-based evaluator
00137      graspit_multiplexer_regression_eval->addEvaluator(regression_eval, model.model_id);
00138    }
00139 
00140    //create a probabilistic estimator based on the graspit raw evaluator
00141    boost::shared_ptr<GraspEvaluatorProb> prob_eval( new GraspEvaluatorProb( graspit_success_distribution,
00142                                                                             graspit_failure_distribution,
00143                                                                             graspit_multiplexer_regression_eval ) );
00144 
00145    //add metadata to grasps we need to evaluate
00146    std::vector<GraspWM> grasps;
00147    appendMetadataToTestGrasps(request.grasps_to_evaluate, grasps, request.target.reference_frame_id);
00148 
00149    std::vector<double> grasp_success_probs;
00150    std::vector<double> grasp_failure_probs;
00151 
00152    // initialize the success_cond_probs and failure_cond_probs for all grasps
00153    BOOST_FOREACH(GraspWM &grasp, grasps)
00154    {
00155      grasp.success_cond_probs.resize(objects.size());
00156      grasp.failure_cond_probs.resize(objects.size());
00157    }
00158 
00159    // evaluate the grasps
00160    for(size_t object_ind = 0; object_ind < objects.size(); object_ind++)
00161    {
00162      // ignore the nd-object case
00163      if(objects[object_ind].potential_models.empty()) continue;
00164 
00165      // evaluate all grasps at once
00166      prob_eval->getProbabilitiesForGraspList(grasps, objects[object_ind], grasp_success_probs, grasp_failure_probs);
00167 
00168      // ROS_INFO("evaluting grasps on object %d", (int)object_ind);
00169      //printf("grasp_success_prob: ");
00170      //pplist(grasp_success_probs);
00171      //printf("grasp_failure_prob: ");
00172      //pplist(grasp_failure_probs);
00173 
00174      // put the results in the appropriate list for this object rep
00175      for (size_t grasp_ind = 0; grasp_ind < grasps.size(); grasp_ind++)
00176      {
00177        grasps[grasp_ind].success_cond_probs[object_ind].push_back(grasp_success_probs[grasp_ind]);
00178        grasps[grasp_ind].failure_cond_probs[object_ind].push_back(grasp_failure_probs[grasp_ind]);
00179      }
00180    }
00181 
00182    response.grasps.reserve(grasps.size());
00183    for(size_t grasp_i = 0; grasp_i < grasps.size(); grasp_i++)
00184    {
00185            double p_success;
00186 
00187            p_success = grasp_success_probs[grasp_i] / (grasp_success_probs[grasp_i] + grasp_failure_probs[grasp_i]);
00188            response.grasps.push_back(grasps[grasp_i].getGrasp());
00189            response.grasps[grasp_i].success_probability = p_success;
00190    }
00191 
00192   ROS_INFO("Regression grasp evaluator: tested %zd grasps", response.grasps.size());
00193   response.error_code.value = response.error_code.SUCCESS;
00194   return true;
00195 }
00196 
00197 void RegressionGraspEvaluatorNode::appendMetadataToTestGrasps(std::vector<object_manipulation_msgs::Grasp> &input_list,
00198                                                       std::vector<GraspWM> &output_list, const std::string frame_id)
00199 {
00200 
00201   output_list.reserve(output_list.size()+input_list.size());
00202 
00203   ROS_INFO("Got %zd grasps to test",input_list.size());
00204   BOOST_FOREACH(object_manipulation_msgs::Grasp &grasp, input_list)
00205   {
00206     GraspWM grasp_wm;
00207     grasp_wm.grasp_ = grasp;
00208     grasp_wm.grasp_.success_probability = 0.0;
00209     grasp_wm.model_id_ = -1;
00210     grasp_wm.grasp_id_ = -1;
00211 
00212     grasp_wm.object_pose_.setIdentity();
00213     grasp_wm.object_pose_.frame_id_ = frame_id;
00214     grasp_wm.object_pose_.stamp_ = ros::Time(0);
00215 
00217     tf::Pose grasp_in_base_frame;
00218     tf::poseMsgToTF(grasp.grasp_pose, grasp_in_base_frame);
00219     double wrist_to_tool_point_offset_ = 0.13;
00220     tf::Pose grasp_to_tool_point(btMatrix3x3::getIdentity(),btVector3(wrist_to_tool_point_offset_,0,0));
00221     grasp_wm.tool_point_pose_ = tf::Stamped<tf::Pose>(
00222         grasp_in_base_frame * grasp_to_tool_point, grasp_wm.object_pose_.stamp_, frame_id);
00223 
00224     output_list.push_back(grasp_wm);
00225   }
00226   ROS_DEBUG("Created %zd grasps", output_list.size());
00227 }
00228 
00229 } // end namespace
00230 
00231 int main(int argc, char** argv)
00232 {
00233   ros::init(argc, argv, "regression_grasp_evaluator_node");
00234   bayesian_grasp_planner::RegressionGraspEvaluatorNode node;
00235   ros::spin();
00236   return 0;
00237 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


bayesian_grasp_planner
Author(s): Kaijen Hsiao and Matei Ciocarlie
autogenerated on Fri Jan 25 2013 15:02:36