$search
00001 /* 00002 * Copyright (c) 2009, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #include <ros/ros.h> 00031 00032 #include <actionlib/client/simple_action_client.h> 00033 00034 #include <sensor_msgs/Image.h> 00035 #include <sensor_msgs/CameraInfo.h> 00036 #include <sensor_msgs/PointCloud2.h> 00037 00038 #include <stereo_msgs/DisparityImage.h> 00039 00040 #include "object_segmentation_gui/ObjectSegmentationGuiAction.h" 00041 #include <rgbd_assembler/RgbdAssembly.h> 00042 00043 template <class ServiceType> 00044 bool connectService( ros::NodeHandle &nh, 00045 ros::ServiceClient& service_client, 00046 std::string topic ) 00047 { 00048 while ( !ros::service::waitForService(topic, ros::Duration(2.0)) && nh.ok() ) 00049 { 00050 ROS_INFO_STREAM("Waiting for '" << topic << "' service to come up.."); 00051 } 00052 service_client = nh.serviceClient<ServiceType>(topic, true); 00053 return true; 00054 } 00055 00056 int main(int argc, char **argv) 00057 { 00058 ros::init(argc, argv, "ping_object_segmentation_gui"); 00059 ros::NodeHandle nh("~"); 00060 std::string topic("/segmentation_popup"); 00061 00062 actionlib::SimpleActionClient<object_segmentation_gui::ObjectSegmentationGuiAction> client(topic, true); 00063 while(!client.waitForServer(ros::Duration(2.0)) && nh.ok()) 00064 { 00065 ROS_INFO("Waiting for action client on topic %s", topic.c_str()); 00066 } 00067 00068 ros::ServiceClient rgbd_assembler_client_; 00069 rgbd_assembler::RgbdAssembly rgbd_assembler_srv_; 00070 00071 ROS_INFO("Connecting service.."); 00072 00073 if ( !connectService<rgbd_assembler::RgbdAssembly>( nh, 00074 rgbd_assembler_client_, 00075 "/rgbd_assembly" ) ) 00076 return -1; 00077 00078 while (nh.ok()) 00079 { 00080 if (!rgbd_assembler_client_.call(rgbd_assembler_srv_)) 00081 { 00082 ROS_ERROR("Call to rgbd assembler service failed"); 00083 return false; 00084 } 00085 00086 if (rgbd_assembler_srv_.response.result != 00087 rgbd_assembler_srv_.response.SUCCESS) 00088 { 00089 ROS_ERROR( "RGB-D Assembler service returned error " ); 00090 return false; 00091 } 00092 00093 00094 object_segmentation_gui::ObjectSegmentationGuiGoal os_gui_goal; 00095 os_gui_goal.image = 00096 rgbd_assembler_srv_.response.image; 00097 os_gui_goal.disparity_image = 00098 rgbd_assembler_srv_.response.disparity_image; 00099 os_gui_goal.camera_info = 00100 rgbd_assembler_srv_.response.camera_info; 00101 os_gui_goal.point_cloud = 00102 rgbd_assembler_srv_.response.point_cloud; 00103 00104 client.sendGoal(os_gui_goal); 00105 while (!client.waitForResult(ros::Duration(0.5)) && nh.ok()) 00106 { 00107 } 00108 if (!nh.ok()) 00109 { 00110 client.cancelGoal(); 00111 break; 00112 } 00113 00114 if (client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) 00115 { 00116 ROS_INFO("The object segmentation action has not succeeded;"); 00117 } 00118 else 00119 { 00120 ROS_INFO("Object Segmentation has succeeded;"); 00121 } 00122 ros::Duration(3.0).sleep(); 00123 } 00124 }