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
00043 #include <sensor_msgs/Image.h>
00044 #include <sensor_msgs/PointCloud2.h>
00045 #include <image_cb_detector/ConfigAction.h>
00046 #include <calibration_msgs/Interval.h>
00047
00048 #include <pcl/point_types.h>
00049 #include <pcl_ros/point_cloud.h>
00050
00051 #include <message_filters/subscriber.h>
00052 #include <message_filters/synchronizer.h>
00053 #include <message_filters/sync_policies/approximate_time.h>
00054
00055 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> CameraSyncPolicy;
00056
00057 class RgbdCbDetectorAction
00058 {
00059 public:
00060 RgbdCbDetectorAction(ros::NodeHandle & n) : nh_(n),
00061 as_("cb_detector_config", false),
00062 image_sub_ (nh_, "image", 3),
00063 cloud_sub_(nh_, "points", 3),
00064 sync_(CameraSyncPolicy(10), image_sub_, cloud_sub_)
00065
00066 {
00067 as_.registerGoalCallback( boost::bind(&RgbdCbDetectorAction::goalCallback, this) );
00068 as_.registerPreemptCallback( boost::bind(&RgbdCbDetectorAction::preemptCallback, this) );
00069
00070 pub_ = nh_.advertise<calibration_msgs::CalibrationPattern>("features",1);
00071 sync_.registerCallback(boost::bind(&RgbdCbDetectorAction::cameraCallback, this, _1, _2));
00072
00073 as_.start();
00074 }
00075
00076 void goalCallback()
00077 {
00078 boost::mutex::scoped_lock lock(run_mutex_);
00079
00080
00081 if (as_.isActive())
00082 as_.setPreempted();
00083
00084
00085 image_cb_detector::ConfigGoalConstPtr goal = as_.acceptNewGoal();
00086 assert(goal);
00087
00088
00089 bool success = detector_.configure(*goal);
00090
00091
00092 if (!success)
00093 as_.setAborted();
00094 }
00095
00096 void preemptCallback()
00097 {
00098 boost::mutex::scoped_lock lock(run_mutex_);
00099
00100
00101 as_.setPreempted();
00102 }
00103
00104 void cameraCallback ( const sensor_msgs::ImageConstPtr& image,
00105 const sensor_msgs::PointCloud2ConstPtr& depth)
00106
00107 {
00108 boost::mutex::scoped_lock lock(run_mutex_);
00109
00110 if (as_.isActive())
00111 {
00112 calibration_msgs::CalibrationPattern features;
00113 bool success;
00114
00115 pcl::PointCloud<pcl::PointXYZRGB> cloud;
00116 pcl::fromROSMsg(*depth, cloud);
00117 success = detector_.detect(image, features);
00118 if (!success)
00119 {
00120 ROS_ERROR("Error trying to detect checkerboard, not going to publish CalibrationPattern");
00121 return;
00122 }
00123
00124 for(size_t i = 0; i< features.image_points.size(); i++){
00125 geometry_msgs::Point *p = &(features.image_points[i]);
00126 pcl::PointXYZRGB pt = cloud((int)(p->x+0.5), (int)(p->y+0.5));
00127 if( isnan(pt.x) || isnan(pt.y) || isnan(pt.z) ) {
00128 ROS_ERROR("Invalid point in checkerboard, not going to publish CalibrationPattern");
00129 return;
00130 }
00131
00132 p->z = sqrt( (pt.x*pt.x) + (pt.y*pt.y) + (pt.z*pt.z) );
00133 }
00134
00135 pub_.publish(features);
00136 }
00137 }
00138
00139 private:
00140 boost::mutex run_mutex_;
00141 ros::NodeHandle nh_;
00142 actionlib::SimpleActionServer<image_cb_detector::ConfigAction> as_;
00143 image_cb_detector::ImageCbDetector detector_;
00144
00145 message_filters::Subscriber<sensor_msgs::Image> image_sub_;
00146 message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
00147 message_filters::Synchronizer<CameraSyncPolicy> sync_;
00148
00149 ros::Publisher pub_;
00150 };
00151
00152 int main(int argc, char** argv)
00153 {
00154 ros::init(argc, argv, "rgbd_cb_detector_action");
00155 ros::NodeHandle n;
00156 RgbdCbDetectorAction detector_action(n);
00157 ros::spin();
00158 return 0;
00159 }
00160