test_table_objects.cpp
Go to the documentation of this file.
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 Willow Garage, Inc. 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  * $Id: table_objects.cpp 30899 2010-07-16 04:56:51Z rusu $
00035  *
00036  */
00037 
00042 #include <ros/ros.h>
00043 #include <sensor_msgs/PointCloud2.h>
00044 #include <visualization_msgs/Marker.h>
00045 #include <pcl/point_types.h>
00046 
00047 #include <table_objects/GetObjects.h>
00048 
00049 ros::ServiceClient client;
00050 ros::Publisher pub;
00051 ros::Publisher pub_marker;
00052 
00053 void callback (sensor_msgs::PointCloud2 cloud)
00054 {
00055   table_objects::GetObjects srv;
00056   srv.request.data = cloud;
00057   if (client.call(srv))
00058   {
00059     //for (std::vector<mapping_msgs::CollisionObject>::iterator it = srv.response.table_objects.begin (); it != srv.response.table_objects.end (); it++)
00060     for (size_t i = 0; i < srv.response.table_objects.size (); i++)
00061     {
00062       mapping_msgs::CollisionObject obj = srv.response.table_objects[i];
00063       //obj.header.stamp += ros::Duration (i * 0.001); // TODO is this hack of Radu's needed?
00064       pub.publish (obj);
00065 
00066       // publishing marker for checking
00067       visualization_msgs::Marker marker;
00068       marker.header = obj.header;
00069       marker.ns = "CollisionObjects";
00070       marker.id = i;
00071       marker.type = visualization_msgs::Marker::CUBE;
00072       marker.action = visualization_msgs::Marker::ADD;
00073       marker.pose.position = obj.poses[0].position;
00074       marker.pose.orientation = obj.poses[0].orientation;
00075       marker.scale.x = obj.shapes[0].dimensions[0];
00076       marker.scale.y = obj.shapes[0].dimensions[1];
00077       marker.scale.z = obj.shapes[0].dimensions[2];
00078       marker.color.a = 0.5;
00079       marker.color.r = 0.5;
00080       marker.color.g = 0.5;
00081       marker.color.b = 0.5;
00082       pub_marker.publish (marker);
00083     }
00084     ROS_INFO("Published %zu objects", srv.response.table_objects.size ());
00085   }
00086   else
00087     ROS_ERROR("Failed to call service!");
00088 }
00089 
00090 /* ---[ */
00091 int
00092   main (int argc, char** argv)
00093 {
00094   ros::init (argc, argv, "test_table_objects");
00095 
00096   ros::NodeHandle nh;
00097 
00098   client = nh.serviceClient<table_objects::GetObjects>("get_table_objects");
00099   
00100   pub = nh.advertise<mapping_msgs::CollisionObject> ("collison_objects", 50);
00101   pub_marker = nh.advertise<visualization_msgs::Marker> ("collison_object_markers", 50);
00102 
00103   ros::Subscriber sub = nh.subscribe("/extract_objects_indices/output", 5, callback);
00104 
00105   ros::spin ();
00106 
00107   return (0);
00108 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


table_objects
Author(s): Zoltan-Csaba Marton
autogenerated on Thu May 23 2013 10:18:16