$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, 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 the Willow Garage 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 00036 00037 #include <ros/ros.h> 00038 #include "sensor_msgs/Image.h" 00039 #include "image_transport/image_transport.h" 00040 #include "cv_bridge/CvBridge.h" 00041 #include <opencv/cv.h> 00042 #include <opencv/highgui.h> 00043 #include <image_cb_detector/image_cb_detector_old.h> 00044 #include <sstream> 00045 00046 using namespace image_cb_detector; 00047 using namespace std; 00048 00049 #define ROS_INFO_CONFIG(name) \ 00050 {\ 00051 ostringstream ss;\ 00052 ss << "[" << #name << "] -> " << config.name;\ 00053 ROS_INFO(ss.str().c_str());\ 00054 } 00055 00056 00057 00058 image_cb_detector::ConfigGoal getParamConfig(ros::NodeHandle &n) 00059 { 00060 image_cb_detector::ConfigGoal config; 00061 00062 int num_x; 00063 n.param("~num_x", num_x, 3); 00064 config.num_x = (unsigned int) num_x; 00065 00066 int num_y; 00067 n.param("~num_y", num_y, 3); 00068 config.num_y = (unsigned int) num_y; 00069 00070 double spacing_x; 00071 n.param("~spacing_x", spacing_x, 1.0); 00072 config.spacing_x = spacing_x; 00073 00074 double spacing_y; 00075 n.param("~spacing_y", spacing_y, 1.0); 00076 config.spacing_y = spacing_y; 00077 00078 double width_scaling; 00079 n.param("~width_scaling", width_scaling, 1.0); 00080 config.width_scaling = width_scaling; 00081 00082 double height_scaling; 00083 n.param("~height_scaling", height_scaling, 1.0); 00084 config.height_scaling = height_scaling; 00085 00086 int subpixel_window; 00087 n.param("~subpixel_window", subpixel_window, 2); 00088 config.subpixel_window = subpixel_window; 00089 00090 n.param("~subpixel_zero_zone", config.subpixel_zero_zone, -1); 00091 00092 ROS_INFO_CONFIG(num_x); 00093 ROS_INFO_CONFIG(num_y); 00094 ROS_INFO_CONFIG(spacing_x); 00095 ROS_INFO_CONFIG(spacing_y); 00096 ROS_INFO_CONFIG(width_scaling); 00097 ROS_INFO_CONFIG(height_scaling); 00098 ROS_INFO_CONFIG(subpixel_window); 00099 ROS_INFO_CONFIG(subpixel_zero_zone); 00100 00101 return config; 00102 } 00103 00104 void imageCallback(ros::Publisher* pub, ImageCbDetectorOld* detector, const sensor_msgs::ImageConstPtr& image) 00105 { 00106 calibration_msgs::CalibrationPattern features; 00107 bool success; 00108 success = detector->detect(image, features); 00109 00110 if (!success) 00111 { 00112 ROS_ERROR("Error trying to detect checkerboard, not going to publish CalibrationPattern"); 00113 return; 00114 } 00115 pub->publish(features); 00116 } 00117 00118 int main(int argc, char** argv) 00119 { 00120 ros::init(argc, argv, "image_converter"); 00121 ros::NodeHandle nh; 00122 00123 ImageCbDetectorOld detector; 00124 00125 detector.configure(getParamConfig(nh)); 00126 00127 image_transport::ImageTransport it_(nh); 00128 ros::Publisher pub = nh.advertise<calibration_msgs::CalibrationPattern>("features",1); 00129 image_transport::Subscriber sub = it_.subscribe("image", 2, boost::bind(&imageCallback, &pub, &detector, _1)); 00130 00131 ros::MultiThreadedSpinner spinner(1); 00132 spinner.spin(); 00133 //ros::spin(); 00134 return 0; 00135 }