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
00031
00032
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
00134 return 0;
00135 }