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