Go to the documentation of this file.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/console.h>
00038 #include <ros/ros.h>
00039 #include <laser_cb_detector/laser_cb_detector.h>
00040 #include <sstream>
00041 #include <actionlib/server/simple_action_server.h>
00042
00043 using namespace laser_cb_detector;
00044 using namespace std;
00045
00046
00047 #define ROS_INFO_CONFIG(name) \
00048 {\
00049 ostringstream ss;\
00050 ss << "[" << #name << "] -> " << config.name;\
00051 ROS_INFO("%s", ss.str().c_str());\
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 class LaserCbDetectorAction {
00122 public:
00123 LaserCbDetectorAction() : as_("cb_detector_config", false) {
00124
00125 laser_cb_detector::ConfigGoal config = getParamConfig(nh_);
00126 detector_.configure(config);
00127
00128
00129 as_.registerGoalCallback( boost::bind(&LaserCbDetectorAction::goalCallback, this) );
00130 as_.registerPreemptCallback( boost::bind(&LaserCbDetectorAction::preemptCallback, this) );
00131
00132
00133 pub_ = nh_.advertise<calibration_msgs::CalibrationPattern>("laser_checkerboard", 1);
00134
00135 image_pub_ = nh_.advertise<sensor_msgs::Image>("image", 1);
00136
00137
00138 sub_ = nh_.subscribe("snapshot", 1, &LaserCbDetectorAction::snapshotCallback, this );
00139
00140
00141 as_.start();
00142 }
00143
00144 void goalCallback() {
00145 boost::mutex::scoped_lock lock(run_mutex_);
00146
00147 if(as_.isActive()) {
00148 as_.setPreempted();
00149 }
00150
00151 laser_cb_detector::ConfigGoalConstPtr goal = as_.acceptNewGoal();
00152
00153 bool success = detector_.configure(*goal);
00154
00155 if( !success ) {
00156 as_.setAborted();
00157 }
00158 }
00159
00160 void preemptCallback() {
00161 boost::mutex::scoped_lock lock(run_mutex_);
00162
00163 as_.setPreempted();
00164 }
00165
00166 void snapshotCallback(const calibration_msgs::DenseLaserSnapshotConstPtr& msg)
00167 {
00168 boost::mutex::scoped_lock lock(run_mutex_);
00169
00170 bool detect_result;
00171 calibration_msgs::CalibrationPattern result;
00172 detect_result = detector_.detect(*msg, result);
00173
00174 if (!detect_result)
00175 ROS_ERROR("Error during checkerboard detection. (This error is worse than simply not seeing a checkerboard");
00176 else
00177 {
00178 result.header.stamp = msg->header.stamp;
00179 pub_.publish(result);
00180 }
00181
00182 sensor_msgs::Image image;
00183 if(detector_.getImage(*msg, image))
00184 {
00185 image.header.stamp = msg->header.stamp;
00186 image_pub_.publish(image);
00187 }
00188 else
00189 ROS_ERROR("Error trying to generate ROS image");
00190 }
00191
00192 private:
00193 boost::mutex run_mutex_;
00194 ros::NodeHandle nh_;
00195 ros::Publisher pub_;
00196 ros::Publisher image_pub_;
00197 ros::Subscriber sub_;
00198 LaserCbDetector detector_;
00199 actionlib::SimpleActionServer<laser_cb_detector::ConfigAction> as_;
00200 };
00201
00202
00203 int main(int argc, char** argv)
00204 {
00205 ros::init(argc, argv, "laser_cb_detector");
00206 LaserCbDetectorAction detector_action;
00207 ros::spin();
00208 return 0;
00209 }