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
00035
00036 #include "jsk_pcl_ros/depth_image_creator.h"
00037 #include <jsk_topic_tools/rosparam_utils.h>
00038
00039 void jsk_pcl_ros::DepthImageCreator::onInit () {
00040 JSK_NODELET_INFO("[%s::onInit]", getName().c_str());
00041 ConnectionBasedNodelet::onInit();
00042 tf_listener_ = TfListenerSingleton::getInstance();
00043
00044 pnh_->param("scale_depth", scale_depth, 1.0);
00045 JSK_ROS_INFO("scale_depth : %f", scale_depth);
00046
00047
00048 pnh_->param("use_fixed_transform", use_fixed_transform, false);
00049 JSK_ROS_INFO("use_fixed_transform : %d", use_fixed_transform);
00050
00051 pnh_->param("use_service", use_service, false);
00052 JSK_ROS_INFO("use_service : %d", use_service);
00053
00054 pnh_->param("use_asynchronous", use_asynchronous, false);
00055 JSK_ROS_INFO("use_asynchronous : %d", use_asynchronous);
00056
00057 pnh_->param("use_approximate", use_approximate, false);
00058 JSK_ROS_INFO("use_approximate : %d", use_approximate);
00059
00060 pnh_->param("info_throttle", info_throttle_, 0);
00061 info_counter_ = 0;
00062 pnh_->param("max_queue_size", max_queue_size_, 3);
00063
00064 std::vector<double> trans_pos(3, 0);
00065 std::vector<double> trans_quat(4, 0); trans_quat[3] = 1.0;
00066 if (pnh_->hasParam("translation")) {
00067 jsk_topic_tools::readVectorParameter(*pnh_, "translation", trans_pos);
00068 }
00069 if (pnh_->hasParam("rotation")) {
00070 jsk_topic_tools::readVectorParameter(*pnh_, "rotation", trans_quat);
00071 }
00072 tf::Quaternion btq(trans_quat[0], trans_quat[1], trans_quat[2], trans_quat[3]);
00073 tf::Vector3 btp(trans_pos[0], trans_pos[1], trans_pos[2]);
00074 fixed_transform.setOrigin(btp);
00075 fixed_transform.setRotation(btq);
00076
00077 pub_image_ = advertise<sensor_msgs::Image> (*pnh_, "output", max_queue_size_);
00078 pub_cloud_ = advertise<PointCloud>(*pnh_, "output_cloud", max_queue_size_);
00079 pub_disp_image_ = advertise<stereo_msgs::DisparityImage> (*pnh_, "output_disp", max_queue_size_);
00080 if (use_service) {
00081 service_ = pnh_->advertiseService("set_point_cloud",
00082 &DepthImageCreator::service_cb, this);
00083 }
00084 }
00085
00086 void jsk_pcl_ros::DepthImageCreator::subscribe() {
00087 if (!use_service) {
00088 if (use_asynchronous) {
00089 sub_as_info_ = pnh_->subscribe<sensor_msgs::CameraInfo> ("info", max_queue_size_,
00090 &DepthImageCreator::callback_info, this);
00091 sub_as_cloud_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_queue_size_,
00092 &DepthImageCreator::callback_cloud, this);
00093 } else {
00094 sub_info_.subscribe(*pnh_, "info", max_queue_size_);
00095 sub_cloud_.subscribe(*pnh_, "input", max_queue_size_);
00096
00097 if (use_approximate) {
00098 sync_inputs_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> > > (max_queue_size_);
00099 sync_inputs_a_->connectInput (sub_info_, sub_cloud_);
00100 sync_inputs_a_->registerCallback (bind (&DepthImageCreator::callback_sync, this, _1, _2));
00101 } else {
00102 sync_inputs_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> > > (max_queue_size_);
00103 sync_inputs_e_->connectInput (sub_info_, sub_cloud_);
00104 sync_inputs_e_->registerCallback (bind (&DepthImageCreator::callback_sync, this, _1, _2));
00105 }
00106 }
00107 } else {
00108
00109 sub_as_info_ = pnh_->subscribe<sensor_msgs::CameraInfo> ("info", max_queue_size_,
00110 &DepthImageCreator::callback_info, this);
00111
00112 }
00113 }
00114
00115 void jsk_pcl_ros::DepthImageCreator::unsubscribe() {
00116 if (!use_service) {
00117 if (use_asynchronous) {
00118 sub_as_info_.shutdown();
00119 sub_as_cloud_.shutdown();
00120 }
00121 else {
00122 sub_info_.unsubscribe();
00123 sub_cloud_.unsubscribe();
00124 }
00125 } else {
00126
00127 sub_as_info_.shutdown();
00128 }
00129 }
00130
00131 bool jsk_pcl_ros::DepthImageCreator::service_cb (std_srvs::Empty::Request &req,
00132 std_srvs::Empty::Response &res) {
00133 return true;
00134 }
00135
00136 void jsk_pcl_ros::DepthImageCreator::callback_sync(const sensor_msgs::CameraInfoConstPtr& info,
00137 const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
00138 JSK_ROS_DEBUG("DepthImageCreator::callback_sync");
00139 publish_points(info, pcloud2);
00140 }
00141
00142 void jsk_pcl_ros::DepthImageCreator::callback_cloud(const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
00143 JSK_ROS_DEBUG("DepthImageCreator::callback_cloud");
00144 boost::mutex::scoped_lock lock(this->mutex_points);
00145 points_ptr_ = pcloud2;
00146 }
00147
00148 void jsk_pcl_ros::DepthImageCreator::callback_info(const sensor_msgs::CameraInfoConstPtr& info) {
00149 JSK_ROS_DEBUG("DepthImageCreator::callback_info");
00150 boost::mutex::scoped_lock lock(this->mutex_points);
00151 if( info_counter_++ >= info_throttle_ ) {
00152 info_counter_ = 0;
00153 } else {
00154 return;
00155 }
00156 publish_points(info, points_ptr_);
00157 }
00158
00159 void jsk_pcl_ros::DepthImageCreator::publish_points(const sensor_msgs::CameraInfoConstPtr& info,
00160 const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
00161 JSK_ROS_DEBUG("DepthImageCreator::publish_points");
00162 if (!pcloud2) return;
00163 bool proc_cloud = true, proc_image = true, proc_disp = true;
00164 if ( pub_cloud_.getNumSubscribers()==0 ) {
00165 proc_cloud = false;
00166 }
00167 if ( pub_image_.getNumSubscribers()==0 ) {
00168 proc_image = false;
00169 }
00170 if ( pub_disp_image_.getNumSubscribers()==0 ) {
00171 proc_disp = false;
00172 }
00173 if( !proc_cloud && !proc_image && !proc_disp) return;
00174
00175 int width = info->width;
00176 int height = info->height;
00177 float fx = info->P[0];
00178 float cx = info->P[2];
00179 float tx = info->P[3];
00180 float fy = info->P[5];
00181 float cy = info->P[6];
00182
00183 Eigen::Affine3f sensorPose;
00184 {
00185 tf::StampedTransform transform;
00186 if(use_fixed_transform) {
00187 transform = fixed_transform;
00188 } else {
00189 try {
00190 tf_listener_->waitForTransform(pcloud2->header.frame_id,
00191 info->header.frame_id,
00192 info->header.stamp,
00193 ros::Duration(0.001));
00194 tf_listener_->lookupTransform(pcloud2->header.frame_id,
00195 info->header.frame_id,
00196 info->header.stamp, transform);
00197 }
00198 catch ( std::runtime_error e ) {
00199 JSK_ROS_ERROR("%s",e.what());
00200 return;
00201 }
00202 }
00203 tf::Vector3 p = transform.getOrigin();
00204 tf::Quaternion q = transform.getRotation();
00205 sensorPose = (Eigen::Affine3f)Eigen::Translation3f(p.getX(), p.getY(), p.getZ());
00206 Eigen::Quaternion<float> rot(q.getW(), q.getX(), q.getY(), q.getZ());
00207 sensorPose = sensorPose * rot;
00208
00209 if (tx != 0.0) {
00210 Eigen::Affine3f trans = (Eigen::Affine3f)Eigen::Translation3f(-tx/fx , 0, 0);
00211 sensorPose = sensorPose * trans;
00212 }
00213 #if 0 // debug print
00214 JSK_ROS_INFO("%f %f %f %f %f %f %f %f %f, %f %f %f",
00215 sensorPose(0,0), sensorPose(0,1), sensorPose(0,2),
00216 sensorPose(1,0), sensorPose(1,1), sensorPose(1,2),
00217 sensorPose(2,0), sensorPose(2,1), sensorPose(2,2),
00218 sensorPose(0,3), sensorPose(1,3), sensorPose(2,3));
00219 #endif
00220 }
00221
00222 PointCloud pointCloud;
00223 pcl::RangeImagePlanar rangeImageP;
00224 {
00225
00226 PointCloud tpc;
00227 pcl::fromROSMsg(*pcloud2, tpc);
00228
00229 Eigen::Affine3f inv;
00230 #if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
00231 inv = sensorPose.inverse();
00232 pcl::transformPointCloud< Point > (tpc, pointCloud, inv);
00233 #else
00234 pcl::getInverse(sensorPose, inv);
00235 pcl::getTransformedPointCloud<PointCloud> (tpc, inv, pointCloud);
00236 #endif
00237
00238 Eigen::Affine3f dummytrans;
00239 dummytrans.setIdentity();
00240 rangeImageP.createFromPointCloudWithFixedSize (pointCloud,
00241 width/scale_depth, height/scale_depth,
00242 cx/scale_depth, cy/scale_depth,
00243 fx/scale_depth, fy/scale_depth,
00244 dummytrans);
00245 }
00246
00247 cv::Mat mat(rangeImageP.height, rangeImageP.width, CV_32FC1);
00248 float *tmpf = (float *)mat.ptr();
00249 for(unsigned int i = 0; i < rangeImageP.height*rangeImageP.width; i++) {
00250 tmpf[i] = rangeImageP.points[i].z;
00251 }
00252
00253 if(scale_depth != 1.0) {
00254 cv::Mat tmpmat(info->height, info->width, CV_32FC1);
00255 cv::resize(mat, tmpmat, cv::Size(info->width, info->height));
00256
00257 mat = tmpmat;
00258 }
00259
00260 if (proc_image) {
00261 sensor_msgs::Image pubimg;
00262 pubimg.header = info->header;
00263 pubimg.width = info->width;
00264 pubimg.height = info->height;
00265 pubimg.encoding = "32FC1";
00266 pubimg.step = sizeof(float)*info->width;
00267 pubimg.data.resize(sizeof(float)*info->width*info->height);
00268
00269
00270 memcpy(&(pubimg.data[0]), mat.ptr(), sizeof(float)*info->height*info->width);
00271 pub_image_.publish(boost::make_shared<sensor_msgs::Image>(pubimg));
00272 }
00273
00274 if(proc_cloud || proc_disp) {
00275
00276 pcl::RangeImagePlanar rangeImagePP;
00277 rangeImagePP.setDepthImage ((float *)mat.ptr(),
00278 width, height,
00279 cx, cy, fx, fy);
00280 #if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
00281 rangeImagePP.header = pcl_conversions::toPCL(info->header);
00282 #else
00283 rangeImagePP.header = info->header;
00284 #endif
00285 if(proc_cloud) {
00286 pub_cloud_.publish(boost::make_shared<pcl::PointCloud<pcl::PointWithRange > >
00287 ( (pcl::PointCloud<pcl::PointWithRange>)rangeImagePP) );
00288 }
00289
00290 if(proc_disp) {
00291 stereo_msgs::DisparityImage disp;
00292 #if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
00293 disp.header = pcl_conversions::fromPCL(rangeImagePP.header);
00294 #else
00295 disp.header = rangeImagePP.header;
00296 #endif
00297 disp.image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00298 disp.image.height = rangeImagePP.height;
00299 disp.image.width = rangeImagePP.width;
00300 disp.image.step = disp.image.width * sizeof(float);
00301 disp.f = fx; disp.T = 0.075;
00302 disp.min_disparity = 0;
00303 disp.max_disparity = disp.T * disp.f / 0.3;
00304 disp.delta_d = 0.125;
00305
00306 disp.image.data.resize (disp.image.height * disp.image.step);
00307 float *data = reinterpret_cast<float*> (&disp.image.data[0]);
00308
00309 float normalization_factor = disp.f * disp.T;
00310 for (int y = 0; y < (int)rangeImagePP.height; y++ ) {
00311 for (int x = 0; x < (int)rangeImagePP.width; x++ ) {
00312 pcl::PointWithRange p = rangeImagePP.getPoint(x,y);
00313 data[y*disp.image.width+x] = normalization_factor / p.z;
00314 }
00315 }
00316 pub_disp_image_.publish(boost::make_shared<stereo_msgs::DisparityImage> (disp));
00317 }
00318 }
00319 }
00320
00321 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::DepthImageCreator, nodelet::Nodelet);