Go to the documentation of this file.00001 #include <ros/ros.h>
00002
00003 #include <opencv/cv.h>
00004 #include <opencv/highgui.h>
00005 #include <sensor_msgs/Image.h>
00006 #include <cv_bridge/CvBridge.h>
00007 #include <image_transport/image_transport.h>
00008
00009
00010 #include <face_contour_detector/GetFilters.h>
00011 #include <face_contour_detector/ApplyFilters.h>
00012
00013 #include <cstdlib>
00014
00015
00016 #include <sstream>
00017
00018 int main(int argc, char **argv)
00019 {
00020 ros::init(argc, argv, "test_filter_services");
00021
00022 if (argc < 2) {
00023 ROS_ERROR("there must be given a image as an argument");
00024 }
00025
00026 ros::NodeHandle n;
00027 ros::ServiceClient client = n.serviceClient<face_contour_detector::GetFilters>("get_filters");
00028 face_contour_detector::GetFilters srv;
00029 if (client.call(srv))
00030 {
00031 std::vector<face_contour_detector::filter>::iterator filterIt;
00032 for (filterIt = srv.response.filters.begin(); filterIt != srv.response.filters.end(); filterIt++) {
00033 ROS_INFO(filterIt->name.c_str());
00034 std::vector<face_contour_detector::parameter>::iterator paramIt;
00035 for (paramIt = filterIt->parameters.begin(); paramIt != filterIt->parameters.end(); paramIt++) {
00036 std::stringstream s;
00037 s<<"-->"<<paramIt->name<<":"<<paramIt->type<<" ["<<paramIt->ruler_minimum<<", "<<paramIt->ruler_maximum<<"]";
00038
00039 ROS_INFO(s.str().c_str());
00040 }
00041 }
00042 }
00043 else
00044 {
00045 ROS_ERROR("Failed to call service get_filters");
00046 return 1;
00047 }
00048
00049 ROS_INFO("Sending filter request");
00050
00051 face_contour_detector::ApplyFilters applySrv;
00052 cv::Mat img = cv::imread(argv[1]);
00053
00054 try {
00055 sensor_msgs::CvBridge cvBridge;
00056 IplImage ipl = img;
00057 applySrv.request.input_image = *(cvBridge.cvToImgMsg(&ipl));
00058 } catch (sensor_msgs::CvBridgeException error) {
00059 ROS_ERROR("could not convert the image with the cvBridge to a sensor_msgs/Image");
00060 return 1;
00061 }
00062
00063
00064 face_contour_detector::name_value_pair param;
00065 face_contour_detector::filter_area_setup areaSetup;
00066 areaSetup.area.x = 0;
00067 areaSetup.area.y = 0;
00068 areaSetup.area.width = img.cols;
00069 areaSetup.area.height = img.rows;
00070
00071 face_contour_detector::filter_setup filterSetup;
00072 filterSetup.name = "GaussianBlur";
00073 areaSetup.filters.push_back(filterSetup);
00074 filterSetup.name = "Canny";
00075 areaSetup.filters.push_back(filterSetup);
00076
00077 applySrv.request.areas.push_back(areaSetup);
00078
00079
00080 areaSetup.area.x = 0;
00081 areaSetup.area.y = 0;
00082 areaSetup.area.width = img.cols/2;
00083 areaSetup.area.height = img.rows/2;
00084
00085 filterSetup.name = "GaussianBlur";
00086 areaSetup.filters.push_back(filterSetup);
00087 filterSetup.name = "Canny";
00088 param.name = "threshold1";
00089 param.type = "float";
00090 param.value = "1000";
00091 filterSetup.parameters.push_back(param);
00092 param.name = "threshold2";
00093 param.type = "float";
00094 param.value = "2000";
00095 filterSetup.parameters.push_back(param);
00096 areaSetup.filters.push_back(filterSetup);
00097
00098 applySrv.request.areas.push_back(areaSetup);
00099
00100 ros::ServiceClient applyClient = n.serviceClient<face_contour_detector::ApplyFilters>("apply_filters");
00101
00102 if (applyClient.call(applySrv)) {
00103 try {
00104 sensor_msgs::CvBridge cvBridge;
00105 cv::Mat result;
00106 if (!cvBridge.fromImage(applySrv.response.result_image))
00107 throw sensor_msgs::CvBridgeException("Conversion to OpenCV image failed");
00108 result = cvBridge.toIpl();
00109 cv::imwrite("test_filter_result_image.png", result);
00110
00111 } catch (sensor_msgs::CvBridgeException error) {
00112 ROS_ERROR("error converting image message");
00113 }
00114 std::vector<face_contour_detector::filter_area_result>::iterator areaIt;
00115 for (areaIt = applySrv.response.area_results.begin(); areaIt != applySrv.response.area_results.end(); areaIt++) {
00116
00117 std::stringstream s;
00118 s<<"area(x="<<areaIt->area.x<<", y="<<areaIt->area.y<<", width="<<areaIt->area.width<<", height="<<areaIt->area.height<<")";
00119 ROS_INFO(s.str().c_str());
00120
00121 std::vector<face_contour_detector::name_value_pair>::iterator propIt;
00122 for (propIt = areaIt->result_properties.begin(); propIt != areaIt->result_properties.end(); propIt++) {
00123 std::stringstream s2;
00124 s2<<"--> "<<propIt->name<<" : "<<propIt->type<<" = "<<propIt->value;
00125 ROS_INFO(s2.str().c_str());
00126 }
00127
00128 }
00129 ROS_INFO("success");
00130 } else
00131 {
00132 ROS_ERROR("Failed to call service apply_filters");
00133 return 1;
00134 }
00135
00136 return 0;
00137 }