$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/limited_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::CLimitedPointCloudPlugin::CLimitedPointCloudPlugin( const std::string & name ) 00038 : srs_env_model::CPointCloudPlugin( name, false ) 00039 , m_bTransformCamera( false ) 00040 , m_bSpinThread( true ) 00041 { 00042 } 00043 00047 srs_env_model::CLimitedPointCloudPlugin::~CLimitedPointCloudPlugin() 00048 { 00049 if (spin_thread_.get()) 00050 { 00051 need_to_terminate_ = true; 00052 spin_thread_->join(); 00053 } 00054 } 00055 00059 void srs_env_model::CLimitedPointCloudPlugin::spinThread() 00060 { 00061 while (node_handle_.ok()) 00062 { 00063 if (need_to_terminate_) 00064 { 00065 break; 00066 } 00067 callback_queue_.callAvailable(ros::WallDuration(0.033f)); 00068 } 00069 } 00070 00071 void srs_env_model::CLimitedPointCloudPlugin::init(ros::NodeHandle & node_handle) 00072 { 00073 if ( m_bSpinThread ) 00074 { 00075 // if we're spinning our own thread, we'll also need our own callback queue 00076 node_handle.setCallbackQueue( &callback_queue_ ); 00077 00078 need_to_terminate_ = false; 00079 spin_thread_.reset( new boost::thread(boost::bind(&CLimitedPointCloudPlugin::spinThread, this)) ); 00080 node_handle_ = node_handle; 00081 } 00082 00083 // Read parameters 00084 node_handle.param("rviz_camera_position_topic_name", m_cameraPositionTopic, SUBSCRIBER_CAMERA_POSITION_NAME ); 00085 00086 // Point cloud publishing topic name 00087 node_handle.param("pointcloud_centers_publisher", m_pcPublisherName, VISIBLE_POINTCLOUD_CENTERS_PUBLISHER_NAME ); 00088 00089 // 2013/01/31 Majkl: I guess we should publish the map in the Octomap TF frame... 00090 node_handle.param("ocmap_frame_id", m_frame_id, m_frame_id); 00091 00092 // Create publisher 00093 m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 5, m_latchedTopics); 00094 00095 // Subscribe to position topic 00096 // Create subscriber 00097 m_camPosSubscriber = node_handle.subscribe<srs_env_model_msgs::RVIZCameraPosition>( m_cameraPositionTopic, 10, &srs_env_model::CLimitedPointCloudPlugin::onCameraPositionChangedCB, this ); 00098 // = new message_filters::Subscriber<srs_env_model_msgs::RVIZCameraPosition>(node_handle, cameraPositionTopic, 1); 00099 00100 if (!m_camPosSubscriber) 00101 { 00102 ROS_ERROR("Not subscribed..."); 00103 PERROR( "Cannot subscribe to the camera position publisher..." ); 00104 } 00105 /* 00106 // Create message filter 00107 m_tfCamPosSub = new tf::MessageFilter<srs_env_model_msgs::RVIZCameraPosition>( *m_camPosSubscriber, m_tfListener, "/map", 1); 00108 m_tfCamPosSub->registerCallback(boost::bind( &CLimitedPointCloudPlugin::onCameraPositionChangedCB, this, _1)); 00109 */ 00110 // Clear old pointcloud data 00111 m_data->clear(); 00112 } 00113 00118 void srs_env_model::CLimitedPointCloudPlugin::newMapDataCB( SMapWithParameters & par ) 00119 { 00120 // Reset counters 00121 m_countVisible = m_countHidden = 0; 00122 00123 min = 10000000; 00124 max = -10000000; 00125 00126 if( m_cameraFrameId.size() == 0 ) 00127 { 00128 m_bTransformCamera = false; 00129 return; 00130 } 00131 00132 if( ! m_publishPointCloud ) 00133 return; 00134 00135 // Just for sure 00136 if(m_frame_id != par.frameId) 00137 { 00138 PERROR("Map frame id has changed, this should never happen. Exiting newMapDataCB."); 00139 return; 00140 } 00141 00142 // Clear data 00143 m_data->clear(); 00144 m_DataTimeStamp = m_time_stamp = par.currentTime; 00145 counter = 0; 00146 00147 // Pointcloud is used as output for octomap... 00148 m_bAsInput = false; 00149 00150 00151 00152 // m_bTransformCamera = m_cameraFrameId != m_ocFrameId; 00153 bool m_bTransformCamera = m_cameraFrameId != m_frame_id; 00154 00155 // If different frame id 00156 if( m_bTransformCamera ) 00157 { 00158 // Some transforms 00159 tf::StampedTransform camToOcTf; 00160 00161 // Get transforms 00162 try { 00163 // Transformation - from, to, time, waiting time 00164 m_tfListener.waitForTransform(m_frame_id, m_cameraFrameId, 00165 par.currentTime, ros::Duration(5)); 00166 00167 m_tfListener.lookupTransform(m_frame_id, m_cameraFrameId, 00168 par.currentTime, camToOcTf); 00169 00170 } catch (tf::TransformException& ex) { 00171 ROS_ERROR_STREAM( m_name << ": Transform error - " << ex.what() << ", quitting callback"); 00172 PERROR( "Camera FID: " << m_cameraFrameId << ", Octomap FID: " << m_frame_id ); 00173 return; 00174 } 00175 // PERROR( "Camera FID: " << m_cameraFrameId << ", Octomap FID: " << m_ocFrameId ); 00176 00177 // Get transformation matrix 00178 Eigen::Matrix4f cameraToOcTM; 00179 pcl_ros::transformAsMatrix(camToOcTf, cameraToOcTM); // Sensor TF to defined base TF 00180 00181 // Get translation and rotation 00182 m_camToOcRot = cameraToOcTM.block<3, 3> (0, 0); 00183 m_camToOcTrans = cameraToOcTM.block<3, 1> (0, 3); 00184 00185 // PERROR( "Camera position: " << m_camToOcTrans ); 00186 } 00187 00188 // Copy buffered camera normal and d parameter 00189 boost::recursive_mutex::scoped_lock lock( m_camPosMutex ); 00190 00191 // PERROR( "Copy position..."); 00192 m_d = m_dBuf; 00193 m_normal = m_normalBuf; 00194 00195 tButServerOcTree & tree( par.map->getTree() ); 00196 srs_env_model::tButServerOcTree::leaf_iterator it, itEnd( tree.end_leafs() ); 00197 00198 // Crawl through nodes 00199 for ( it = tree.begin_leafs(m_crawlDepth); it != itEnd; ++it) 00200 { 00201 // Node is occupied? 00202 if (tree.isNodeOccupied(*it)) 00203 { 00204 handleOccupiedNode(it, par); 00205 }// Node is occupied? 00206 00207 } // Iterate through octree 00208 00209 // 2013/01/31 Majkl 00210 m_data->header.frame_id = par.frameId; 00211 m_data->header.stamp = par.currentTime; 00212 00213 m_DataTimeStamp = par.currentTime; 00214 00215 lock.unlock(); 00216 00217 invalidate(); 00218 } 00219 00223 void srs_env_model::CLimitedPointCloudPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapWithParameters & mp) 00224 { 00225 Eigen::Vector3f p( it.getX(), it.getY(), it.getZ() ); 00226 00227 float f( p.dot( m_normal ) + m_d); 00228 if( f > max ) 00229 max = f; 00230 if( f < min) 00231 min = f; 00232 00233 // Test if point is in front of camera 00234 if( (p.dot( m_normal ) + m_d) > 0 ) 00235 { 00236 // Ok, add it... 00237 CPointCloudPlugin::handleOccupiedNode( it, mp ); 00238 ++m_countVisible; 00239 } 00240 else 00241 { 00242 ++m_countHidden; 00243 // PERROR( "Point behind camera... "); 00244 } 00245 } 00246 00250 void srs_env_model::CLimitedPointCloudPlugin::onCameraPositionChangedCB(const srs_env_model_msgs::RVIZCameraPosition::ConstPtr& cameraPosition) 00251 { 00252 // Set camera position frame id 00253 m_cameraFrameId = cameraPosition->header.frame_id; 00254 00255 // Orientation shortcut 00256 const srs_env_model_msgs::RVIZCameraPosition::_position_type & p( cameraPosition->position ); 00257 00258 // Camera direction 00259 const srs_env_model_msgs::RVIZCameraPosition::_position_type & d( cameraPosition->direction ); 00260 00261 // Convert to eigen 00262 Eigen::Vector3f point( p.x, p.y, p.z ); 00263 Eigen::Vector3f normal( d.x, d.y, d.z ); 00264 00265 00266 if( m_bTransformCamera ) 00267 { 00268 // PERROR( "Transform camera"); 00269 // Transform point to the octomap frame id 00270 point = m_camToOcRot * point + m_camToOcTrans; 00271 00272 // Transform normal 00273 normal = m_camToOcRot * normal; 00274 } 00275 00276 // Normalize normal vector 00277 normal.normalize(); 00278 00279 // Set parameters to the buffer 00280 boost::recursive_mutex::scoped_lock lock( m_camPosMutex ); 00281 00282 m_normalBuf = normal; 00283 00284 // Compute last plane equation parameter 00285 m_dBuf = - point.dot( normal ); 00286 00287 // PERROR( "Equation: " << normal[0] << ", " << normal[1] << ", " << normal[2] << ", " << m_d ); 00288 // PERROR( "Camera position: " << point[0] << ", " << point[1] << ", " << point[2] ); 00289 } 00290 00292 void srs_env_model::CLimitedPointCloudPlugin::publishInternal(const ros::Time & timestamp) 00293 { 00294 // PERROR( "Visible: " << m_countVisible << ", hidden: " << m_countHidden << ", min: " << min << ", max: " << max ); 00295 // PERROR( "Num of points: " << m_data->size() ); 00296 srs_env_model::CPointCloudPlugin::publishInternal( timestamp ); 00297 } 00298 00302 void srs_env_model::CLimitedPointCloudPlugin::pause( bool bPause, ros::NodeHandle & node_handle) 00303 { 00304 boost::recursive_mutex::scoped_lock lock( m_camPosMutex ); 00305 00306 if( bPause ) 00307 { 00308 m_pcPublisher.shutdown(); 00309 m_camPosSubscriber.shutdown(); 00310 } 00311 else 00312 { 00313 // Create publisher 00314 m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 5, m_latchedTopics); 00315 00316 // Subscribe to position topic 00317 // Create subscriber 00318 m_camPosSubscriber = node_handle.subscribe<srs_env_model_msgs::RVIZCameraPosition>( m_cameraPositionTopic, 10, &srs_env_model::CLimitedPointCloudPlugin::onCameraPositionChangedCB, this ); 00319 } 00320 } 00321 00322 00323