cr_node.cpp
Go to the documentation of this file.
00001 
00002 //
00003 // JSK CR(ColorRange) Capture
00004 //
00005 
00006 #include <ros/ros.h>
00007 #include <ros/names.h>
00008 
00009 #include <sensor_msgs/Image.h>
00010 #include <sensor_msgs/CameraInfo.h>
00011 #include <sensor_msgs/PointCloud.h>
00012 #include <sensor_msgs/PointCloud2.h>
00013 #include <sensor_msgs/point_cloud_conversion.h>
00014 
00015 #include <image_transport/image_transport.h>
00016 #include <image_transport/subscriber_filter.h>
00017 #include <message_filters/subscriber.h>
00018 #include <message_filters/time_synchronizer.h>
00019 
00020 #include <tf/transform_listener.h>
00021 
00022 #include <opencv/cv.h>
00023 #include <cv_bridge/CvBridge.h>
00024 
00025 #include "SRCalibratedLib.h"
00026 #include "CRLib.h"
00027 
00028 // dynamic reconfigure
00029 #include <dynamic_reconfigure/server.h>
00030 #include "cr_capture/CRCaptureConfig.h"
00031 
00032 #include "cr_capture/RawCloudData.h"
00033 #include "cr_capture/PullRawData.h"
00034 #include "cr_capture/PixelIndices.h"
00035 
00036 class CRCaptureNode {
00037 private:
00038   ros::NodeHandle nh_;
00039   image_transport::ImageTransport it_;
00040   image_transport::CameraSubscriber camera_sub_l_, camera_sub_r_, camera_sub_depth_;
00041   ros::Publisher cloud_pub_;
00042   ros::Publisher cloud2_pub_;
00043   ros::Publisher index_pub_;
00044   ros::ServiceServer rawdata_service_;
00045 
00046   std::string left_ns_, right_ns_, range_ns_;
00047 
00048   // all subscriber
00049   message_filters::Subscriber<sensor_msgs::Image> image_conf_sub_;
00050   message_filters::Subscriber<sensor_msgs::Image> image_intent_sub_;
00051   message_filters::Subscriber<sensor_msgs::Image> image_depth_sub_;
00052   message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
00053   message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image,
00054                                     sensor_msgs::Image, sensor_msgs::CameraInfo> sync_;
00055 
00056   // parameter
00057   bool calc_pixelpos;
00058   bool use_images;
00059   bool pull_raw_data;
00060 
00061   // libraries
00062   SRCalibratedLib sr_lib_;
00063   CRLib cr_lib_;
00064 
00065   // dynamic reconfigure
00066   typedef dynamic_reconfigure::Server<cr_capture::CRCaptureConfig> ReconfigureServer;
00067   ReconfigureServer reconfigure_server_;
00068 
00069   //buffer
00070   cr_capture::RawCloudData raw_cloud_;
00071 
00072 public:
00073   CRCaptureNode () : nh_("~"), it_(nh_), sync_(5)
00074   {
00075     // Set up dynamic reconfiguration
00076     ReconfigureServer::CallbackType f = boost::bind(&CRCaptureNode::config_cb, this, _1, _2);
00077     reconfigure_server_.setCallback(f);
00078 
00079     // parameter
00080     nh_.param("max_range", sr_lib_.max_range, 5.0);
00081     ROS_INFO("max_range : %f", sr_lib_.max_range);
00082 
00083     nh_.param("depth_scale", sr_lib_.depth_scale, 1.0); // not using
00084     ROS_INFO("depth_scale : %f", sr_lib_.depth_scale);
00085 
00086     // set transformation
00087     double trans_pos[3];
00088     double trans_quat[4];
00089     trans_pos[0] = trans_pos[1] = trans_pos[2] = 0;
00090     if (nh_.hasParam("translation")) {
00091       XmlRpc::XmlRpcValue param_val;
00092       nh_.getParam("translation", param_val);
00093       if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray && param_val.size() == 3) {
00094         trans_pos[0] = param_val[0];
00095         trans_pos[1] = param_val[1];
00096         trans_pos[2] = param_val[2];
00097       }
00098     }
00099     ROS_INFO("translation : [%f, %f, %f]", trans_pos[0], trans_pos[1], trans_pos[2]);
00100     trans_quat[0] = trans_quat[1] = trans_quat[2] = 0;
00101     trans_quat[3] = 1;
00102     if (nh_.hasParam("rotation")) {
00103       XmlRpc::XmlRpcValue param_val;
00104       nh_.getParam("rotation", param_val);
00105       if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray && param_val.size() == 4) {
00106         trans_quat[0] = param_val[0];
00107         trans_quat[1] = param_val[1];
00108         trans_quat[2] = param_val[2];
00109         trans_quat[3] = param_val[3];
00110       }
00111     }
00112     ROS_INFO("rotation : [%f, %f, %f, %f]", trans_quat[0], trans_quat[1],
00113              trans_quat[2], trans_quat[3]);
00114     btQuaternion btq(trans_quat[0], trans_quat[1], trans_quat[2], trans_quat[3]);
00115     btVector3 btp(trans_pos[0], trans_pos[1], trans_pos[2]);
00116     cr_lib_.cam_trans.setOrigin(btp);
00117     cr_lib_.cam_trans.setRotation(btq);
00118 
00119     nh_.param("use_filter", sr_lib_.use_filter, true);
00120     ROS_INFO("use_filter : %d", sr_lib_.use_filter);
00121     sr_lib_.edge1 = 40.0; sr_lib_.edge2 = 80; sr_lib_.dilate_times = 1;
00122 
00123     nh_.param("use_smooth", sr_lib_.use_smooth, false);
00124     ROS_INFO("use_smooth : %d", sr_lib_.use_smooth);
00125     if (sr_lib_.use_smooth) {
00126       nh_.param("smooth_size", sr_lib_.smooth_size, 6);
00127       ROS_INFO("smooth_size : %d", sr_lib_.smooth_size);
00128       nh_.param("smooth_depth", sr_lib_.smooth_depth, 0.04);
00129       ROS_INFO("smooth_depth : %f", sr_lib_.smooth_depth);
00130       sr_lib_.smooth_depth = (sr_lib_.smooth_depth / sr_lib_.max_range) * 0xFFFF;
00131       nh_.param("smooth_space", sr_lib_.smooth_space, 6.0);
00132       ROS_INFO("smooth_space : %f", sr_lib_.smooth_space);
00133     }
00134 
00135     nh_.param("clear_uncolored_points", cr_lib_.clear_uncolored_points, true);
00136     ROS_INFO("clear_uncolored_points : %d", cr_lib_.clear_uncolored_points);
00137 
00138     nh_.param("short_range", sr_lib_.short_range, false);
00139     ROS_INFO("short_range : %d", sr_lib_.short_range);
00140 
00141     //
00142     // ros node setting
00143     //
00144     cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud> ("color_pcloud", 1);
00145     cloud2_pub_ = nh_.advertise<sensor_msgs::PointCloud2> ("color_pcloud2", 1);
00146 
00147     left_ns_ = nh_.resolveName("left");
00148     right_ns_ = nh_.resolveName("right");
00149     range_ns_ = nh_.resolveName("range");
00150 
00151     camera_sub_l_ = it_.subscribeCamera(left_ns_ + "/image", 1, &CRCaptureNode::cameraleftCB, this);
00152     camera_sub_r_ = it_.subscribeCamera(right_ns_ + "/image", 1, &CRCaptureNode::camerarightCB, this);
00153 
00154     // use images for confidence, intensity threshold
00155     nh_.param("use_images", use_images, false);
00156     ROS_INFO("use_images : %d", use_images);
00157     if(use_images) {
00158       // all subscribe
00159       nh_.param("intensity_threshold", sr_lib_.intensity_threshold, -1);
00160       if(sr_lib_.intensity_threshold >= 0) {
00161         ROS_INFO("intensity_threshold : %d", sr_lib_.intensity_threshold);
00162       }
00163       nh_.param("confidence_threshold", sr_lib_.confidence_threshold, -1);
00164       if(sr_lib_.confidence_threshold >= 0) {
00165         ROS_INFO("confidence_threshold : %d", sr_lib_.confidence_threshold);
00166       }
00167 
00168       image_conf_sub_.subscribe(nh_, range_ns_ + "/confidence/image_raw", 1);
00169       image_intent_sub_.subscribe(nh_, range_ns_ + "/intensity/image_raw", 1);
00170       image_depth_sub_.subscribe(nh_, range_ns_ + "/distance/image_raw16", 1);
00171       info_sub_.subscribe(nh_, range_ns_ + "/camera_info", 1);
00172 
00173       sync_.connectInput(image_conf_sub_, image_intent_sub_, image_depth_sub_, info_sub_);
00174       sync_.registerCallback(boost::bind(&CRCaptureNode::cameraallCB, this, _1, _2, _3, _4));
00175     } else {
00176       // not all subscribe
00177       camera_sub_depth_ = it_.subscribeCamera(range_ns_ + "/image", 1, &CRCaptureNode::camerarangeCB, this);
00178     }
00179 
00180     // pull raw data service
00181     nh_.param("pull_raw_data", pull_raw_data, false);
00182     ROS_INFO("pull_raw_data : %d", pull_raw_data);
00183     if(pull_raw_data) {
00184       rawdata_service_ = nh_.advertiseService("pull_raw_data", &CRCaptureNode::pullData, this);
00185     }
00186 
00187     nh_.param("calc_pixel_color", calc_pixelpos, false); // not using
00188     ROS_INFO("calc_pixel_color : %d", calc_pixelpos);
00189     if(calc_pixelpos) {
00190       index_pub_ = nh_.advertise<cr_capture::PixelIndices>("pixel_indices", 1);
00191     }
00192   }
00193 
00194   bool
00195   pullData (cr_capture::PullRawDataRequest &req,
00196             cr_capture::PullRawDataResponse &res)
00197   {
00198     res.data = raw_cloud_;
00199     return true;
00200   }
00201 
00202   void
00203   config_cb (cr_capture::CRCaptureConfig &config, uint32_t level)
00204   {
00205     cr_lib_.clear_uncolored_points = config.clear_uncolored;
00206     sr_lib_.short_range = config.short_range;
00207 
00208     sr_lib_.use_filter   = config.use_filter;
00209     sr_lib_.edge1        = config.canny_parameter1;
00210     sr_lib_.edge2        = config.canny_parameter2;
00211     sr_lib_.dilate_times = config.dilate_times;
00212 
00213     sr_lib_.use_smooth   = config.use_smooth;
00214     sr_lib_.smooth_size  = config.smooth_size;
00215     sr_lib_.smooth_space = config.smooth_space;
00216     sr_lib_.smooth_depth = config.smooth_depth;
00217     sr_lib_.smooth_depth = (sr_lib_.smooth_depth / sr_lib_.max_range) * 0xFFFF;
00218 
00219     sr_lib_.intensity_threshold = config.intensity_threshold;
00220     sr_lib_.confidence_threshold = config.confidence_threshold;
00221   }
00222 
00223   void cameraleftCB(const sensor_msgs::ImageConstPtr &img,
00224                     const sensor_msgs::CameraInfoConstPtr &info)
00225   {
00226 
00227     cr_lib_.setLeftImg(img, info);
00228 
00229     if(pull_raw_data)
00230     {
00231       raw_cloud_.left_image = *img;
00232       raw_cloud_.left_info = *info;
00233     }
00234   }
00235 
00236   void
00237   camerarightCB(const sensor_msgs::ImageConstPtr &img,
00238                 const sensor_msgs::CameraInfoConstPtr &info)
00239   {
00240 
00241     cr_lib_.setRightImg(img, info);
00242 
00243     if(pull_raw_data)
00244     {
00245       raw_cloud_.right_image = *img;
00246       raw_cloud_.right_info = *info;
00247     }
00248   }
00249 
00250   void
00251   cameraallCB (const sensor_msgs::ImageConstPtr &img_c,
00252                const sensor_msgs::ImageConstPtr &img_i,
00253                const sensor_msgs::ImageConstPtr &img_d,
00254                const sensor_msgs::CameraInfoConstPtr &info)
00255   {
00256     sr_lib_.setRengeImg(img_c, img_i, img_d, info);
00257 
00258     if(pull_raw_data) {
00259       raw_cloud_.intensity = *img_i;
00260       raw_cloud_.confidence = *img_c;
00261       raw_cloud_.depth16 = *img_d;
00262       raw_cloud_.range_info = *info;
00263 
00264       raw_cloud_.header = info->header;
00265     }
00266 
00267     publishCloud( info->header );
00268   }
00269 
00270   void
00271   camerarangeCB (const sensor_msgs::ImageConstPtr &img,
00272                  const sensor_msgs::CameraInfoConstPtr &info)
00273   {
00274     sr_lib_.setRengeImg(img, img, img, info);
00275 
00276     if(pull_raw_data) {
00277       raw_cloud_.depth16 = *img;
00278       raw_cloud_.range_info = *info;
00279 
00280       raw_cloud_.header = info->header;
00281     }
00282 
00283     publishCloud( info->header );
00284   }
00285 
00286   void
00287   publishCloud (const std_msgs::Header &header)
00288   {
00289     sensor_msgs::PointCloud pts;
00290 
00291     sr_lib_.calc3DPoints(pts);
00292 
00293     if ( calc_pixelpos ) {
00294       cr_capture::PixelIndices pidx;
00295       cr_lib_.calcColor(pts, sr_lib_.width(), sr_lib_.height(), &pidx);
00296       index_pub_.publish(pidx);
00297       if (pull_raw_data)
00298         raw_cloud_.pixel_indices = pidx;
00299     } else {
00300       cr_lib_.calcColor(pts, sr_lib_.width(), sr_lib_.height());
00301     }
00302 
00303     // advertise
00304     if (cloud_pub_.getNumSubscribers() > 0)
00305     {
00306       pts.header = header;
00307       sensor_msgs::PointCloudPtr ptr = boost::make_shared <sensor_msgs::PointCloud> (pts);
00308       cloud_pub_.publish(ptr);
00309     }
00310     if (cloud2_pub_.getNumSubscribers() > 0 || pull_raw_data)
00311     {
00312       pts.header = header;
00313       sensor_msgs::PointCloud2 outbuf;
00314       if(!sensor_msgs::convertPointCloudToPointCloud2 (pts, outbuf))
00315       {
00316         ROS_ERROR ("[cr_capture] Conversion from sensor_msgs::PointCloud2 to sensor_msgs::PointCloud failed!");
00317         return;
00318       }
00319       outbuf.width = sr_lib_.width();
00320       outbuf.height = sr_lib_.height();
00321       outbuf.row_step = outbuf.width * outbuf.point_step;
00322       if(pull_raw_data)
00323         raw_cloud_.point_cloud = outbuf;
00324 
00325       if (cloud2_pub_.getNumSubscribers() > 0 )
00326       {
00327         sensor_msgs::PointCloud2Ptr ptr = boost::make_shared <sensor_msgs::PointCloud2> (outbuf);
00328         cloud2_pub_.publish(ptr);
00329       }
00330     }
00331   }
00332 };
00333 
00334 int main(int argc, char **argv)
00335 {
00336   ros::init(argc, argv, "cr_capture");
00337 
00338   CRCaptureNode cap_node;
00339 
00340   ros::spin();
00341   return 0;
00342 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


cr_capture
Author(s): youhei kakiuchi, JSK
autogenerated on Sun Mar 24 2013 02:36:21