bayesian_grasp_planner_node.cpp
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 #include <utility>
00036 #include <algorithm>
00037 #include <string>
00038 #include <functional>
00039 
00040 #include <time.h>
00041 
00042 #include <boost/foreach.hpp>
00043 #include <boost/lexical_cast.hpp>
00044 
00045 #include <boost/shared_ptr.hpp>
00046 using boost::shared_ptr;
00047 
00048 #include <ros/ros.h>
00049 #include <actionlib/server/simple_action_server.h>
00050 
00051 #include <object_manipulation_msgs/Grasp.h>
00052 #include <object_manipulation_msgs/GraspableObject.h>
00053 #include <object_manipulation_msgs/GraspPlanning.h>
00054 #include <object_manipulation_msgs/GraspPlanningAction.h>
00055 #include <object_manipulation_msgs/GraspPlanningGoal.h>
00056 #include <object_manipulation_msgs/GraspPlanningResult.h>
00057 
00058 #include <object_manipulator/tools/grasp_marker_publisher.h>
00059 
00060 #include <household_objects_database/objects_database.h>
00061 using household_objects_database::ObjectsDatabasePtr;
00062 
00063 #include "bayesian_grasp_planner/bayesian_grasp_planner.h"
00064 
00065 namespace bayesian_grasp_planner {
00066 
00067 static const std::string PLANNING_MARKERS_OUT_NAME = "grasp_planning_markers";
00068 
00069 class BayesianGraspPlannerNode
00070 {
00071 private:
00072 
00073   ros::NodeHandle priv_nh_;
00074   ros::NodeHandle root_nh_;
00075 
00077   ros::ServiceServer grasp_execution_srv_;
00078 
00080   actionlib::SimpleActionServer<object_manipulation_msgs::GraspPlanningAction> grasp_planning_aserv_;
00081   object_manipulation_msgs::GraspPlanningResult grasp_planning_result_;
00082 
00084   ros::ServiceServer grasp_test_srv_;
00085 
00087   object_manipulator::GraspMarkerPublisher debug_grasp_marker_pub_;
00088 
00090   object_manipulator::GraspMarkerPublisher results_grasp_marker_pub_;
00091 
00093   object_manipulator::GraspMarkerPublisher rank_grasp_marker_pub_;
00094 
00095   ObjectsDatabasePtr database_;
00096 
00097   //bool show_colored_grasps_;
00098 
00100   BayesianGraspPlannerPtr planner_;
00101 
00103   bool initializeDatabase();
00104 
00106   bool graspPlanningCB(object_manipulation_msgs::GraspPlanning::Request &request, 
00107                        object_manipulation_msgs::GraspPlanning::Response &response);
00108 
00110   bool graspTestCB(object_manipulation_msgs::GraspPlanning::Request &request,
00111                    object_manipulation_msgs::GraspPlanning::Response &response);
00112 
00114   void graspPlanningExecuteCB(const object_manipulation_msgs::GraspPlanningGoalConstPtr &goal);
00115 
00116 
00117 public:
00118   BayesianGraspPlannerNode();
00119   virtual ~BayesianGraspPlannerNode();
00120 };
00121 
00122 BayesianGraspPlannerNode::BayesianGraspPlannerNode():
00123     priv_nh_("~"), 
00124     root_nh_(""),
00125     debug_grasp_marker_pub_(PLANNING_MARKERS_OUT_NAME, "", 5.0),
00126     results_grasp_marker_pub_("final_sorted_grasp_list", "", 5.0),
00127     rank_grasp_marker_pub_("debug_ranked_grasp_list", "", 5.0),
00128     grasp_planning_aserv_(root_nh_, "probabilistic_grasp_planning",
00129         boost::bind(&BayesianGraspPlannerNode::graspPlanningExecuteCB, this, _1), false)
00130 
00131 {
00132   if ( !initializeDatabase() ) ros::shutdown();
00133   planner_.reset(new BayesianGraspPlanner(database_));
00134 
00135   //Advertise the service
00136   grasp_execution_srv_ = root_nh_.advertiseService("probabilistic_grasp_planning", 
00137                                                    &BayesianGraspPlannerNode::graspPlanningCB, this);
00138 
00139 
00140   grasp_test_srv_ = root_nh_.advertiseService("test_grasp_on_probabilistic_planner",
00141                                               &BayesianGraspPlannerNode::graspTestCB, this);
00142 
00143   grasp_planning_aserv_.start();
00144 
00145   ROS_INFO("Robust grasp planner service ready");
00146 }
00147 
00148   
00149 bool BayesianGraspPlannerNode::initializeDatabase()
00150 {
00151   //try to open a connection to the model database
00152   std::string database_host, database_port, database_user, database_pass, database_name;
00153   root_nh_.param<std::string> ("household_objects_database/database_host", database_host, "wgs36");
00154   //the param server decides by itself that this is an int, so we have to get it as one
00155   int port_int;
00156   root_nh_.param<int> ("household_objects_database/database_port", port_int, 5432);
00157   //convert it to a string
00158   std::stringstream ss;
00159   ss << port_int;
00160   database_port = ss.str();
00161   root_nh_.param<std::string> ("household_objects_database/database_user", database_user, "willow");
00162   root_nh_.param<std::string> ("household_objects_database/database_pass", database_pass, "willow");
00163   root_nh_.param<std::string> ("household_objects_database/database_name", database_name, "household_objects");
00164   database_.reset(new household_objects_database::ObjectsDatabase(database_host, database_port, database_user,
00165                                                               database_pass, database_name));
00166   if (!database_->isConnected())
00167   {
00168     ROS_ERROR("Failed to open model database on host %s, port %s, user %s with password %s, database %s",
00169               database_host.c_str(), database_port.c_str(), database_user.c_str(), database_pass.c_str(),
00170               database_name.c_str());
00171     database_.reset(); //Reset the shared ptr
00172     return false;
00173   }
00174   return true;
00175 }
00176 
00177 bool BayesianGraspPlannerNode::graspPlanningCB(object_manipulation_msgs::GraspPlanning::Request &request,
00178                                                object_manipulation_msgs::GraspPlanning::Response &response)
00179 {
00180   //If we are actually requesting that these grasps are evaluated in our planner, tell the planner to use those
00181   if (request.grasps_to_evaluate.size() > 0)
00182   {
00183     ROS_WARN("Grasps in test list, but grasp planner was called");
00184   }
00185   else
00186   {
00187     planner_->plan(request.arm_name, request.target, response.grasps);
00188   }
00189   ROS_INFO("Probabilistic grasp planner: returning %zd grasps", response.grasps.size());
00190   response.error_code.value = response.error_code.SUCCESS;
00191   return true;
00192 }
00193 
00194 void BayesianGraspPlannerNode::graspPlanningExecuteCB(
00195     const object_manipulation_msgs::GraspPlanningGoalConstPtr &goal)
00196 {
00197   //If we are actually requesting that these grasps are evaluated in our planner, tell the planner to use those
00198   if (goal->grasps_to_evaluate.size() > 0)
00199   {
00200     ROS_WARN("Grasps in test list, but grasp planner was called");
00201   }
00202   else
00203   {
00204     planner_->plan(goal->arm_name, goal->target, grasp_planning_result_.grasps);
00205   }
00206   ROS_INFO("Probabilistic grasp planner: returning %zd grasps", grasp_planning_result_.grasps.size());
00207   grasp_planning_result_.error_code.value = grasp_planning_result_.error_code.SUCCESS;
00208   grasp_planning_aserv_.setSucceeded(grasp_planning_result_);
00209 }
00210 
00211 
00212 bool BayesianGraspPlannerNode::graspTestCB(object_manipulation_msgs::GraspPlanning::Request &request,
00213                                                                                    object_manipulation_msgs::GraspPlanning::Response &response)
00214 {
00215   //If we are actually requesting that these grasps are evaluated in our planner, tell the planner to use those
00216   if (request.grasps_to_evaluate.size() > 0)
00217   {
00218     planner_->plan(request.arm_name, request.target, request.grasps_to_evaluate);
00219     response.grasps = request.grasps_to_evaluate;
00220   }
00221   else
00222   {
00223     ROS_WARN("Grasp test service called but no grasps given to test");
00224   }
00225   ROS_INFO("Probabilistic grasp planner: tested %zd grasps", response.grasps.size());
00226   response.error_code.value = response.error_code.SUCCESS;
00227   return true;
00228 }
00229 
00230 
00231 BayesianGraspPlannerNode::~BayesianGraspPlannerNode()
00232 {
00233 }
00234 
00235 } // end namespace
00236 
00237 int main(int argc, char** argv)
00238 {
00239   ros::init(argc, argv, "bayesian_grasp_planner_node");
00240   bayesian_grasp_planner::BayesianGraspPlannerNode node;
00241   ros::spin();
00242   return 0;
00243 }


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