pointcloud_publisher.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <sensor_msgs/PointCloud.h>
00003 #include <pcl/ros/conversions.h>
00004 #include "pcl/io/pcd_io.h"
00005 #include "pcl/point_types.h"
00006 #include <sensor_msgs/point_cloud_conversion.h>
00007 
00008 int main(int argc, char** argv)
00009 {
00010   if (argc < 2)
00011   {
00012     ROS_INFO("Usage <%s> <sleep rate[s]>", argv[0]);
00013     exit(0);
00014   }
00015   ros::init(argc, argv, "pointcloud_publisher");
00016   ros::NodeHandle n;
00017   pcl::PointCloud<pcl::PointXYZ> cloud;
00018 
00019   // Fill in the cloud data
00020   cloud.width    = 300;
00021   cloud.height   = 1;
00022   cloud.is_dense = false;
00023   cloud.points.resize (cloud.width * cloud.height);
00024 
00025   for (size_t i = 0; i < cloud.points.size (); ++i)
00026   {
00027     cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
00028     cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
00029     cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
00030   }
00031 
00032   sensor_msgs::PointCloud2 mycloud2;
00033   //sensor_msgs::PointCloud mycloud1;
00034   pcl::toROSMsg(cloud, mycloud2);
00035   //sensor_msgs::convertPointCloud2ToPointCloud(mycloud2, mycloud1);
00036 
00037   ros::Publisher pointcloud_pub = n.advertise<sensor_msgs::PointCloud2>("point_cloud", 1);
00038   ros::Rate loop_rate(atof(argv[1]));
00039   while (ros::ok())
00040   {
00041     mycloud2.header.frame_id = "/map";
00042     mycloud2.header.stamp = ros::Time::now();
00043     pointcloud_pub.publish(mycloud2);
00044     ROS_INFO("PointCloud published");
00045     ros::spinOnce();
00046     loop_rate.sleep();
00047   }
00048 }
00049 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_to_octree
Author(s): Hozefa Indorewala, Dejan Pangercic
autogenerated on Sun Oct 6 2013 12:03:28