Create a gist now

Instantly share code, notes, and snippets.

Drawing Point Cloud retrieve from Velodyne VLP-16
cmake_minimum_required( VERSION 2.8 )
# Create Project
project( solution )
add_executable( project main.cpp )
# Set StartUp Project (Option)
# (This setting is able to enable by using CMake 3.6.0 RC1 or later.)
set_property( DIRECTORY PROPERTY VS_STARTUP_PROJECT "project" )
# Find Packages
find_package( PCL 1.8 REQUIRED )
if( PCL_FOUND )
# [C/C++]>[General]>[Additional Include Directories]
include_directories( ${PCL_INCLUDE_DIRS} )
# [C/C++]>[Preprocessor]>[Preprocessor Definitions]
add_definitions( ${PCL_DEFINITIONS} )
# For Use Not PreCompiled Features
#add_definitions( -DPCL_NO_PRECOMPILE )
# [Linker]>[General]>[Additional Library Directories]
link_directories( ${PCL_LIBRARY_DIRS} )
# [Linker]>[Input]>[Additional Dependencies]
target_link_libraries( project ${PCL_LIBRARIES} )
endif()
#include <iostream>
#include <string>
#include <vector>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/vlp_grabber.h>
#include <pcl/console/parse.h>
#include <pcl/visualization/pcl_visualizer.h>
// Point Type
// pcl::PointXYZ, pcl::PointXYZI, pcl::PointXYZRGBA
typedef pcl::PointXYZI PointType;
int main( int argc, char *argv[] )
{
// Command-Line Argument Parsing
if( pcl::console::find_switch( argc, argv, "-help" ) ){
std::cout << "usage: " << argv[0]
<< " [-ipaddress <192.168.1.70>]"
<< " [-port <2368>]"
<< " [-pcap <*.pcap>]"
<< " [-help]"
<< std::endl;
return 0;
}
std::string ipaddress( "192.168.1.70" );
std::string port( "2368" );
std::string pcap;
pcl::console::parse_argument( argc, argv, "-ipaddress", ipaddress );
pcl::console::parse_argument( argc, argv, "-port", port );
pcl::console::parse_argument( argc, argv, "-pcap", pcap );
std::cout << "-ipadress : " << ipaddress << std::endl;
std::cout << "-port : " << port << std::endl;
std::cout << "-pcap : " << pcap << std::endl;
// Point Cloud
pcl::PointCloud<PointType>::ConstPtr cloud;
// PCL Visualizer
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer( new pcl::visualization::PCLVisualizer( "Velodyne Viewer" ) );
viewer->addCoordinateSystem( 3.0, "coordinate" );
viewer->setBackgroundColor( 0.0, 0.0, 0.0, 0 );
viewer->initCameraParameters();
viewer->setCameraPosition( 0.0, 0.0, 30.0, 0.0, 1.0, 0.0, 0 );
// Point Cloud Color Hndler
pcl::visualization::PointCloudColorHandler<PointType>::Ptr handler;
const std::type_info& type = typeid( PointType );
if( type == typeid( pcl::PointXYZ ) ){
std::vector<double> color = { 255.0, 255.0, 255.0 };
boost::shared_ptr<pcl::visualization::PointCloudColorHandlerCustom<PointType>> color_handler( new pcl::visualization::PointCloudColorHandlerCustom<PointType>( color[0], color[1], color[2] ) );
handler = color_handler;
}
else if( type == typeid( pcl::PointXYZI ) ){
boost::shared_ptr<pcl::visualization::PointCloudColorHandlerGenericField<PointType>> color_handler( new pcl::visualization::PointCloudColorHandlerGenericField<PointType>( "intensity" ) );
handler = color_handler;
}
else if( type == typeid( pcl::PointXYZRGBA ) ){
boost::shared_ptr<pcl::visualization::PointCloudColorHandlerRGBField<PointType>> color_handler( new pcl::visualization::PointCloudColorHandlerRGBField<PointType>() );
handler = color_handler;
}
else{
throw std::runtime_error( "This PointType is unsupported." );
}
// Retrieved Point Cloud Callback Function
boost::mutex mutex;
boost::function<void( const pcl::PointCloud<PointType>::ConstPtr& )> function =
[ &cloud, &mutex ]( const pcl::PointCloud<PointType>::ConstPtr& ptr ){
boost::mutex::scoped_lock lock( mutex );
/* Point Cloud Processing */
cloud = ptr;
};
// VLP Grabber
boost::shared_ptr<pcl::VLPGrabber> grabber;
if( !pcap.empty() ){
std::cout << "Capture from PCAP..." << std::endl;
grabber = boost::shared_ptr<pcl::VLPGrabber>( new pcl::VLPGrabber( pcap ) );
}
else if( !ipaddress.empty() && !port.empty() ){
std::cout << "Capture from Sensor..." << std::endl;
grabber = boost::shared_ptr<pcl::VLPGrabber>( new pcl::VLPGrabber( boost::asio::ip::address::from_string( ipaddress ), boost::lexical_cast<unsigned short>( port ) ) );
}
// Register Callback Function
boost::signals2::connection connection = grabber->registerCallback( function );
// Start Grabber
grabber->start();
while( !viewer->wasStopped() ){
// Update Viewer
viewer->spinOnce();
boost::mutex::scoped_try_lock lock( mutex );
if( lock.owns_lock() && cloud ){
// Update Point Cloud
handler->setInputCloud( cloud );
if( !viewer->updatePointCloud( cloud, *handler, "cloud" ) ){
viewer->addPointCloud( cloud, *handler, "cloud" );
}
}
}
// Stop Grabber
grabber->stop();
// Disconnect Callback Function
if( connection.connected() ){
connection.disconnect();
}
return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment