tabletop_complete_node.cpp
Go to the documentation of this file.
00001 
00002 /*********************************************************************
00003 *
00004 *  Copyright (c) 2009, 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): Matei Ciocarlie
00036 
00037 #include <ros/ros.h>
00038 
00039 #include <string>
00040 #include <vector>
00041 
00042 #include "tabletop_object_detector/TabletopDetection.h"
00043 #include "tabletop_object_detector/TabletopSegmentation.h"
00044 #include "tabletop_object_detector/TabletopObjectRecognition.h"
00045 
00046 namespace tabletop_object_detector {
00047 
00048 class TabletopCompleteNode
00049 {
00050 private:
00051   ros::NodeHandle nh_;
00052   ros::NodeHandle priv_nh_;
00053 
00054   ros::ServiceClient seg_srv_;
00055   ros::ServiceClient rec_srv_;
00056   ros::ServiceServer complete_srv_;
00057   
00059   bool perform_fit_merge_;
00060 
00061   bool serviceCallback(TabletopDetection::Request &request, TabletopDetection::Response &response);
00062 
00063 public:
00064   TabletopCompleteNode();
00065 };
00066 
00067 TabletopCompleteNode::TabletopCompleteNode() : nh_(""), priv_nh_("~")
00068 {
00069   std::string service_name;
00070 
00071   priv_nh_.param<std::string>("segmentation_srv", service_name, "/tabletop_segmentation");
00072   while ( !ros::service::waitForService(service_name, ros::Duration(2.0)) && nh_.ok() ) 
00073   {
00074     ROS_INFO("Waiting for %s service to come up", service_name.c_str());
00075   }
00076   if (!nh_.ok()) exit(0);
00077   seg_srv_ = nh_.serviceClient<TabletopSegmentation>(service_name, true);
00078 
00079   priv_nh_.param<std::string>("recognition_srv", service_name, "/tabletop_object_recognition");
00080   while ( !ros::service::waitForService(service_name, ros::Duration(2.0)) && nh_.ok() ) 
00081   {
00082     ROS_INFO("Waiting for %s service to come up", service_name.c_str());
00083   }
00084   if (!nh_.ok()) exit(0);
00085   rec_srv_ = nh_.serviceClient<TabletopObjectRecognition>(service_name, true);
00086   
00087   complete_srv_ = nh_.advertiseService("object_detection", &TabletopCompleteNode::serviceCallback, this);
00088   ROS_INFO("Tabletop complete node ready");
00089 
00090   priv_nh_.param<bool>("perform_fit_merge", perform_fit_merge_, true);
00091 }
00092 
00093 bool TabletopCompleteNode::serviceCallback(TabletopDetection::Request &request, TabletopDetection::Response &response)
00094 {
00095   tabletop_object_detector::TabletopSegmentation segmentation_srv;
00096   if (!seg_srv_.call(segmentation_srv))
00097   {
00098     ROS_ERROR("Call to segmentation service failed");
00099     response.detection.result = response.detection.OTHER_ERROR;
00100     return true;
00101   }
00102   response.detection.result = segmentation_srv.response.result;
00103   if (segmentation_srv.response.result != segmentation_srv.response.SUCCESS)
00104   {
00105     ROS_ERROR("Segmentation service returned error %d", segmentation_srv.response.result);
00106     return true;
00107   }
00108   ROS_INFO("Segmentation service succeeded. Detected %d clusters", (int)segmentation_srv.response.clusters.size());
00109   response.detection.table = segmentation_srv.response.table;
00110   response.detection.clusters = segmentation_srv.response.clusters;
00111   if (segmentation_srv.response.clusters.empty() || !request.return_models) return true;
00112 
00113   tabletop_object_detector::TabletopObjectRecognition recognition_srv;
00114   recognition_srv.request.table = segmentation_srv.response.table;
00115   recognition_srv.request.clusters = segmentation_srv.response.clusters;
00116   recognition_srv.request.num_models = request.num_models;
00117   recognition_srv.request.perform_fit_merge = perform_fit_merge_;
00118   if (!rec_srv_.call(recognition_srv))
00119   {
00120     ROS_ERROR("Call to recognition service failed");
00121     response.detection.result = response.detection.OTHER_ERROR;
00122     return true;
00123   }
00124   response.detection.models = recognition_srv.response.models;
00125   response.detection.cluster_model_indices = recognition_srv.response.cluster_model_indices;
00126   return true;
00127 } 
00128 
00129 }
00130 
00131 int main(int argc, char **argv)
00132 {
00133   ros::init(argc, argv, "tabletop_node");
00134   ros::NodeHandle nh;
00135   tabletop_object_detector::TabletopCompleteNode node;
00136   ros::spin();
00137   return true;
00138 }


tabletop_object_detector
Author(s): Marius Muja and Matei Ciocarlie
autogenerated on Fri Jan 3 2014 11:48:47