$search
00001 #include <ros/ros.h> 00002 #include <ros/console.h> 00003 00004 #include <sensor_msgs/PointCloud.h> 00005 #include <message_transport/message_transport.h> 00006 #include "numpoints.h" 00007 00008 class PointCloudPublisher { 00009 protected: 00010 00011 ros::NodeHandle n_; 00012 message_transport::MessageTransport<sensor_msgs::PointCloud> it_; 00013 message_transport::Publisher pcmsg_pub_; 00014 sensor_msgs::PointCloud pointcloud; 00015 00016 00017 public: 00018 00019 PointCloudPublisher(ros::NodeHandle &n) : n_(n), 00020 it_(n_,"pointcloud_transport","sensor_msgs::PointCloud") { 00021 pcmsg_pub_ = it_.advertise("pc_source",1); 00022 00023 } 00024 00025 ~PointCloudPublisher() 00026 { 00027 } 00028 00029 int mainloop() 00030 { 00031 00032 ROS_INFO("Entering main loop"); 00033 ros::Rate loop_rate(3); 00034 while (ros::ok()) 00035 { 00036 unsigned int i,num; 00037 num = numpoints + (rand() % 1000); 00038 pointcloud.header.stamp = ros::Time::now(); 00039 ROS_INFO("Publishing %d points",num); 00040 pointcloud.points.resize(num); 00041 pointcloud.channels.resize(1); 00042 pointcloud.channels[0].name = "intensity"; 00043 pointcloud.channels[0].values.resize(num); 00044 for (i=0;i<num;i++) { 00045 pointcloud.points[i].x = i; 00046 pointcloud.points[i].y = i/10; 00047 pointcloud.points[i].z = i/100; 00048 pointcloud.channels[0].values[i] = i; 00049 } 00050 pcmsg_pub_.publish(pointcloud); 00051 ROS_DEBUG("Published pointcloud at %f",pointcloud.header.stamp.toSec()); 00052 ros::spinOnce(); 00053 loop_rate.sleep(); 00054 } 00055 00056 return 0; 00057 } 00058 00059 }; 00060 00061 void setDebugLevel(const std::string & logname) { 00062 log4cxx::LoggerPtr logger = log4cxx::Logger::getLogger(logname); 00063 logger->setLevel(log4cxx::Level::getDebug()); 00064 ros::console::notifyLoggerLevelsChanged(); 00065 } 00066 00067 int main(int argc, char** argv) 00068 { 00069 ros::init(argc, argv, "test_publisher"); 00070 ros::NodeHandle n; 00071 PointCloudPublisher ic(n); 00072 // setDebugLevel("ros.pointcloud_transport"); 00073 // setDebugLevel("ros.sharedmem_transport"); 00074 ic.mainloop(); 00075 return 0; 00076 } 00077