$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/plugins/compressed_point_cloud_plugin.h> 00029 #include <srs_env_model/topics_list.h> 00030 00031 #include <pcl_ros/transforms.h> 00032 #include <Eigen/src/Geometry/Quaternion.h> 00033 00037 srs_env_model::CCompressedPointCloudPlugin::CCompressedPointCloudPlugin( const std::string & name ) 00038 : srs_env_model::CPointCloudPlugin( name, false ) 00039 , m_bTransformCamera( false ) 00040 , m_bSpinThread( true ) 00041 , m_frame_counter(0) 00042 , m_uncomplete_frames( 10 ) 00043 , m_bPublishComplete( false ) 00044 , m_octomap_updates_msg( new OctomapUpdates ) 00045 , m_bTransformOutput( false ) 00046 { 00047 m_use_every_nth = 1; 00048 } 00049 00053 srs_env_model::CCompressedPointCloudPlugin::~CCompressedPointCloudPlugin() 00054 { 00055 if (spin_thread_.get()) 00056 { 00057 need_to_terminate_ = true; 00058 spin_thread_->join(); 00059 } 00060 } 00061 00065 void srs_env_model::CCompressedPointCloudPlugin::spinThread() 00066 { 00067 while (node_handle_.ok()) 00068 { 00069 if (need_to_terminate_) 00070 { 00071 break; 00072 } 00073 callback_queue_.callAvailable(ros::WallDuration(0.033f)); 00074 } 00075 } 00076 00080 void srs_env_model::CCompressedPointCloudPlugin::init(ros::NodeHandle & node_handle) 00081 { 00082 ROS_DEBUG("Initializing CCompressedPointCloudPlugin"); 00083 00084 // 2013/01/31 Majkl: I guess we should publish the map in the Octomap TF frame... 00085 node_handle.param("ocmap_frame_id", m_frame_id, m_frame_id); 00086 00087 if ( m_bSpinThread ) 00088 { 00089 // if we're spinning our own thread, we'll also need our own callback queue 00090 node_handle.setCallbackQueue( &callback_queue_ ); 00091 00092 need_to_terminate_ = false; 00093 spin_thread_.reset( new boost::thread(boost::bind(&CCompressedPointCloudPlugin::spinThread, this)) ); 00094 node_handle_ = node_handle; 00095 } 00096 00097 // Read parameters 00098 { 00099 // Where to get camera position information 00100 node_handle.param("compressed_pc_camera_info_topic_name", m_cameraInfoTopic, CPC_CAMERA_INFO_PUBLISHER_NAME ); 00101 ROS_INFO("SRS_ENV_SERVER: parameter - compressed_pc_camera_info_topic_name: %s", m_cameraInfoTopic.c_str() ); 00102 00103 // Point cloud publishing topic name 00104 node_handle.param("compressed_pc_pointcloud_centers_publisher", m_pcPublisherName, CPC_SIMPLE_PC_PUBLISHING_TOPIC_NAME); 00105 ROS_INFO("SRS_ENV_SERVER: parameter - compressed_pc_pointcloud_centers_publisher: %s", m_pcPublisherName.c_str() ); 00106 00107 // Should simple pointcloud be published too? 00108 node_handle.param("publish_simple_cloud", m_bPublishSimpleCloud, false ); 00109 00110 // How many uncomplete frames should be published before complete one 00111 int uf; 00112 node_handle.param("compressed_pc_differential_frames_count", uf, CPC_NUM_DIFFERENTIAL_FRAMES ); 00113 m_uncomplete_frames = uf; 00114 00115 ROS_INFO("SRS_ENV_SERVER: parameter - compressed_pc_update_data_topic_name: %i", uf ); 00116 00117 // Complete data topic name 00118 node_handle.param( "compressed_pc_update_data_topic_name", m_ocUpdatePublisherName, CPC_COMPLETE_TOPIC_NAME ); 00119 ROS_INFO("SRS_ENV_SERVER: parameter - compressed_pc_update_data_topic_name: %s", m_ocUpdatePublisherName.c_str() ); 00120 } 00121 00122 // Services 00123 { 00124 m_serviceSetNumIncomplete = node_handle.advertiseService( SetNumIncompleteFrames_SRV, 00125 &srs_env_model::CCompressedPointCloudPlugin::setNumIncompleteFramesCB, this ); 00126 } 00127 00128 // Create publisher - simple point cloud 00129 if( m_bPublishSimpleCloud) 00130 m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 5, m_latchedTopics); 00131 00132 // Create publisher - packed info - cam position and update pointcloud 00133 m_ocUpdatePublisher = node_handle.advertise< srs_env_model::OctomapUpdates > ( m_ocUpdatePublisherName, 5, m_latchedTopics ); 00134 00135 // Subscribe to position topic 00136 // Create subscriber 00137 m_camPosSubscriber = node_handle.subscribe<sensor_msgs::CameraInfo>( m_cameraInfoTopic, 10, &srs_env_model::CCompressedPointCloudPlugin::onCameraChangedCB, this ); 00138 // = new message_filters::Subscriber<srs_env_model_msgs::RVIZCameraPosition>(node_handle, cameraPositionTopic, 1); 00139 00140 if (!m_camPosSubscriber) 00141 { 00142 ROS_ERROR("Not subscribed..."); 00143 PERROR( "Cannot subscribe to the camera position publisher..." ); 00144 } 00145 /* 00146 // Create message filter 00147 m_tfCamPosSub = new tf::MessageFilter<srs_env_model_msgs::RVIZCameraPosition>( *m_camPosSubscriber, m_tfListener, "/map", 1); 00148 m_tfCamPosSub->registerCallback(boost::bind( &CCompressedPointCloudPlugin::onCameraPositionChangedCB, this, _1)); 00149 */ 00150 // Clear old pointcloud data 00151 m_data->clear(); 00152 00153 // stereo cam params for sensor cone: 00154 node_handle.param<int> ("compressed_pc_camera_stereo_offset_left", m_camera_stereo_offset_left, 0); // 128 00155 node_handle.param<int> ("compressed_pc_camera_stereo_offset_right", m_camera_stereo_offset_right, 0); 00156 00157 } 00158 00163 void srs_env_model::CCompressedPointCloudPlugin::newMapDataCB( SMapWithParameters & par ) 00164 { 00165 ROS_DEBUG( "CCompressedPointCloudPlugin: onFrameStart" ); 00166 00167 // Copy buffered camera normal and d parameter 00168 boost::recursive_mutex::scoped_lock lock( m_camPosMutex ); 00169 00170 // Increase frames count 00171 ++m_frame_counter; 00172 00173 // Is this frame complete? 00174 m_bPublishComplete = m_frame_counter % m_uncomplete_frames == 0; 00175 00176 // Create packed octomap update info? 00177 m_bCreatePackedInfoMsg = m_ocUpdatePublisher.getNumSubscribers() > 0; 00178 00179 // Reset counters 00180 m_countVisible = m_countAll = 0; 00181 00182 min = 10000000; 00183 max = -10000000; 00184 00185 // Call parent frame start 00186 if( ! m_publishPointCloud ) 00187 return; 00188 00189 // Just for sure 00190 if(m_frame_id != par.frameId) 00191 { 00192 PERROR("Map frame id has changed, this should never happen. Exiting newMapDataCB."); 00193 return; 00194 } 00195 00196 // Clear data 00197 m_data->clear(); 00198 m_DataTimeStamp = m_time_stamp = par.currentTime; 00199 counter = 0; 00200 00201 // Pointcloud is used as output for octomap... 00202 m_bAsInput = false; 00203 00204 if( m_cameraFrameId.size() == 0 ) 00205 { 00206 ROS_DEBUG("CCompressedPointCloudPlugin::newMapDataCB: Wrong camera frame id..."); 00207 // m_bTransformCamera = false; 00208 return; 00209 } 00210 00211 // m_bTransformCamera = m_cameraFrameId != m_pcFrameId; 00212 bool m_bTransformCamera(m_cameraFrameId != m_frame_id); 00213 00214 m_to_sensor = tf::StampedTransform::getIdentity(); 00215 00216 // If different frame id 00217 if( m_bTransformCamera ) 00218 { 00219 // Some transforms 00220 tf::StampedTransform camToOcTf, ocToCamTf; 00221 00222 // Get transforms 00223 try { 00224 // Transformation - to, from, time, waiting time 00225 m_tfListener.waitForTransform(m_cameraFrameId, m_frame_id, 00226 par.currentTime, ros::Duration(5)); 00227 00228 m_tfListener.lookupTransform( m_cameraFrameId, m_frame_id, 00229 par.currentTime, ocToCamTf ); 00230 00231 } catch (tf::TransformException& ex) { 00232 ROS_ERROR_STREAM( m_name << ": Transform error - " << ex.what() << ", quitting callback"); 00233 PERROR( "Camera FID: " << m_cameraFrameId << ", Octomap FID: " << m_frame_id ); 00234 return; 00235 } 00236 00237 m_to_sensor = ocToCamTf; 00238 00239 // PERROR( "Camera position: " << m_camToOcTrans ); 00240 } 00241 00242 // Store camera information 00243 m_camera_size = m_camera_size_buffer; 00244 m_camera_model.fromCameraInfo( m_camera_info_buffer); 00245 m_octomap_updates_msg->camera_info = m_camera_info_buffer; 00246 m_octomap_updates_msg->pointcloud2.header.stamp = par.currentTime; 00247 00248 // Majkl 2013/01/24: missing frame id in the message header 00249 m_octomap_updates_msg->pointcloud2.header.frame_id = par.frameId; 00250 00251 // Initialize leaf iterators 00252 tButServerOcTree & tree( par.map->getTree() ); 00253 srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() ); 00254 00255 // Crawl through nodes 00256 for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it) 00257 { 00258 // Node is occupied? 00259 if (tree.isNodeOccupied(*it)) 00260 { 00261 handleOccupiedNode(it, par); 00262 }// Node is occupied? 00263 00264 } // Iterate through octree 00265 00266 // 2013/01/31 Majkl 00267 m_data->header.frame_id = par.frameId; 00268 m_data->header.stamp = par.currentTime; 00269 00270 m_DataTimeStamp = par.currentTime; 00271 00272 lock.unlock(); 00273 00274 invalidate(); 00275 } 00276 00280 void srs_env_model::CCompressedPointCloudPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapWithParameters & mp) 00281 { 00282 // PERROR("OnHandleOccupied"); 00283 00284 ++m_countAll; 00285 00286 if( ! m_bCamModelInitialized ) 00287 return; 00288 00289 if( ! m_bPublishComplete ) 00290 { 00291 // Test input point 00292 tf::Point pos(it.getX(), it.getY(), it.getZ()); 00293 tf::Point posRel = m_to_sensor(pos); 00294 cv::Point2d uv = m_camera_model.project3dToPixel(cv::Point3d( posRel.x(), posRel.y(), posRel.z())); 00295 00296 // ignore point if not in sensor cone 00297 if (!inSensorCone(uv)) 00298 return; 00299 } 00300 00301 // Ok, add it... 00302 CPointCloudPlugin::handleOccupiedNode( it, mp ); 00303 ++m_countVisible; 00304 } 00305 00309 void srs_env_model::CCompressedPointCloudPlugin::onCameraChangedCB(const sensor_msgs::CameraInfo::ConstPtr &cam_info) 00310 { 00311 boost::recursive_mutex::scoped_lock lock( m_camPosMutex ); 00312 00313 //PERROR("OnCameraChange."); 00314 00315 // ROS_DEBUG( "CCompressedPointCloudPlugin: onCameraChangedCB" ); 00316 00317 // Set camera position frame id 00318 m_cameraFrameId = cam_info->header.frame_id; 00319 00320 ROS_DEBUG("OctMapPlugin: Set camera info: %d x %d\n", cam_info->height, cam_info->width); 00321 m_camera_model_buffer.fromCameraInfo(*cam_info); 00322 m_camera_size_buffer = m_camera_model_buffer.fullResolution(); 00323 00324 // Set flag 00325 m_bCamModelInitialized = true; 00326 00327 m_camera_info_buffer = *cam_info; 00328 } 00329 00331 void srs_env_model::CCompressedPointCloudPlugin::publishInternal(const ros::Time & timestamp) 00332 { 00333 // ROS_DEBUG( "CCompressedPointCloudPlugin: onPublish" ); 00334 00335 // PERROR( "Visible: " << m_countVisible << ", all: " << m_countAll << ", compression: " << double(m_countVisible)/double(m_countAll) ); 00336 // PERROR( "Num of points: " << m_data->size() ); 00337 // srs_env_model::CPointCloudPlugin::publishInternal( timestamp ); 00338 00339 if( shouldPublish() ) 00340 { 00341 // Fill header information 00342 m_octomap_updates_msg->header = m_data->header; 00343 00344 // Majkl 2013/1/24: trying to solve empty header of the output 00345 m_octomap_updates_msg->header.stamp = m_DataTimeStamp; 00346 m_octomap_updates_msg->header.frame_id = m_frame_id; 00347 00348 // Convert data 00349 pcl::toROSMsg< tPclPoint >(*m_data, m_octomap_updates_msg->pointcloud2); 00350 00351 // Set message parameters and publish 00352 m_octomap_updates_msg->pointcloud2.header.frame_id = m_frame_id; 00353 // m_octomap_updates_msg->pointcloud2.header.stamp = timestamp; 00354 00355 // Majkl 2013/1/24: trying to solve empty header of the output 00356 m_octomap_updates_msg->pointcloud2.header.stamp = m_DataTimeStamp; 00357 00358 if( m_bPublishComplete ) 00359 { 00360 // std::cerr << "Publishing complete cloud. Size: " << m_data->points.size() << std::endl; 00361 m_octomap_updates_msg->isPartial = 0; 00362 } 00363 else 00364 { 00365 // std::cerr << "Publishing only differential cloud. Size: " << m_data->points.size() << std::endl; 00366 m_octomap_updates_msg->isPartial = 1; 00367 } 00368 00369 // Publish data 00370 m_ocUpdatePublisher.publish( m_octomap_updates_msg ); 00371 00372 if( m_bPublishSimpleCloud ) 00373 m_pcPublisher.publish( m_octomap_updates_msg->pointcloud2 ); 00374 } 00375 } 00376 00380 void srs_env_model::CCompressedPointCloudPlugin::pause( bool bPause, ros::NodeHandle & node_handle) 00381 { 00382 boost::recursive_mutex::scoped_lock lock( m_camPosMutex ); 00383 00384 if( bPause ) 00385 { 00386 m_pcPublisher.shutdown(); 00387 m_camPosSubscriber.shutdown(); 00388 } 00389 else 00390 { 00391 // Create publisher 00392 m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 5, m_latchedTopics); 00393 00394 // Subscribe to position topic 00395 // Create subscriber 00396 m_camPosSubscriber = node_handle.subscribe<sensor_msgs::CameraInfo>( m_cameraInfoTopic, 10, &srs_env_model::CCompressedPointCloudPlugin::onCameraChangedCB, this ); 00397 } 00398 } 00399 00403 bool srs_env_model::CCompressedPointCloudPlugin::inSensorCone(const cv::Point2d& uv) const 00404 { 00405 //PERROR( uv.x << " > " << m_camera_stereo_offset_left + 1 << " && " << uv.x << " < " << m_camera_size.width + m_camera_stereo_offset_right - 2 ); 00406 //PERROR( uv.y << " > " << 1 << " && " << uv.y << " < " << m_camera_size.height - 2 ); 00407 // Check if projected 2D coordinate in pixel range. 00408 // This check is a little more restrictive than it should be by using 00409 // 1 pixel less to account for rounding / discretization errors. 00410 // Otherwise points on the corner are accounted to be in the sensor cone. 00411 return ((uv.x > m_camera_stereo_offset_left + 1) && 00412 (uv.x < m_camera_size.width + m_camera_stereo_offset_right - 2) && 00413 (uv.y > 1) && 00414 (uv.y < m_camera_size.height - 2)); 00415 } 00416 00420 bool srs_env_model::CCompressedPointCloudPlugin::shouldPublish() 00421 { 00422 ROS_DEBUG( "CCompressedPointCloudPlugin: shouldPublish" ); 00423 00424 bool rv( m_bCamModelInitialized && m_publishPointCloud && (m_pcPublisher.getNumSubscribers() > 0 || m_ocUpdatePublisher.getNumSubscribers() > 0 ) ); 00425 00426 if( !rv ) 00427 { 00428 // PERROR( "Should not publish." << int(m_bCamModelInitialized) << "." << int(m_publishPointCloud) << "." << int(m_pcPublisher.getNumSubscribers() > 0)); 00429 } 00430 00431 return rv; 00432 } 00433 00437 bool srs_env_model::CCompressedPointCloudPlugin::setNumIncompleteFramesCB( srs_env_model::SetNumIncompleteFrames::Request & req, srs_env_model::SetNumIncompleteFrames::Response & res ) 00438 { 00439 m_use_every_nth = req.num; 00440 PERROR( "New number of incomplete frames set: " << m_use_every_nth ); 00441 00442 return true; 00443 }