$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/point_cloud_plugin.h> 00029 #include <srs_env_model/topics_list.h> 00030 00031 #include <pcl/ros/conversions.h> 00032 #include <pcl_ros/transforms.h> 00033 #include <pcl/filters/passthrough.h> 00034 #include <pcl/io/io.h> 00035 #include "pcl_ros/pcl_nodelet.h" 00036 #include <pcl/io/pcd_io.h> 00037 #include <pcl/point_types.h> 00038 00039 #define DEFAULT_INPUT_CLOUD_FRAME_ID "/head_cam3d_link" 00040 00042 srs_env_model::CPointCloudPlugin::CPointCloudPlugin(const std::string & name, bool subscribe) 00043 : srs_env_model::CServerPluginBase(name) 00044 , m_publishPointCloud(true) 00045 , m_pcPublisherName(POINTCLOUD_CENTERS_PUBLISHER_NAME) 00046 , m_pcSubscriberName("") 00047 , m_inputPcFrameId(DEFAULT_INPUT_CLOUD_FRAME_ID) 00048 , m_bSubscribe( subscribe ) 00049 , m_latchedTopics( false ) 00050 , m_bFilterPC(true) 00051 , m_pointcloudMinZ(-std::numeric_limits<double>::max()) 00052 , m_pointcloudMaxZ(std::numeric_limits<double>::max()) 00053 , m_oldCloud( new tPointCloud ) 00054 , m_bufferCloud( new tPointCloud ) 00055 , m_frame_number( 0 ) 00056 , m_use_every_nth( 1 ) 00057 , m_bUseInputColor(true) 00058 , m_r(128) 00059 , m_g(128) 00060 , m_b(128) 00061 { 00062 assert( m_data != 0 ); 00063 m_frame_id = "/map"; 00064 } 00065 00067 srs_env_model::CPointCloudPlugin::~CPointCloudPlugin() 00068 { 00069 00070 } 00071 00073 void srs_env_model::CPointCloudPlugin::init(ros::NodeHandle & node_handle) 00074 { 00075 // PERROR( "Initializing PointCloudPlugin" ); 00076 00077 // Frame skipping 00078 int fs( m_use_every_nth ); 00079 node_handle.param( "pointcloud_frame_skip", fs, 1 ); 00080 m_use_every_nth = (fs >= 1) ? fs : 1; 00081 00082 // Point cloud publishing topic name 00083 node_handle.param("pointcloud_centers_publisher", m_pcPublisherName, POINTCLOUD_CENTERS_PUBLISHER_NAME ); 00084 00085 // Point cloud subscribing topic name - try to get it from parameter server if not given 00086 if( m_pcSubscriberName.length() == 0 ) 00087 node_handle.param("pointcloud_subscriber", m_pcSubscriberName, SUBSCRIBER_POINT_CLOUD_NAME); 00088 else 00089 m_bSubscribe = true; 00090 00091 // 2013/01/31 Majkl: I guess we should publish the map in the Octomap TF frame... 00092 // We will use the same frame id as octomap plugin 00093 node_handle.param("ocmap_frame_id", m_frame_id, m_frame_id); 00094 00095 // Point cloud limits 00096 node_handle.param("pointcloud_min_z", m_pointcloudMinZ, m_pointcloudMinZ); 00097 node_handle.param("pointcloud_max_z", m_pointcloudMaxZ, m_pointcloudMaxZ); 00098 00099 // Use input color default state 00100 node_handle.param("pointcloud_use_input_color", m_bUseInputColor, m_bUseInputColor); 00101 00102 // Default color 00103 int c; 00104 c = m_r; node_handle.param("pointcloud_default_color_r", c, c); m_r = c; 00105 c = m_g; node_handle.param("pointcloud_default_color_g", c, c); m_g = c; 00106 c = m_b; node_handle.param("pointcloud_default_color_b", c, c); m_b = c; 00107 00108 // std::cerr << "Use input color: " << std::string(m_bUseInputColor ? "yes" : "no") << std::endl; 00109 // std::cerr << "Color: " << m_r << ", " << m_g << ", " << m_b << std::endl; 00110 00111 // Create publisher 00112 m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 5, m_latchedTopics); 00113 00114 00115 // If should subscribe, create message filter and connect to the topic 00116 if( m_bSubscribe ) 00117 { 00118 std::cerr << "Plugin name: " << this->m_name << std::endl; 00119 00120 // PERROR("Subscribing to: " << m_pcSubscriberName ); 00121 // Create subscriber 00122 m_pcSubscriber = new message_filters::Subscriber<tIncommingPointCloud>(node_handle, m_pcSubscriberName, 1); 00123 00124 if (!m_pcSubscriber) 00125 { 00126 ROS_ERROR("Not subscribed to point clouds subscriber..."); 00127 } 00128 00129 // Create message filter 00130 m_tfPointCloudSub = new tf::MessageFilter<tIncommingPointCloud>( *m_pcSubscriber, m_tfListener, m_inputPcFrameId, 1); 00131 m_tfPointCloudSub->registerCallback(boost::bind( &CPointCloudPlugin::insertCloudCallback, this, _1)); 00132 00133 //std::cerr << "SUBSCRIBER NAME: " << m_pcSubscriberName << ", FRAMEID: " << m_pcFrameId << std::endl; 00134 } 00135 00136 // Clear old pointcloud data 00137 m_data->clear(); 00138 00139 // PERROR( "PointCloudPlugin initialized..." ); 00140 } 00141 00143 void srs_env_model::CPointCloudPlugin::publishInternal(const ros::Time & timestamp) 00144 { 00145 // PERROR("Publish: Try lock"); 00146 00147 boost::mutex::scoped_lock lock(m_lockData); 00148 00149 // PERROR("Publish: Lock"); 00150 00151 // No subscriber or disabled 00152 if( ! shouldPublish() ) 00153 return; 00154 00155 // No data... 00156 if( m_data->size() == 0 ) 00157 return; 00158 00159 // Convert data 00160 sensor_msgs::PointCloud2 cloud; 00161 pcl::toROSMsg< tPclPoint >(*m_data, cloud); 00162 00163 // Set message parameters and publish 00164 if( m_data->header.frame_id != m_frame_id ) 00165 { 00166 ROS_ERROR("CPointCloudPlugin::publishInternal: Internal frame id is not compatible with the output one."); 00167 return; 00168 } 00169 cloud.header.frame_id = m_frame_id; 00170 cloud.header.stamp = timestamp; 00171 00172 // PERROR( "Publishing cloud. Size: " << m_data->size() << ", topic: " << m_pcPublisher.getTopic() ); 00173 m_pcPublisher.publish(cloud); 00174 00175 // PERROR("Publish: Unlock"); 00176 } 00177 00179 void srs_env_model::CPointCloudPlugin::newMapDataCB( SMapWithParameters & par ) 00180 { 00181 if( ! m_publishPointCloud ) 00182 return; 00183 00184 // PERROR("New map: Try lock"); 00185 00186 boost::mutex::scoped_lock lock(m_lockData); 00187 00188 // PERROR("New map: Lock"); 00189 00190 m_data->clear(); 00191 00192 // Just for sure 00193 if(m_frame_id != par.frameId) 00194 { 00195 ROS_ERROR("Map frame id has changed, this should never happen. Exiting newMapDataCB."); 00196 return; 00197 } 00198 00199 m_frame_id = par.frameId; 00200 m_DataTimeStamp = m_time_stamp = par.currentTime; 00201 counter = 0; 00202 00203 // Pointcloud is used as output for octomap... 00204 m_bAsInput = false; 00205 00206 // Initialize leaf iterators 00207 tButServerOcTree & tree( par.map->getTree() ); 00208 srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() ); 00209 00210 // Crawl through nodes 00211 for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it) 00212 { 00213 // Node is occupied? 00214 if (tree.isNodeOccupied(*it)) 00215 { 00216 handleOccupiedNode(it, par); 00217 }// Node is occupied? 00218 00219 } // Iterate through octree 00220 00221 // 2013/01/31 Majkl 00222 m_data->header.frame_id = par.frameId; 00223 m_data->header.stamp = par.currentTime; 00224 00225 lock.unlock(); 00226 // PERROR( "New map: Unlocked"); 00227 00228 invalidate(); 00229 } 00230 00232 void srs_env_model::CPointCloudPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapWithParameters & mp) 00233 { 00234 // std::cerr << "PCP: handle occupied" << std::endl; 00235 tPclPoint point; 00236 00237 // Set position 00238 point.x = it.getX(); 00239 point.y = it.getY(); 00240 point.z = it.getZ(); 00241 00242 // Set color 00243 point.r = it->r(); 00244 point.g = it->g(); 00245 point.b = it->b(); 00246 00247 // std::cerr << "Occupied node r " << (int)point.r << ", g " << (int)point.g << ", b " << (int)point.b << std::endl; 00248 /* 00249 // Set color 00250 point.r = 255 - counter % 255; 00251 point.g = counter % 255; 00252 point.b = 128; 00253 */ 00254 m_data->push_back( point ); 00255 00256 ++counter; 00257 } 00258 00260 00264 void srs_env_model::CPointCloudPlugin::insertCloudCallback( const tIncommingPointCloud::ConstPtr& cloud) 00265 { 00266 // PERROR("insertCloud: Try lock"); 00267 00268 boost::mutex::scoped_lock lock(m_lockData); 00269 00270 // PERROR("insertCloud: Locked"); 00271 00272 if( ! useFrame() ) 00273 { 00274 // std::cerr << "Frame skipping" << std::endl; 00275 return; 00276 } 00277 00278 // std::cerr << "PCP.iccb start. Time: " << ros::Time::now() << std::endl; 00279 00280 m_bAsInput = true; 00281 00282 // Convert input pointcloud 00283 m_data->clear(); 00284 bool bIsRgbCloud(isRGBCloud( cloud )); 00285 00286 if( !bIsRgbCloud ) 00287 { 00288 pcl::PointCloud< pcl::PointXYZ >::Ptr bufferCloud( new pcl::PointCloud< pcl::PointXYZ> ); 00289 00290 pcl::fromROSMsg(*cloud, *bufferCloud ); 00291 pcl::copyPointCloud< pcl::PointXYZ, tPclPoint >( *bufferCloud, *m_data ); 00292 } 00293 else 00294 { 00295 pcl::PointCloud< pcl::PointXYZRGB >::Ptr bufferCloud( new pcl::PointCloud< pcl::PointXYZRGB > ); 00296 00297 pcl::fromROSMsg(*cloud, *bufferCloud); 00298 pcl::copyPointCloud<pcl::PointXYZRGB, tPclPoint>( *bufferCloud, *m_data ); 00299 } 00300 00301 if( !(bIsRgbCloud && m_bUseInputColor) ) 00302 { 00303 // Use our default color to colorize cloud 00304 tPointCloud::iterator it, itEnd(m_data->points.end()); 00305 for( it = m_data->points.begin(); it != itEnd; ++it) 00306 { 00307 it->r = m_r; it->g = m_g; it->b = m_b; 00308 } 00309 } 00310 00311 //*/ 00312 // std::cerr << "Input cloud frame id: " << cloud->header.frame_id << std::endl; 00313 00314 // If different frame id 00315 // if( cloud->header.frame_id != m_frame_id ) 00316 if( cloud->header.frame_id != m_inputPcFrameId ) 00317 { 00318 // PERROR( "Wait for input transform" ); 00319 00320 // Some transforms 00321 tf::StampedTransform sensorToPcTf; 00322 00323 // Get transforms 00324 try { 00325 // Transformation - from, to, time, waiting time 00326 // m_tfListener.waitForTransform(m_frame_id, cloud->header.frame_id, 00327 m_tfListener.waitForTransform(m_inputPcFrameId, cloud->header.frame_id, 00328 cloud->header.stamp, ros::Duration(5)); 00329 00330 // m_tfListener.lookupTransform(m_frame_id, cloud->header.frame_id, 00331 m_tfListener.lookupTransform(m_inputPcFrameId, cloud->header.frame_id, 00332 cloud->header.stamp, sensorToPcTf); 00333 00334 } catch (tf::TransformException& ex) { 00335 ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback"); 00336 PERROR("Transform error"); 00337 00338 return; 00339 } 00340 00341 Eigen::Matrix4f sensorToPcTM; 00342 00343 // Get transformation matrix 00344 pcl_ros::transformAsMatrix(sensorToPcTf, sensorToPcTM); // Sensor TF to defined base TF 00345 00346 // transform pointcloud from sensor frame to the preset frame 00347 pcl::transformPointCloud< tPclPoint >(*m_data, *m_data, sensorToPcTM); 00348 m_data->header = cloud->header; 00349 // m_data->header.frame_id = m_frame_id; 00350 m_data->header.frame_id = m_inputPcFrameId; 00351 } 00352 00353 // PERROR("1"); 00354 00355 // PERROR("2"); 00356 00357 // Filter input pointcloud 00358 if( m_bFilterPC ) // TODO: Optimize this by removing redundant transforms 00359 { 00360 // PERROR( "Wait for filtering transform"); 00361 00362 // Get transforms to and from base id 00363 tf::StampedTransform pcToBaseTf, baseToPcTf; 00364 try { 00365 // Transformation - to, from, time, waiting time 00366 // m_tfListener.waitForTransform(BASE_FRAME_ID, m_frame_id, 00367 m_tfListener.waitForTransform(BASE_FRAME_ID, m_inputPcFrameId, 00368 cloud->header.stamp, ros::Duration(5)); 00369 00370 // m_tfListener.lookupTransform(BASE_FRAME_ID, m_frame_id, 00371 m_tfListener.lookupTransform(BASE_FRAME_ID, m_inputPcFrameId, 00372 cloud->header.stamp, pcToBaseTf); 00373 00374 // m_tfListener.lookupTransform(m_frame_id, BASE_FRAME_ID, 00375 m_tfListener.lookupTransform(m_inputPcFrameId, BASE_FRAME_ID, 00376 cloud->header.stamp, baseToPcTf ); 00377 00378 } catch (tf::TransformException& ex) { 00379 ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback"); 00380 PERROR( "Transform error."); 00381 return; 00382 } 00383 00384 Eigen::Matrix4f pcToBaseTM, baseToPcTM; 00385 00386 // Get transformation matrix 00387 pcl_ros::transformAsMatrix(pcToBaseTf, pcToBaseTM); // Sensor TF to defined base TF 00388 pcl_ros::transformAsMatrix(baseToPcTf, baseToPcTM); // Sensor TF to defined base TF 00389 00390 // transform pointcloud from pc frame to the base frame 00391 pcl::transformPointCloud< tPclPoint >(*m_data, *m_data, pcToBaseTM); 00392 00393 // filter height and range, also removes NANs: 00394 pcl::PassThrough<tPclPoint> pass; 00395 pass.setFilterFieldName("z"); 00396 pass.setFilterLimits(m_pointcloudMinZ, m_pointcloudMaxZ); 00397 pass.setInputCloud(m_data->makeShared()); 00398 pass.filter(*m_data); 00399 00400 // transform pointcloud back to pc frame from the base frame 00401 pcl::transformPointCloud< tPclPoint >(*m_data, *m_data, baseToPcTM); 00402 } 00403 00404 // Modify header 00405 m_data->header = cloud->header; 00406 // m_data->header.frame_id = m_frame_id; 00407 m_data->header.frame_id = m_inputPcFrameId; 00408 00409 // Store timestamp 00410 m_DataTimeStamp = cloud->header.stamp; 00411 00412 // ROS_DEBUG("CPointCloudPlugin::insertCloudCallback(): stamp = %f", cloud->header.stamp.toSec()); 00413 00414 // PERROR("Insert cloud CB. Size: " << m_data->size() ); 00415 00416 // Unlock for invalidation (it has it's own lock) 00417 lock.unlock(); 00418 // PERROR("insertCloud: Unlocked"); 00419 00420 invalidate(); 00421 00422 // PERROR("Unlock"); 00423 } 00424 00426 bool srs_env_model::CPointCloudPlugin::shouldPublish() 00427 { 00428 return( (!m_bAsInput) && m_publishPointCloud && m_pcPublisher.getNumSubscribers() > 0 ); 00429 } 00430 00434 bool srs_env_model::CPointCloudPlugin::isRGBCloud( const tIncommingPointCloud::ConstPtr& cloud ) 00435 { 00436 tIncommingPointCloud::_fields_type::const_iterator i, end; 00437 00438 for( i = cloud->fields.begin(), end = cloud->fields.end(); i != end; ++i ) 00439 { 00440 if( i->name == "rgb" ) 00441 { 00442 // PERROR("HAS RGB"); 00443 return true; 00444 } 00445 } 00446 00447 // PERROR("NO RGB"); 00448 00449 return false; 00450 } 00451 00455 void srs_env_model::CPointCloudPlugin::pause( bool bPause, ros::NodeHandle & node_handle ) 00456 { 00457 // PERROR("Try lock"); 00458 00459 boost::mutex::scoped_lock lock(m_lockData); 00460 00461 // PERROR("Lock"); 00462 00463 if( bPause ) 00464 { 00465 m_pcPublisher.shutdown(); 00466 00467 if( m_bSubscribe ) 00468 { 00469 00470 m_pcSubscriber->unsubscribe(); 00471 00472 m_tfPointCloudSub->clear(); 00473 00474 delete m_tfPointCloudSub; 00475 delete m_pcSubscriber; 00476 } 00477 } 00478 else 00479 { 00480 // Create publisher 00481 m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 5, m_latchedTopics); 00482 00483 if( m_bSubscribe ) 00484 { 00485 m_pcSubscriber = new message_filters::Subscriber<tIncommingPointCloud>(node_handle, m_pcSubscriberName, 1); 00486 00487 // Create message filter 00488 // m_tfPointCloudSub = new tf::MessageFilter<tIncommingPointCloud>( *m_pcSubscriber, m_tfListener, m_frame_id, 1); 00489 m_tfPointCloudSub = new tf::MessageFilter<tIncommingPointCloud>( *m_pcSubscriber, m_tfListener, m_inputPcFrameId, 1); 00490 m_tfPointCloudSub->registerCallback(boost::bind( &CPointCloudPlugin::insertCloudCallback, this, _1)); 00491 } 00492 } 00493 00494 // PERROR("Unlock"); 00495 00496 } 00497 00501 bool srs_env_model::CPointCloudPlugin::wantsMap() 00502 { 00503 return ! m_bAsInput; 00504 } 00505 00506