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
  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 #include <vision_msgs/cop_answer.h>
  50 
  51 ros::ServiceClient client;
  52 ros::Publisher pub;
  53 ros::Publisher pub_marker;
  54 ros::Publisher tabletop_pub;
  55 
  56 //for cop_answer
  57 std::string cop_descriptor_name;
  58 std::string aposteriori_position_name;
  59 std::stringstream ss;
  60 int object_id = 700000;
  61 
  62 void callback (sensor_msgs::PointCloud2 cloud)
  63 {
  64   vision_msgs::cop_answer msg;
  65   table_objects::GetObjects srv;
  66   srv.request.data = cloud;
  67   if (client.call(srv))
  68     {
  69       //for (std::vector<mapping_msgs::CollisionObject>::iterator it = srv.response.table_objects.begin (); it != srv.response.table_objects.end (); it++)
  70       for (size_t i = 0; i < srv.response.table_objects.size (); i++)
  71 	{
  72 	  mapping_msgs::CollisionObject obj = srv.response.table_objects[i];
  73 	  pub.publish (obj);
  74 	  
  75 	  ss << i;
  76 	  cop_descriptor_name = "cop_descriptor" + ss.str();
  77 	  aposteriori_position_name = "aposteriori_position" + ss.str();
  78 	  vision_msgs::cop_descriptor cop_descriptor_name;
  79 	  vision_msgs::aposteriori_position aposteriori_position_name;
  80 	  msg.found_poses.push_back(aposteriori_position_name);
  81 	  msg.found_poses[i].objectId = i;
  82 	  msg.found_poses[i].models.push_back(cop_descriptor_name);	  
  83 	  msg.found_poses[i].models[0].type = "ODUFinder";
  84 	  msg.found_poses[i].models[0].sem_class = obj.id;
  85 	  msg.found_poses[i].models[0].object_id = ++object_id;
  86 	  msg.found_poses[i].position = 0;
  87 
  88 	  // publishing marker for checking
  89 	  visualization_msgs::Marker marker;
  90 	  marker.header = obj.header;
  91 	  marker.ns = "CollisionObjects";
  92 	  marker.id = i;
  93 	  marker.type = visualization_msgs::Marker::CUBE;
  94 	  marker.action = visualization_msgs::Marker::ADD;
  95 	  marker.pose.position = obj.poses[0].position;
  96 	  marker.pose.orientation = obj.poses[0].orientation;
  97 	  marker.scale.x = obj.shapes[0].dimensions[0];
  98 	  marker.scale.y = obj.shapes[0].dimensions[1];
  99 	  marker.scale.z = obj.shapes[0].dimensions[2];
 100 	  marker.color.a = 0.5;
 101 	  marker.color.r = 0.5;
 102 	  marker.color.g = 0.5;
 103 	  marker.color.b = 0.5;
 104 	  pub_marker.publish (marker);
 105 	}
 106       ROS_INFO("Published %zu objects", srv.response.table_objects.size ());
 107     }
 108   else
 109     {
 110       ROS_ERROR("Failed to call service!");
 111     }
 112   tabletop_pub.publish(msg);
 113 }
 114 
 115 /* ---[ */
 116 int
 117   main (int argc, char** argv)
 118 {
 119   ros::init (argc, argv, "synthetic_percepts");
 120 
 121   ros::NodeHandle nh;
 122 
 123   client = nh.serviceClient<table_objects::GetObjects>("get_table_objects");
 124   
 125   pub = nh.advertise<mapping_msgs::CollisionObject> ("collison_objects", 50);
 126   pub_marker = nh.advertise<visualization_msgs::Marker> ("collison_object_markers", 50);
 127   tabletop_pub = nh.advertise<vision_msgs::cop_answer>("/synthetic_percepts/tabletop_percepts",10);
 128 
 129   ros::Subscriber sub = nh.subscribe("/extract_objects_indices/output", 5, callback);
 130 
 131   ros::spin ();
 132 
 133   return (0);
 134 }