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