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 <laser_cb_detector/laser_cb_detector.h>
00039 #include <sstream>
00040
00041 using namespace laser_cb_detector;
00042 using namespace std;
00043
00044
00045 #define ROS_INFO_CONFIG(name) \
00046 {\
00047 ostringstream ss;\
00048 ss << "[" << #name << "] -> " << config.name;\
00049 ROS_INFO(ss.str().c_str());\
00050 }
00051
00052
00053
00054 laser_cb_detector::ConfigGoal getParamConfig(ros::NodeHandle &n)
00055 {
00056 laser_cb_detector::ConfigGoal config;
00057
00058 ros::NodeHandle pn("~");
00059
00060 int num_x;
00061 pn.param("num_x", num_x, 3);
00062 config.num_x = (unsigned int) num_x;
00063
00064 int num_y;
00065 pn.param("num_y", num_y, 3);
00066 config.num_y = (unsigned int) num_y;
00067
00068 double spacing_x;
00069 pn.param("spacing_x", spacing_x, 1.0);
00070 config.spacing_x = spacing_x;
00071
00072 double spacing_y;
00073 pn.param("spacing_y", spacing_y, 1.0);
00074 config.spacing_y = spacing_y;
00075
00076 double width_scaling;
00077 pn.param("width_scaling", width_scaling, 1.0);
00078 config.width_scaling = width_scaling;
00079
00080 double height_scaling;
00081 pn.param("height_scaling", height_scaling, 1.0);
00082 config.height_scaling = height_scaling;
00083
00084 double min_intensity;
00085 pn.param("min_intensity", min_intensity, 500.0);
00086 config.min_intensity = min_intensity;
00087
00088 double max_intensity;
00089 pn.param("max_intensity", max_intensity, 5000.0);
00090 config.max_intensity = max_intensity;
00091
00092 int subpixel_window;
00093 pn.param("subpixel_window", subpixel_window, 2);
00094 config.subpixel_window = subpixel_window;
00095
00096 pn.param("subpixel_zero_zone", config.subpixel_zero_zone, -1);
00097
00098 int flip_horizontal;
00099 pn.param("flip_horizontal", flip_horizontal, 0);
00100 if (flip_horizontal)
00101 config.flip_horizontal = 1;
00102 else
00103 config.flip_horizontal = 0;
00104
00105 ROS_INFO_CONFIG(num_x);
00106 ROS_INFO_CONFIG(num_y);
00107 ROS_INFO_CONFIG(spacing_x);
00108 ROS_INFO_CONFIG(spacing_y);
00109 ROS_INFO_CONFIG(width_scaling);
00110 ROS_INFO_CONFIG(height_scaling);
00111 ROS_INFO_CONFIG(min_intensity);
00112 ROS_INFO_CONFIG(max_intensity);
00113 ROS_INFO_CONFIG(subpixel_window);
00114 ROS_INFO_CONFIG(subpixel_zero_zone);
00115
00116 ROS_INFO("[flip_horizontal]->%u", config.flip_horizontal);
00117
00118 return config;
00119 }
00120
00121 void snapshotCallback(ros::Publisher* pub, ros::Publisher* image_pub, LaserCbDetector* detector, const calibration_msgs::DenseLaserSnapshotConstPtr& msg)
00122 {
00123 bool detect_result;
00124 calibration_msgs::CalibrationPattern result;
00125 detect_result = detector->detect(*msg, result);
00126
00127 if (!detect_result)
00128 ROS_ERROR("Error during checkerboard detection. (This error is worse than simply not seeing a checkerboard");
00129 else
00130 {
00131 pub->publish(result);
00132 }
00133
00134 sensor_msgs::Image image;
00135 if(detector->getImage(*msg, image))
00136 {
00137 image.header.stamp = msg->header.stamp;
00138 image_pub->publish(image);
00139 }
00140 else
00141 ROS_ERROR("Error trying to generate ROS image");
00142 }
00143
00144 int main(int argc, char** argv)
00145 {
00146 ros::init(argc, argv, "laser_cb_detector");
00147
00148 ros::NodeHandle n;
00149
00150
00151 laser_cb_detector::ConfigGoal config = getParamConfig(n);
00152 LaserCbDetector detector;
00153 detector.configure(config);
00154
00155
00156 ros::Publisher pub = n.advertise<calibration_msgs::CalibrationPattern>("laser_checkerboard", 1);
00157 ros::Publisher image_pub = n.advertise<sensor_msgs::Image>("image", 1);
00158
00159
00160 boost::function<void (const calibration_msgs::DenseLaserSnapshotConstPtr&)> cb
00161 = boost::bind(&snapshotCallback, &pub, &image_pub, &detector, _1);
00162
00163 ros::Subscriber sub = n.subscribe(std::string("snapshot"), 1, cb);
00164
00165 ros::spin();
00166 }