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 }