1 /*
   2  * Software License Agreement (BSD License)
   3  *
   4  *  Copyright (c) 2010, Willow Garage, Inc.
   5  *  All rights reserved.
   6  *
   7  *  Redistribution and use in source and binary forms, with or without
   8  *  modification, are permitted provided that the following conditions
   9  *  are met:
  10  *
  11  *   * Redistributions of source code must retain the above copyright
  12  *     notice, this list of conditions and the following disclaimer.
  13  *   * Redistributions in binary form must reproduce the above
  14  *     copyright notice, this list of conditions and the following
  15  *     disclaimer in the documentation and/or other materials provided
  16  *     with the distribution.
  17  *   * Neither the name of Willow Garage, Inc. nor the names of its
  18  *     contributors may be used to endorse or promote products derived
  19  *     from this software without specific prior written permission.
  20  *
  21  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  22  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  23  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  24  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  25  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  26  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  27  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  28  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  29  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  30  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  31  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  32  *  POSSIBILITY OF SUCH DAMAGE.
  33  *
  34  * $Id: table_objects.cpp 30899 2010-07-16 04:56:51Z rusu $
  35  *
  36  */
  37 
  38 /**
  39 \author Zoltan-Csaba Marton, Dejan Pangercic
  40  **/
  41 
  42 #include <ros/ros.h>
  43 #include <sensor_msgs/PointCloud2.h>
  44 #include <visualization_msgs/Marker.h>
  45 #include <pcl/point_types.h>
  46 
  47 #include <table_objects/GetObjects.h>
  48 
  49 ros::ServiceClient client;
  50 ros::Publisher pub;
  51 ros::Publisher pub_marker;
  52 ros::Publisher tabletop_pub;
  53 
  54 std::string cop_descriptor_name;
  55 std::string aposteriori_position_name;
  56 std::stringstream ss;
  57 int object_id = 700000;
  58 
  59 
  60 void callback (sensor_msgs::PointCloud2 cloud)
  61 {
  62   vision_msgs::cop_answer msg;
  63   //TODO:
  64   // call service and publish results on topic "/synthetic_percepts/tabletop_percepts"
  65 
  66   if ()
  67     {
  68       ///TODO: You'll need a "for" loop and following inside it
  69       ss << i;
  70       cop_descriptor_name = "cop_descriptor" + ss.str();
  71       aposteriori_position_name = "aposteriori_position" + ss.str();
  72       vision_msgs::cop_descriptor cop_descriptor_name;
  73       vision_msgs::aposteriori_position aposteriori_position_name;
  74       msg.found_poses.push_back(aposteriori_position_name);
  75       msg.found_poses[i].objectId = i;
  76       msg.found_poses[i].models.push_back(cop_descriptor_name);	  
  77       msg.found_poses[i].models[0].type = "ODUFinder";
  78       msg.found_poses[i].models[0].sem_class = /*TODO: assign object type*/;
  79       msg.found_poses[i].models[0].object_id = ++object_id;
  80       msg.found_poses[i].position = 0;
  81     }
  82   else
  83     {
  84       ROS_ERROR("Failed to call service!");
  85     }
  86   tabletop_pub.publish(msg);
  87 }
  88 
  89 /* ---[ */
  90 int
  91   main (int argc, char** argv)
  92 {
  93   ros::init (argc, argv, "test_table_objects");
  94 
  95   ros::NodeHandle nh;
  96 
  97   //TODO: 
  98   //instantiate a service client
  99   //  service name: "get_table_objects"
 100   //  service type: "table_objects::GetObjects"
 101   
 102   
 103   //TODO:
 104   //subscribe to the points supported by the plane
 105   //  topic name: "/extract_objects_indices/output"
 106 
 107   //TODO: 
 108   //advertise a topic with name "/synthetic_percepts/tabletop_percepts" and the message type "vision_msgs::cop_answer".
 109   ros::spin ();
 110   return (0);
 111 }