$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id:$ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Vit Stancl (stancl@fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: dd/mm/2012 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00028 #include <srs_env_model/but_server/compressed_pc_publisher.h> 00029 #include <srs_env_model/topics_list.h> 00030 #include <pcl/ros/conversions.h> 00031 #include <pcl_ros/transforms.h> 00032 #include <pcl/io/io.h> 00033 00034 00038 srs_env_model::CCompressedPCPublisher::CCompressedPCPublisher(ros::NodeHandle & nh) 00039 : m_camera_stereo_offset_left(0) 00040 , m_camera_stereo_offset_right(0) 00041 , m_publishFID(CPC_WORLD_FRAME) 00042 , m_cameraFID(CPC_CAMERA_FRAME) 00043 , m_tfListener() 00044 //, m_tfListener2() 00045 , m_tfListener2(ros::Duration(30)) 00046 { 00047 // Read input parameters via the private node handle 00048 ros::NodeHandle pnh("~"); 00049 00050 // Point cloud subscribing topic name 00051 // pnh.param("octomap_updates_publisher", m_subscribedTopicName, CPC_INPUT_TOPIC_NAME ); 00052 m_subscribedTopicName = CPC_INPUT_TOPIC_NAME; 00053 00054 // Point cloud publishing topic name 00055 // pnh.param("output_topic_name", m_publishedTopicName, CPC_OUTPUT_TOPIC_NAME); 00056 m_publishedTopicName = CPC_OUTPUT_TOPIC_NAME; 00057 00058 // TF frames 00059 pnh.param("world_frame", m_publishFID, CPC_WORLD_FRAME); 00060 pnh.param("camera_frame", m_cameraFID, CPC_CAMERA_FRAME); 00061 00062 // Create publisher 00063 m_pub = nh.advertise<sensor_msgs::PointCloud2> (m_publishedTopicName, 5); 00064 // m_pub = nh.advertise<sensor_msgs::PointCloud2> (m_publishedTopicName, 1); 00065 00066 // Create subscriber 00067 // m_sub = nh.subscribe(m_subscribedTopicName, 5, &CCompressedPCPublisher::incommingDataCB, this ); 00068 // m_sub = nh.subscribe(m_subscribedTopicName, 1, &CCompressedPCPublisher::incommingDataCB, this ); 00069 m_sub.subscribe(nh, m_subscribedTopicName, 5); 00070 m_tf_filter = new tf::MessageFilter<tInputData>(m_sub, m_tfListener, m_publishFID, 5); 00071 m_tf_filter->registerCallback( boost::bind(&CCompressedPCPublisher::incommingDataCB, this, _1) ); 00072 00073 /* if (!m_sub) 00074 { 00075 ROS_ERROR("Not subscribed..."); 00076 }*/ 00077 } 00078 00082 void srs_env_model::CCompressedPCPublisher::incommingDataCB( const tInputData::ConstPtr & data ) 00083 { 00084 ROS_DEBUG_ONCE("CCompressedPCPublisher::incommingDataCB: message received"); 00085 00086 // Get transform 00087 { 00088 // Some transforms 00089 tf::StampedTransform pcToPubTf; 00090 00091 // Get transforms 00092 try { 00093 // Transformation - to, from, time, waiting time 00094 // m_tfListener.waitForTransform(m_publishFID, data->pointcloud2.header.frame_id, 00095 // data->pointcloud2.header.stamp, ros::Duration(5)); 00096 00097 m_tfListener.lookupTransform( m_publishFID, data->pointcloud2.header.frame_id, 00098 data->pointcloud2.header.stamp, pcToPubTf ); 00099 00100 } catch (tf::TransformException& ex) { 00101 ROS_ERROR_STREAM( "CCompressedPCPublisher::incommingDataCB" << ": Transform error - " << ex.what() << ", quitting callback"); 00102 return; 00103 } 00104 00105 m_to_pfid = pcToPubTf; 00106 } 00107 00108 // Input buffer 00109 tInputCloudMsg inbuffer; 00110 00111 // Transform input point cloud to the internally used frame id 00112 pcl_ros::transformPointCloud( m_publishFID, m_to_pfid, data->pointcloud2, inbuffer ); 00113 00114 if( data->isPartial != 0 ) 00115 { 00116 // long oldcount( m_cloud.size() ), copied; 00117 00118 // Remove old data from cloud 00119 // copied = 00120 removeFrustumFromCloud( data, m_cloud ); 00121 00122 // Copy rest of the points from the actual point cloud 00123 copyCloud( inbuffer, m_cloud, true ); 00124 00125 // std::cerr << "Merging. Old size: " << oldcount << ", copied: " << copied << ", removed: " << oldcount - copied << ", current size: " << m_cloud.size() << std::endl; 00126 00127 // Convert data 00128 sensor_msgs::PointCloud2 cloud; 00129 pcl::toROSMsg< tPclPoint >(m_cloud, cloud); 00130 00131 // Set message parameters and publish 00132 cloud.header.frame_id = m_publishFID; 00133 cloud.header.stamp = data->pointcloud2.header.stamp; 00134 00135 // Publish new full data 00136 m_pub.publish( cloud ); 00137 00138 // std::cerr << "Published. M_cloud: " << m_cloud.size() << std::endl; 00139 } 00140 else 00141 { 00142 // std::cerr << "Complete cloud." << std::endl; 00143 00144 // Set message parameters and publish 00145 inbuffer.header.frame_id = m_publishFID; 00146 inbuffer.header.stamp = data->pointcloud2.header.stamp; 00147 00148 // Publish new full data 00149 m_pub.publish( inbuffer ); 00150 00151 // Copy data to the buffer 00152 m_cloud.clear(); 00153 copyCloud( inbuffer, m_cloud ); 00154 } 00155 } 00159 long srs_env_model::CCompressedPCPublisher::copyCloud( const tInputCloudMsg & input, tPointCloudInternal & output, bool bAdd /*= false*/ ) 00160 { 00161 00162 if( bAdd ) 00163 { 00164 tPointCloudInternal buffer; 00165 output.header = input.header; 00166 00167 if( ! isRGBCloud( input ) ) 00168 { 00169 pcl::PointCloud< pcl::PointXYZ >::Ptr bufferCloud( new pcl::PointCloud< pcl::PointXYZ> ); 00170 00171 pcl::fromROSMsg(input, *bufferCloud ); 00172 pcl::copyPointCloud< pcl::PointXYZ, tPclPoint >( *bufferCloud, buffer ); 00173 00174 } 00175 else 00176 { 00177 pcl::PointCloud< pcl::PointXYZRGB >::Ptr bufferCloud( new pcl::PointCloud< pcl::PointXYZRGB > ); 00178 00179 pcl::fromROSMsg(input, *bufferCloud); 00180 pcl::copyPointCloud<pcl::PointXYZRGB, tPclPoint>( *bufferCloud, buffer ); 00181 } 00182 00183 // Merge clouds 00184 output += buffer; 00185 00186 } 00187 else 00188 { 00189 if( ! isRGBCloud( input ) ) 00190 { 00191 pcl::PointCloud< pcl::PointXYZ >::Ptr bufferCloud( new pcl::PointCloud< pcl::PointXYZ> ); 00192 00193 pcl::fromROSMsg(input, *bufferCloud ); 00194 pcl::copyPointCloud< pcl::PointXYZ, tPclPoint >( *bufferCloud, output ); 00195 00196 } 00197 else 00198 { 00199 pcl::PointCloud< pcl::PointXYZRGB >::Ptr bufferCloud( new pcl::PointCloud< pcl::PointXYZRGB > ); 00200 00201 pcl::fromROSMsg(input, *bufferCloud); 00202 pcl::copyPointCloud<pcl::PointXYZRGB, tPclPoint>( *bufferCloud, output ); 00203 } 00204 00205 } 00206 00207 00208 00209 return output.size(); 00210 } 00211 00215 bool srs_env_model::CCompressedPCPublisher::isRGBCloud( const tInputCloudMsg & cloud ) 00216 { 00217 tInputCloudMsg::_fields_type::const_iterator i, end; 00218 00219 for( i = cloud.fields.begin(), end = cloud.fields.end(); i != end; ++i ) 00220 { 00221 if( i->name == "rgb" ) 00222 { 00223 // PERROR("HAS RGB"); 00224 return true; 00225 } 00226 } 00227 00228 // PERROR("NO RGB"); 00229 00230 return false; 00231 } 00232 00236 long srs_env_model::CCompressedPCPublisher::removeFrustumFromCloud( const tInputData::ConstPtr & data, tPointCloudInternal & cloud ) 00237 { 00238 // Get camera information 00239 { 00240 // Set camera position frame id 00241 m_cameraFrameId = data->camera_info.header.frame_id; 00242 00243 ROS_DEBUG("CCompressedPCPublisher::removeFrustumFromCloud: Set camera info: %d x %d\n", data->camera_info.height, data->camera_info.width); 00244 00245 m_camera_model.fromCameraInfo(data->camera_info); 00246 m_camera_size = m_camera_model.fullResolution(); 00247 } 00248 00249 // Get transformation from m_publishFID frame id to the camera frame id 00250 { 00251 m_to_sensor = tf::StampedTransform::getIdentity(); 00252 00253 //std::cerr << "CamFID: " << m_cameraFrameId << ", PCFID: " << data->pointcloud2.header.frame_id << std::endl; 00254 00255 // If different frame id 00256 if( m_cameraFrameId != m_publishFID ) 00257 { 00258 // Some transforms 00259 tf::StampedTransform ocToCamTf; 00260 00261 ros::Time stamp( data->header.stamp ); 00262 // Get transforms 00263 try { 00264 // Transformation - to, from, time, waiting time 00265 m_tfListener2.waitForTransform(m_cameraFrameId, m_publishFID, 00266 stamp, ros::Duration(5)); 00267 00268 m_tfListener2.lookupTransform( m_cameraFrameId, m_publishFID, 00269 stamp, ocToCamTf ); 00270 00271 } catch (tf::TransformException& ex) { 00272 ROS_ERROR_STREAM( "CCompressedPCPublisher::removeFrustumFromCloud: Transform error - " << ex.what() << ", quitting callback"); 00273 return 0; 00274 } 00275 00276 m_to_sensor = ocToCamTf; 00277 } 00278 } 00279 00280 long count(0); 00281 00282 // Copy current cloud to the buffer 00283 tPointCloudInternal buffer( cloud ); 00284 tPointCloudInternal::iterator p, end(buffer.end()); 00285 00286 // Clear cloud 00287 cloud.clear(); 00288 //std::cerr << "Buffer size: " << buffer.size() << std::endl; 00289 00290 00291 // Copy from the buffer only points out of the sensor cone 00292 for( p = buffer.begin(); p != end; ++p ) 00293 { 00294 // Test input point 00295 tf::Point pos(p->x, p->y, p->z); 00296 tf::Point posRel = m_to_sensor(pos); 00297 cv::Point2d uv = m_camera_model.project3dToPixel(cv::Point3d( posRel.x(), posRel.y(), posRel.z())); 00298 00299 // Add point if not in sensor cone 00300 if (!inSensorCone(uv)) 00301 { 00302 ++count; 00303 cloud.push_back( *p ); 00304 } 00305 } 00306 00307 return count; 00308 } 00309 00313 bool srs_env_model::CCompressedPCPublisher::inSensorCone(const cv::Point2d& uv) const 00314 { 00315 //std::cerr << uv.x << " > " << m_camera_stereo_offset_left + 1 << " && " << uv.x << " < " << m_camera_size.width + m_camera_stereo_offset_right - 2 << std::endl; 00316 //std::cerr << uv.y << " > " << 1 << " && " << uv.y << " < " << m_camera_size.height - 2 << std::endl; 00317 // Check if projected 2D coordinate in pixel range. 00318 // This check is a little more restrictive than it should be by using 00319 // 1 pixel less to account for rounding / discretization errors. 00320 // Otherwise points on the corner are accounted to be in the sensor cone. 00321 return ((uv.x > m_camera_stereo_offset_left + 1) && 00322 (uv.x < m_camera_size.width + m_camera_stereo_offset_right - 2) && 00323 (uv.y > 1) && 00324 (uv.y < m_camera_size.height - 2)); 00325 } 00326