pointcloud_reader_node.cc
Go to the documentation of this file.
00001 #include <pcl/point_cloud.h>
00002 #include "pcl/point_types.h"
00003 #include "ros/ros.h"
00004 #include <tf/transform_broadcaster.h>
00005 #include "pcl/io/pcd_io.h"
00006 #include <pointcloud_utils.h>
00007 
00008 int
00009 main (int argc, char** argv)
00010 {
00011     if(argc<2)
00012     {
00013         ROS_INFO("wrong number of arguments!");
00014         return(-1);
00015     }
00016 
00017     sensor_msgs::PointCloud2 cloudMSG;
00018     pcl::PointCloud<pcl::PointXYZ> cloud;
00019 
00020     //load vrml file into cloud
00021     cloud = lslgeneric::readVRML<pcl::PointXYZ>(argv[1]);
00022     pcl::toROSMsg(cloud,cloudMSG);
00023     ROS_INFO ("Loaded %d data points from %s with the following fields: %s", (int)(cloudMSG.width * cloudMSG.height),
00024               argv[1], pcl::getFieldsList (cloudMSG).c_str ());
00025 
00026     //create a broadcaster node
00027     ros::init(argc, argv, "pointcloud_vrml");
00028     ros::NodeHandle n;
00029     ros::Publisher vrmlPublisher =
00030         n.advertise<sensor_msgs::PointCloud2>("points2_in", 10);
00031     ros::Rate loop_rate(0.1);
00032     int count = 0;
00033 
00034     static tf::TransformBroadcaster br;
00035     tf::Transform transform;
00036     transform.setOrigin(tf::Vector3(0, 0, 0.0));
00037     transform.setRotation(tf::Quaternion(0, 0, 0));
00038     cloudMSG.header.frame_id = "/pc_frame";
00039 
00040     while (ros::ok())
00041     {
00042         cloudMSG.header.stamp = ros::Time::now();
00043         vrmlPublisher.publish(cloudMSG);
00044         br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "/pc_frame"));
00045         ros::spinOnce();
00046         loop_rate.sleep();
00047         ++count;
00048     }
00049 
00050     return (0);
00051 }
00052 
00053 
00054 


pointcloud_vrml
Author(s): Dev
autogenerated on Mon Jan 6 2014 11:31:55