cr_node_sync.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 #include <message_filters/sync_policies/approximate_time.h>
00020 
00021 #include <tf/transform_listener.h>
00022 
00023 #include <opencv/cv.h>
00024 #include <cv_bridge/cv_bridge.h>
00025 
00026 #include "SRCalibratedLib.h"
00027 #include "CRLib.h"
00028 
00029 // dynamic reconfigure
00030 #include <dynamic_reconfigure/server.h>
00031 #include "cr_capture/CRCaptureConfig.h"
00032 
00033 #include "cr_capture/RawCloudData.h"
00034 #include "cr_capture/PullRawData.h"
00035 #include "cr_capture/PixelIndices.h"
00036 
00037 class CRCaptureSyncNode {
00038 private:
00039   ros::NodeHandle nh_;
00040   image_transport::ImageTransport it_;
00041 
00042   ros::Publisher cloud_pub_;
00043   ros::Publisher cloud2_pub_;
00044   ros::Publisher index_pub_;
00045   ros::ServiceServer rawdata_service_;
00046 
00047   std::string left_ns_, right_ns_, range_ns_;
00048 
00049   // all subscriber
00050   image_transport::SubscriberFilter image_sub_left_, image_sub_right_;
00051   image_transport::SubscriberFilter image_sub_depth_, image_sub_intent_, image_sub_confi_;
00052   message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_left_, info_sub_right_, info_sub_range_;
00053 
00054   message_filters::Synchronizer<
00055     message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo,
00056                                                     sensor_msgs::Image, sensor_msgs::CameraInfo,
00057                                                     sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image,
00058                                                     sensor_msgs::CameraInfo> > sync_;
00059   // parameter
00060   bool calc_pixelpos;
00061   bool use_images;
00062   bool pull_raw_data;
00063 
00064   // libraries
00065   SRCalibratedLib sr_lib_;
00066   CRLib cr_lib_;
00067 
00068   // dynamic reconfigure
00069   typedef dynamic_reconfigure::Server<cr_capture::CRCaptureConfig> ReconfigureServer;
00070   ReconfigureServer reconfigure_server_;
00071 
00072   //buffer
00073   cr_capture::RawCloudData raw_cloud_;
00074 
00075 public:
00076   CRCaptureSyncNode () : nh_("~"), it_(nh_), sync_(30)
00077   {
00078     // Set up dynamic reconfiguration
00079     ReconfigureServer::CallbackType f = boost::bind(&CRCaptureSyncNode::config_cb, this, _1, _2);
00080     reconfigure_server_.setCallback(f);
00081 
00082     // parameter
00083     nh_.param("max_range", sr_lib_.max_range, 5.0);
00084     ROS_INFO("max_range : %f", sr_lib_.max_range);
00085 
00086     nh_.param("depth_scale", sr_lib_.depth_scale, 1.0); // not using
00087     ROS_INFO("depth_scale : %f", sr_lib_.depth_scale);
00088 
00089     // set transformation
00090     double trans_pos[3];
00091     double trans_quat[4];
00092     trans_pos[0] = trans_pos[1] = trans_pos[2] = 0;
00093     if (nh_.hasParam("translation")) {
00094       XmlRpc::XmlRpcValue param_val;
00095       nh_.getParam("translation", param_val);
00096       if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray && param_val.size() == 3) {
00097         trans_pos[0] = param_val[0];
00098         trans_pos[1] = param_val[1];
00099         trans_pos[2] = param_val[2];
00100       }
00101     }
00102     ROS_INFO("translation : [%f, %f, %f]", trans_pos[0], trans_pos[1], trans_pos[2]);
00103     trans_quat[0] = trans_quat[1] = trans_quat[2] = 0;
00104     trans_quat[3] = 1;
00105     if (nh_.hasParam("rotation")) {
00106       XmlRpc::XmlRpcValue param_val;
00107       nh_.getParam("rotation", param_val);
00108       if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray && param_val.size() == 4) {
00109         trans_quat[0] = param_val[0];
00110         trans_quat[1] = param_val[1];
00111         trans_quat[2] = param_val[2];
00112         trans_quat[3] = param_val[3];
00113       }
00114     }
00115     ROS_INFO("rotation : [%f, %f, %f, %f]", trans_quat[0], trans_quat[1],
00116              trans_quat[2], trans_quat[3]);
00117     tf::Quaternion btq(trans_quat[0], trans_quat[1], trans_quat[2], trans_quat[3]);
00118     tf::Vector3 btp(trans_pos[0], trans_pos[1], trans_pos[2]);
00119     cr_lib_.cam_trans.setOrigin(btp);
00120     cr_lib_.cam_trans.setRotation(btq);
00121 
00122     nh_.param("use_filter", sr_lib_.use_filter, true);
00123     ROS_INFO("use_filter : %d", sr_lib_.use_filter);
00124     sr_lib_.edge1 = 40.0; sr_lib_.edge2 = 80; sr_lib_.dilate_times = 1;
00125 
00126     nh_.param("use_smooth", sr_lib_.use_smooth, false);
00127     ROS_INFO("use_smooth : %d", sr_lib_.use_smooth);
00128     if (sr_lib_.use_smooth) {
00129       nh_.param("smooth_size", sr_lib_.smooth_size, 6);
00130       ROS_INFO("smooth_size : %d", sr_lib_.smooth_size);
00131       nh_.param("smooth_depth", sr_lib_.smooth_depth, 0.04);
00132       ROS_INFO("smooth_depth : %f", sr_lib_.smooth_depth);
00133       sr_lib_.smooth_depth = (sr_lib_.smooth_depth / sr_lib_.max_range) * 0xFFFF;
00134       nh_.param("smooth_space", sr_lib_.smooth_space, 6.0);
00135       ROS_INFO("smooth_space : %f", sr_lib_.smooth_space);
00136     }
00137 
00138     nh_.param("clear_uncolored_points", cr_lib_.clear_uncolored_points, true);
00139     ROS_INFO("clear_uncolored_points : %d", cr_lib_.clear_uncolored_points);
00140 
00141     nh_.param("short_range", sr_lib_.short_range, false);
00142     ROS_INFO("short_range : %d", sr_lib_.short_range);
00143 
00144     //
00145     // ros node setting
00146     //
00147     cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud> ("color_pcloud", 1);
00148     cloud2_pub_ = nh_.advertise<sensor_msgs::PointCloud2> ("color_pcloud2", 1);
00149 
00150     left_ns_ = nh_.resolveName("left");
00151     right_ns_ = nh_.resolveName("right");
00152     range_ns_ = nh_.resolveName("range");
00153 
00154     image_sub_left_.subscribe(it_, left_ns_  + "/image", 8);
00155     info_sub_left_ .subscribe(nh_, left_ns_  + "/camera_info", 8);
00156 
00157     image_sub_right_.subscribe(it_, right_ns_ + "/image", 8);
00158     info_sub_right_ .subscribe(nh_, right_ns_ + "/camera_info", 8);
00159 
00160     image_sub_depth_.subscribe(it_, range_ns_ + "/distance/image_raw16", 8);
00161     image_sub_intent_.subscribe(it_, range_ns_ + "/intensity/image_raw", 8);
00162     image_sub_confi_.subscribe(it_, range_ns_ + "/confidence/image_raw", 8);
00163     info_sub_range_ .subscribe(nh_, range_ns_ + "/camera_info", 8);
00164 
00165 
00166     nh_.param("intensity_threshold", sr_lib_.intensity_threshold, -1);
00167     if(sr_lib_.intensity_threshold >= 0) {
00168       ROS_INFO("intensity_threshold : %d", sr_lib_.intensity_threshold);
00169     }
00170     nh_.param("confidence_threshold", sr_lib_.confidence_threshold, -1);
00171     if(sr_lib_.confidence_threshold >= 0) {
00172       ROS_INFO("confidence_threshold : %d", sr_lib_.confidence_threshold);
00173     }
00174 
00175     sync_.connectInput(image_sub_left_, info_sub_left_, image_sub_right_, info_sub_right_,
00176                        image_sub_confi_, image_sub_intent_, image_sub_depth_, info_sub_range_);
00177 
00178     sync_.registerCallback(boost::bind(&CRCaptureSyncNode::imageCB, this, _1, _2, _3, _4, _5, _6, _7, _8));
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", &CRCaptureSyncNode::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
00224   imageCB(const sensor_msgs::ImageConstPtr& pimage_left,
00225           const sensor_msgs::CameraInfoConstPtr& pinfo_left,
00226           const sensor_msgs::ImageConstPtr& pimage_right,
00227           const sensor_msgs::CameraInfoConstPtr& pinfo_right,
00228           const sensor_msgs::ImageConstPtr& pimage_confi,
00229           const sensor_msgs::ImageConstPtr& pimage_intent,
00230           const sensor_msgs::ImageConstPtr& pimage_depth,
00231           const sensor_msgs::CameraInfoConstPtr& pinfo_range )
00232   {
00233     if(pull_raw_data) {
00234       raw_cloud_.intensity = *pimage_intent;
00235       raw_cloud_.confidence = *pimage_confi;
00236       raw_cloud_.depth16 = *pimage_depth;
00237       raw_cloud_.range_info = *pinfo_range;
00238 
00239       raw_cloud_.left_image = *pimage_left;
00240       raw_cloud_.left_info = *pinfo_left;
00241 
00242       raw_cloud_.right_image = *pimage_right;
00243       raw_cloud_.right_info = *pinfo_right;
00244 
00245       raw_cloud_.header = pinfo_range->header;
00246     }
00247 
00248     cr_lib_.setLeftImg(pimage_left, pinfo_left);
00249     cr_lib_.setRightImg(pimage_right, pinfo_right);
00250     sr_lib_.setRengeImg(pimage_confi, pimage_intent, pimage_depth, pinfo_range);
00251 
00252     publishCloud( pinfo_range->header );
00253   }
00254 
00255   void
00256   publishCloud (const std_msgs::Header &header)
00257   {
00258     sensor_msgs::PointCloud pts;
00259 
00260     sr_lib_.calc3DPoints(pts);
00261 
00262     if ( calc_pixelpos ) {
00263       cr_capture::PixelIndices pidx;
00264       cr_lib_.calcColor(pts, sr_lib_.width(), sr_lib_.height(), &pidx);
00265       index_pub_.publish(pidx);
00266       if (pull_raw_data)
00267         raw_cloud_.pixel_indices = pidx;
00268     } else {
00269       cr_lib_.calcColor(pts, sr_lib_.width(), sr_lib_.height());
00270     }
00271 
00272     // advertise
00273     if (cloud_pub_.getNumSubscribers() > 0)
00274     {
00275       pts.header = header;
00276       sensor_msgs::PointCloudPtr ptr = boost::make_shared <sensor_msgs::PointCloud> (pts);
00277       cloud_pub_.publish(ptr);
00278     }
00279     if (cloud2_pub_.getNumSubscribers() > 0 || pull_raw_data)
00280     {
00281       pts.header = header;
00282       sensor_msgs::PointCloud2 outbuf;
00283       if(!sensor_msgs::convertPointCloudToPointCloud2 (pts, outbuf))
00284       {
00285         ROS_ERROR ("[cr_capture] Conversion from sensor_msgs::PointCloud2 to sensor_msgs::PointCloud failed!");
00286         return;
00287       }
00288       outbuf.width = sr_lib_.width();
00289       outbuf.height = sr_lib_.height();
00290       outbuf.row_step = outbuf.width * outbuf.point_step;
00291       if(pull_raw_data)
00292         raw_cloud_.point_cloud = outbuf;
00293 
00294       if (cloud2_pub_.getNumSubscribers() > 0 )
00295       {
00296         sensor_msgs::PointCloud2Ptr ptr = boost::make_shared <sensor_msgs::PointCloud2> (outbuf);
00297         cloud2_pub_.publish(ptr);
00298       }
00299     }
00300   }
00301 };
00302 
00303 int main(int argc, char **argv)
00304 {
00305   ros::init(argc, argv, "cr_capture_sync");
00306 
00307   CRCaptureSyncNode cap_node;
00308 
00309   ros::spin();
00310   return 0;
00311 }


cr_capture
Author(s): youhei kakiuchi, JSK
autogenerated on Tue Jan 27 2015 11:52:47