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 <boost/thread.hpp>
00038 #include <ros/console.h>
00039 #include <ros/ros.h>
00040 #include <actionlib/server/simple_action_server.h>
00041 #include <image_cb_detector/image_cb_detector.h>
00042 #include <image_transport/image_transport.h>
00043 #include <sensor_msgs/Image.h>
00044 #include <image_cb_detector/ConfigAction.h>
00045 #include <calibration_msgs/Interval.h>
00046
00047 using namespace image_cb_detector;
00048
00049 class ImageCbDetectorAction
00050 {
00051 public:
00052 ImageCbDetectorAction() : as_("cb_detector_config", false), it_(nh_)
00053 {
00054 as_.registerGoalCallback( boost::bind(&ImageCbDetectorAction::goalCallback, this) );
00055 as_.registerPreemptCallback( boost::bind(&ImageCbDetectorAction::preemptCallback, this) );
00056
00057 pub_ = nh_.advertise<calibration_msgs::CalibrationPattern>("features",1);
00058 sub_ = it_.subscribe("image", 2, boost::bind(&ImageCbDetectorAction::imageCallback, this, _1));
00059 as_.start();
00060 }
00061
00062 void goalCallback()
00063 {
00064 boost::mutex::scoped_lock lock(run_mutex_);
00065
00066
00067 if (as_.isActive())
00068 as_.setPreempted();
00069
00070
00071 image_cb_detector::ConfigGoalConstPtr goal = as_.acceptNewGoal();
00072 assert(goal);
00073
00074
00075 bool success = detector_.configure(*goal);
00076
00077
00078 if (!success)
00079 as_.setAborted();
00080 }
00081
00082 void preemptCallback()
00083 {
00084 boost::mutex::scoped_lock lock(run_mutex_);
00085
00086
00087 as_.setPreempted();
00088 }
00089
00090 void imageCallback(const sensor_msgs::ImageConstPtr& image)
00091 {
00092 boost::mutex::scoped_lock lock(run_mutex_);
00093
00094 if (as_.isActive())
00095 {
00096 calibration_msgs::CalibrationPattern features;
00097 bool success;
00098 success = detector_.detect(image, features);
00099
00100 if (!success)
00101 {
00102 ROS_ERROR("Error trying to detect checkerboard, not going to publish CalibrationPattern");
00103 return;
00104 }
00105
00106 pub_.publish(features);
00107 }
00108 }
00109
00110 private:
00111 boost::mutex run_mutex_;
00112 ros::NodeHandle nh_;
00113 actionlib::SimpleActionServer<image_cb_detector::ConfigAction> as_;
00114 ImageCbDetector detector_;
00115
00116 ros::Publisher pub_;
00117 image_transport::ImageTransport it_;
00118 image_transport::Subscriber sub_;
00119
00120 };
00121
00122 int main(int argc, char** argv)
00123 {
00124 ros::init(argc, argv, "image_cb_detector_action");
00125 ros::NodeHandle n;
00126 ImageCbDetectorAction detector_action;
00127 ros::spin();
00128 return 0;
00129 }