ping_tabletop_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/TabletopSegmentation.h"
00043 #include "tabletop_object_detector/TabletopObjectRecognition.h"
00044 
00046 int main(int argc, char **argv)
00047 {
00048   ros::init(argc, argv, "ping_tabletop_node");
00049   ros::NodeHandle nh;
00050 
00051   std::string service_name("/tabletop_segmentation");
00052   while ( !ros::service::waitForService(service_name, ros::Duration().fromSec(3.0)) && nh.ok() )
00053   {
00054     ROS_INFO("Waiting for service %s...", service_name.c_str());
00055   }
00056   if (!nh.ok()) exit(0);
00057 
00058   tabletop_object_detector::TabletopSegmentation segmentation_srv;
00059   if (!ros::service::call(service_name, segmentation_srv))
00060   {
00061     ROS_ERROR("Call to segmentation service failed");
00062     exit(0);
00063   }
00064   if (segmentation_srv.response.result != segmentation_srv.response.SUCCESS)
00065   {
00066     ROS_ERROR("Segmentation service returned error %d", segmentation_srv.response.result);
00067     exit(0);
00068   }
00069   ROS_INFO("Segmentation service succeeded. Detected %d clusters", (int)segmentation_srv.response.clusters.size());
00070   if (segmentation_srv.response.clusters.empty()) exit(0);
00071 
00072   //also test segmenting with input table
00073   sleep(0.5);
00074   segmentation_srv.request.table = segmentation_srv.response.table;
00075   ROS_INFO("Re-segmenting with the same table shifted 10 cm closer to the robot");
00076   for(size_t i=0; i<segmentation_srv.request.table.convex_hull.vertices.size(); i++)
00077   {
00078     segmentation_srv.request.table.convex_hull.vertices[i].x -= .10;
00079   }
00080   if (!ros::service::call(service_name, segmentation_srv))
00081   {
00082     ROS_ERROR("Call to segmentation service failed");
00083     exit(0);
00084   }
00085   if (segmentation_srv.response.result != segmentation_srv.response.SUCCESS)
00086   {
00087     ROS_ERROR("Segmentation service returned error %d", segmentation_srv.response.result);
00088     exit(0);
00089   }
00090   ROS_INFO("Segmentation service with table input succeeded. Detected %d clusters", (int)segmentation_srv.response.clusters.size());
00091 
00092   service_name = "/tabletop_object_recognition";
00093   if ( !ros::service::waitForService(service_name, ros::Duration().fromSec(3.0)) )
00094   {
00095     ROS_INFO("Recognition service %s is not available", service_name.c_str());
00096     exit(0);
00097   }
00098 
00099   tabletop_object_detector::TabletopObjectRecognition recognition_srv;
00100   recognition_srv.request.table = segmentation_srv.response.table;
00101   recognition_srv.request.clusters = segmentation_srv.response.clusters;
00102   recognition_srv.request.num_models = 5;
00103   if (!ros::service::call(service_name, recognition_srv))
00104   {
00105     ROS_ERROR("Call to recognition service failed");
00106     exit(0);
00107   }
00108 
00109   ROS_INFO("Recognition results:");
00110   for (size_t i=0; i<recognition_srv.response.models.size(); i++)
00111   {
00112     if (recognition_srv.response.models[i].model_list.empty())
00113     {
00114       ROS_INFO("  Unidentifiable cluster");
00115     }
00116     else
00117     {
00118       ROS_INFO("  Model id %d",recognition_srv.response.models[i].model_list[0].model_id);
00119     }
00120   }
00121 
00122   ROS_INFO("Clusters correspond to following recognition results:");
00123   for (size_t i=0; i<recognition_srv.response.cluster_model_indices.size(); i++)
00124   {
00125     ROS_INFO("  Cluster %u: recognition result %d", (unsigned int)i, 
00126              recognition_srv.response.cluster_model_indices.at(i));
00127   }
00128   return true;
00129 }


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