void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud){ try { //actually convert the PointCloud2 message into a type we can reason about pcl::PointCloud<pcl::PointXYZ> pcl_cloud; pcl::fromROSMsg(cloud, pcl_cloud); bufferCloud(pcl_cloud); } catch(pcl::PCLException& ex){ ROS_ERROR("Failed to convert a message to a pcl type, dropping observation: %s", ex.what()); return; } }
/** Cloud insertion callback */ void srs_env_model::CPointCloudPlugin::insertCloudCallback( const tIncommingPointCloud::ConstPtr& cloud) { // PERROR("Try lock"); boost::mutex::scoped_lock lock(m_lockData); // PERROR("Lock"); if( ! useFrame() ) { PERROR("Skipping frame: " << cloud->header.stamp); return; } // std::cerr << "PCP.iccb start. Time: " << ros::Time::now() << std::endl; ros::WallTime startTime = ros::WallTime::now(); m_bAsInput = true; // Convert input pointcloud m_data->clear(); if( ! isRGBCloud( cloud ) ) { pcl::PointCloud< pcl::PointXYZ >::Ptr bufferCloud( new pcl::PointCloud< pcl::PointXYZ> ); pcl::fromROSMsg(*cloud, *bufferCloud ); pcl::copyPointCloud< pcl::PointXYZ, tPclPoint >( *bufferCloud, *m_data ); } else { pcl::PointCloud< pcl::PointXYZRGB >::Ptr bufferCloud( new pcl::PointCloud< pcl::PointXYZRGB > ); pcl::fromROSMsg(*cloud, *bufferCloud); pcl::copyPointCloud<pcl::PointXYZRGB, tPclPoint>( *bufferCloud, *m_data ); } //*/ // If different frame id if( cloud->header.frame_id != m_pcFrameId ) { // PERROR( "Wait for input transform" ); // Some transforms tf::StampedTransform sensorToPcTf; // Get transforms try { // Transformation - from, to, time, waiting time m_tfListener.waitForTransform(m_pcFrameId, cloud->header.frame_id, cloud->header.stamp, ros::Duration(5)); m_tfListener.lookupTransform(m_pcFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToPcTf); } catch (tf::TransformException& ex) { ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback"); PERROR("Transform error"); return; } Eigen::Matrix4f sensorToPcTM; // Get transformation matrix pcl_ros::transformAsMatrix(sensorToPcTf, sensorToPcTM); // Sensor TF to defined base TF // transform pointcloud from sensor frame to the preset frame pcl::transformPointCloud< tPclPoint >(*m_data, *m_data, sensorToPcTM); } // Filter input pointcloud if( m_bFilterPC ) // TODO: Optimize this by removing redundant transforms { // PERROR( "Wait for filtering transform"); // Get transforms to and from base id tf::StampedTransform pcToBaseTf, baseToPcTf; try { // Transformation - to, from, time, waiting time m_tfListener.waitForTransform(BASE_FRAME_ID, m_pcFrameId, cloud->header.stamp, ros::Duration(5)); m_tfListener.lookupTransform(BASE_FRAME_ID, m_pcFrameId, cloud->header.stamp, pcToBaseTf); m_tfListener.lookupTransform(m_pcFrameId, BASE_FRAME_ID, cloud->header.stamp, baseToPcTf ); } catch (tf::TransformException& ex) { ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback"); PERROR( "Transform error."); return; } Eigen::Matrix4f pcToBaseTM, baseToPcTM; // Get transformation matrix pcl_ros::transformAsMatrix(pcToBaseTf, pcToBaseTM); // Sensor TF to defined base TF pcl_ros::transformAsMatrix(baseToPcTf, baseToPcTM); // Sensor TF to defined base TF // transform pointcloud from pc frame to the base frame pcl::transformPointCloud< tPclPoint >(*m_data, *m_data, pcToBaseTM); // filter height and range, also removes NANs: pcl::PassThrough<tPclPoint> pass; pass.setFilterFieldName("z"); pass.setFilterLimits(m_pointcloudMinZ, m_pointcloudMaxZ); pass.setInputCloud(m_data->makeShared()); pass.filter(*m_data); // transform pointcloud back to pc frame from the base frame pcl::transformPointCloud< tPclPoint >(*m_data, *m_data, baseToPcTM); } // Modify header m_data->header = cloud->header; m_data->header.frame_id = m_pcFrameId; // PERROR("Insert cloud CB. Size: " << m_data->size() ); invalidate(); // std::cerr << "PCP.iccb end. Time: " << ros::Time::now() << std::endl; // PERROR("Unlock"); }
/** Cloud insertion callback */ void srs_env_model::CPointCloudPlugin::insertCloudCallback( const tIncommingPointCloud::ConstPtr& cloud) { // PERROR("insertCloud: Try lock"); boost::mutex::scoped_lock lock(m_lockData); // PERROR("insertCloud: Locked"); if( ! useFrame() ) { // std::cerr << "Frame skipping" << std::endl; return; } // std::cerr << "PCP.iccb start. Time: " << ros::Time::now() << std::endl; m_bAsInput = true; // Convert input pointcloud m_data->clear(); bool bIsRgbCloud(isRGBCloud( cloud )); if( !bIsRgbCloud ) { pcl::PointCloud< pcl::PointXYZ >::Ptr bufferCloud( new pcl::PointCloud< pcl::PointXYZ> ); pcl::fromROSMsg(*cloud, *bufferCloud ); pcl::copyPointCloud< pcl::PointXYZ, tPclPoint >( *bufferCloud, *m_data ); } else { pcl::PointCloud< pcl::PointXYZRGB >::Ptr bufferCloud( new pcl::PointCloud< pcl::PointXYZRGB > ); pcl::fromROSMsg(*cloud, *bufferCloud); pcl::copyPointCloud<pcl::PointXYZRGB, tPclPoint>( *bufferCloud, *m_data ); } if( !(bIsRgbCloud && m_bUseInputColor) ) { // Use our default color to colorize cloud tPointCloud::iterator it, itEnd(m_data->points.end()); for( it = m_data->points.begin(); it != itEnd; ++it) { it->r = m_r; it->g = m_g; it->b = m_b; } } //*/ // std::cerr << "Input cloud frame id: " << cloud->header.frame_id << std::endl; // If different frame id // if( cloud->header.frame_id != m_frame_id ) if( cloud->header.frame_id != m_inputPcFrameId ) { // PERROR( "Wait for input transform" ); // Some transforms tf::StampedTransform sensorToPcTf; // Get transforms try { // Transformation - from, to, time, waiting time // m_tfListener.waitForTransform(m_frame_id, cloud->header.frame_id, m_tfListener.waitForTransform(m_inputPcFrameId, cloud->header.frame_id, cloud->header.stamp, ros::Duration(5)); // m_tfListener.lookupTransform(m_frame_id, cloud->header.frame_id, m_tfListener.lookupTransform(m_inputPcFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToPcTf); } catch (tf::TransformException& ex) { ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback"); PERROR("Transform error"); return; } Eigen::Matrix4f sensorToPcTM; // Get transformation matrix pcl_ros::transformAsMatrix(sensorToPcTf, sensorToPcTM); // Sensor TF to defined base TF // transform pointcloud from sensor frame to the preset frame pcl::transformPointCloud< tPclPoint >(*m_data, *m_data, sensorToPcTM); m_data->header = cloud->header; // m_data->header.frame_id = m_frame_id; m_data->header.frame_id = m_inputPcFrameId; } // PERROR("1"); // PERROR("2"); // Filter input pointcloud if( m_bFilterPC ) // TODO: Optimize this by removing redundant transforms { // PERROR( "Wait for filtering transform"); // Get transforms to and from base id tf::StampedTransform pcToBaseTf, baseToPcTf; try { // Transformation - to, from, time, waiting time // m_tfListener.waitForTransform(BASE_FRAME_ID, m_frame_id, m_tfListener.waitForTransform(BASE_FRAME_ID, m_inputPcFrameId, cloud->header.stamp, ros::Duration(5)); // m_tfListener.lookupTransform(BASE_FRAME_ID, m_frame_id, m_tfListener.lookupTransform(BASE_FRAME_ID, m_inputPcFrameId, cloud->header.stamp, pcToBaseTf); // m_tfListener.lookupTransform(m_frame_id, BASE_FRAME_ID, m_tfListener.lookupTransform(m_inputPcFrameId, BASE_FRAME_ID, cloud->header.stamp, baseToPcTf ); } catch (tf::TransformException& ex) { ROS_ERROR_STREAM("Transform error: " << ex.what() << ", quitting callback"); PERROR( "Transform error."); return; } Eigen::Matrix4f pcToBaseTM, baseToPcTM; // Get transformation matrix pcl_ros::transformAsMatrix(pcToBaseTf, pcToBaseTM); // Sensor TF to defined base TF pcl_ros::transformAsMatrix(baseToPcTf, baseToPcTM); // Sensor TF to defined base TF // transform pointcloud from pc frame to the base frame pcl::transformPointCloud< tPclPoint >(*m_data, *m_data, pcToBaseTM); // filter height and range, also removes NANs: pcl::PassThrough<tPclPoint> pass; pass.setFilterFieldName("z"); pass.setFilterLimits(m_pointcloudMinZ, m_pointcloudMaxZ); pass.setInputCloud(m_data->makeShared()); pass.filter(*m_data); // transform pointcloud back to pc frame from the base frame pcl::transformPointCloud< tPclPoint >(*m_data, *m_data, baseToPcTM); } // Modify header m_data->header = cloud->header; // m_data->header.frame_id = m_frame_id; m_data->header.frame_id = m_inputPcFrameId; // Store timestamp m_DataTimeStamp = cloud->header.stamp; // ROS_DEBUG("CPointCloudPlugin::insertCloudCallback(): stamp = %f", cloud->header.stamp.toSec()); // PERROR("Insert cloud CB. Size: " << m_data->size() ); // Unlock for invalidation (it has it's own lock) lock.unlock(); // PERROR("insertCloud: Unlocked"); invalidate(); // PERROR("Unlock"); }