bayesian_grasp_planner_tools.h
Go to the documentation of this file.
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 _BAYES_NET_GRASP_PLANNER_TOOLS_H_
00036 #define _BAYES_NET_GRASP_PLANNER_TOOLS_H_
00037 
00038 #include <tf/transform_datatypes.h>
00039 #include <ros/ros.h>
00040 
00041 namespace bayesian_grasp_planner {
00042 
00044 class GraspWM
00045 {
00046 public:
00047   int grasp_id_;
00048   int model_id_;
00049   double energy_function_score;
00050   object_manipulation_msgs::Grasp grasp_;
00051   tf::Stamped<tf::Pose> object_pose_;
00052   tf::Stamped<tf::Pose> tool_point_pose_;
00053   double success_probability;
00054   std::vector< std::vector<double> > success_cond_probs;  //conditional probabilities for successful grasps
00055   std::vector< std::vector<double> > failure_cond_probs;  //and failed grasps (one vector for each object rep)
00056 
00057   GraspWM() : success_probability(0.0) {}
00058 
00059   void getDistance(const GraspWM &other, double &cartesian_dist, double &rotation_dist, bool debug=false) const
00060   {
00061     tf::Vector3 gstar_position = tool_point_pose_.getOrigin();
00062     tf::Quaternion gstar_orientation = tool_point_pose_.getRotation();
00063 
00064     tf::Vector3 grasp_position = other.tool_point_pose_.getOrigin();
00065     tf::Quaternion grasp_orientation = other.tool_point_pose_.getRotation();
00066 
00067     if (debug) ROS_INFO("grasp: %f %f %f; other: %f %f %f", 
00068                         gstar_position.x(),gstar_position.y(),gstar_position.z(),
00069                         grasp_position.x(),grasp_position.y(),grasp_position.z());
00070 
00071     tf::Vector3 delta_position = gstar_position - grasp_position;
00072     cartesian_dist = delta_position.length();
00073 
00074     rotation_dist = gstar_orientation.angleShortestPath(grasp_orientation);
00075   }
00076 
00077   object_manipulation_msgs::Grasp getGrasp()
00078   {
00079         return grasp_;
00080   }
00081 };
00082 
00083 inline bool operator< (const GraspWM &g1, const GraspWM &g2)
00084 {
00085   return g1.grasp_.success_probability < g2.grasp_.success_probability;
00086 }
00087 
00088 
00089 //pretty-print a std::vector<double>
00090 inline void pplist(const std::vector<double> &list)
00091 {
00092   for (size_t ind = 0; ind < list.size(); ind++)
00093   {
00094         printf("%.3f ", list[ind]);
00095   }
00096   printf("\n");
00097 }
00098 
00099 
00100 template <class svcType>
00101 inline ros::ServiceClient register_service(ros::NodeHandle &nh, const std::string &service_name)
00102 {
00103   while (!ros::service::waitForService(service_name, ros::Duration(2.0)) && nh.ok())
00104   {
00105     ROS_INFO("Waiting for %s service to come up", service_name.c_str());
00106   }
00107   if (!nh.ok()) exit(0);
00108   return nh.serviceClient<svcType>(service_name, true);
00109 }
00110 
00111 
00113 
00116 template <class ServiceDataType>
00117 class ServiceWrapper
00118 {
00119  private:
00121   bool initialized_;
00123   std::string service_name_;
00125   ros::NodeHandle nh_;
00127   ros::ServiceClient client_;
00128  public:
00129  ServiceWrapper(std::string service_name) : initialized_(false), 
00130     service_name_(service_name),
00131     nh_("")
00132     {}
00133   
00135   ros::ServiceClient& client() 
00136   {
00137     if (!initialized_)
00138     {
00139       while ( !ros::service::waitForService(service_name_, ros::Duration(2.0)) && nh_.ok() )
00140       {
00141         ROS_INFO_STREAM("Waiting for service: " << service_name_);
00142       }
00143       if (!nh_.ok()) exit(0);
00144       client_ = nh_.serviceClient<ServiceDataType>(service_name_);      
00145       initialized_ = true;
00146     }
00147     return client_;
00148   }
00149 };
00150 
00151 
00152 
00153 } //namespace
00154 
00155 #endif


bayesian_grasp_planner
Author(s): Kaijen Hsiao and Matei Ciocarlie
autogenerated on Thu Jan 2 2014 11:40:34