Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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 }