$search
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id: test_passing.cpp 33304 2010-10-15 00:09:25Z rusu $ 00035 * 00036 */ 00037 00038 #include <boost/thread.hpp> 00039 #include <pluginlib/class_list_macros.h> 00040 #include <pcl/point_types.h> 00041 #include <sensor_msgs/PointCloud2.h> 00042 #include <pcl_ros/pcl_nodelet.h> 00043 00044 namespace pcl_ros 00045 { 00046 class TestTalker: public PCLNodelet 00047 { 00048 public: 00049 TestTalker () 00050 { 00051 // Generate the data 00052 pcl::PointCloud<pcl::PointXYZ> cloud; 00053 00054 cloud.width = 640; 00055 cloud.height = 480; 00056 cloud.points.resize (cloud.width * cloud.height); 00057 cloud.is_dense = true; 00058 00059 srand (time (NULL)); 00060 size_t nr_p = cloud.points.size (); 00061 // Randomly create a new point cloud 00062 for (size_t i = 0; i < nr_p; ++i) 00063 { 00064 cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0); 00065 cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0); 00066 cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0); 00067 } 00068 00069 // Convert from data type to blob 00070 pcl::toROSMsg (cloud, cloud_blob_); 00071 cloud_blob_ptr_ = boost::make_shared<sensor_msgs::PointCloud2> (cloud_blob_); 00072 } 00073 00074 protected: 00075 sensor_msgs::PointCloud2 cloud_blob_; 00076 sensor_msgs::PointCloud2::Ptr cloud_blob_ptr_; 00077 ros::Publisher pub_output_; 00078 00079 virtual void 00080 onInit () 00081 { 00082 ros::NodeHandle private_nh = this->getMTPrivateNodeHandle (); 00083 pub_output_ = private_nh.advertise<sensor_msgs::PointCloud2> ("output", 1); 00084 00085 NODELET_INFO ("[onInit] Data initialized. Starting to publish..."); 00086 ros::Duration d (2), d2 (0.001); 00087 d.sleep (); 00088 for (size_t i = 0; i < 1000; ++i) 00089 { 00090 NODELET_INFO ("[onInit] Publishing... %d", (int)i); 00091 pub_output_.publish (cloud_blob_ptr_); 00092 d2.sleep (); 00093 } 00094 } 00095 }; 00096 00097 class TestListener: public PCLNodelet 00098 { 00099 public: 00100 TestListener () {}; 00101 00102 protected: 00103 ros::Subscriber sub_input_; 00104 00105 virtual void 00106 onInit () 00107 { 00108 ros::NodeHandle private_nh = this->getMTPrivateNodeHandle (); 00109 sub_input_ = private_nh.subscribe ("input", 1, &TestListener::input_callback, this); 00110 NODELET_INFO ("[onInit] Waiting for data..."); 00111 } 00112 00113 void 00114 input_callback (const sensor_msgs::PointCloud2ConstPtr &cloud) 00115 { 00116 NODELET_INFO ("[input_callback] PointCloud with %d data points and frame %s on topic %s received.", cloud->width * cloud->height, cloud->header.frame_id.c_str (), this->getMTPrivateNodeHandle ().resolveName ("input").c_str ()); 00117 } 00118 }; 00119 00120 class TestPingPong: public PCLNodelet 00121 { 00122 public: 00123 TestPingPong () : msg_count_ (0), byte_count_ (0) 00124 { 00125 // Generate the data 00126 pcl::PointCloud<pcl::PointXYZ> cloud; 00127 00128 cloud.width = 640; 00129 cloud.height = 480; 00130 cloud.points.resize (cloud.width * cloud.height); 00131 cloud.is_dense = true; 00132 00133 srand (time (NULL)); 00134 size_t nr_p = cloud.points.size (); 00135 // Randomly create a new point cloud 00136 for (size_t i = 0; i < nr_p; ++i) 00137 { 00138 cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0); 00139 cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0); 00140 cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0); 00141 } 00142 00143 // Convert from data type to blob 00144 pcl::toROSMsg (cloud, cloud_blob_); 00145 cloud_blob_ptr_ = boost::make_shared<sensor_msgs::PointCloud2> (cloud_blob_); 00146 } 00147 00148 virtual ~TestPingPong () 00149 { 00150 t_end_ = ros::WallTime::now (); 00151 ROS_INFO ("Sent %d messages (%ld bytes) in %f seconds (%d msg/s).", msg_count_, byte_count_, (t_end_ - t_start_).toSec (), (int)(msg_count_ / (t_end_ - t_start_).toSec ())); 00152 } 00153 00154 protected: 00155 sensor_msgs::PointCloud2 cloud_blob_; 00156 sensor_msgs::PointCloud2::Ptr cloud_blob_ptr_; 00157 ros::Subscriber sub_input_; 00158 ros::Publisher pub_output_; 00159 int msg_count_, total_msgs_; 00160 long int byte_count_; 00161 ros::WallTime t_start_, t_end_; 00162 00163 virtual void 00164 onInit () 00165 { 00166 ros::NodeHandle private_nh = this->getMTPrivateNodeHandle (); 00167 00168 private_nh.getParam ("total_msgs", total_msgs_); 00169 00170 sub_input_ = private_nh.subscribe ("input", 1, &TestPingPong::input_callback, this); 00171 pub_output_ = private_nh.advertise<sensor_msgs::PointCloud2> ("output", 1); 00172 NODELET_INFO ("[onInit] %d threads should be available.", boost::thread::hardware_concurrency ()); 00173 ros::Duration (1).sleep (); 00174 pub_output_.publish (cloud_blob_ptr_); 00175 t_start_ = ros::WallTime::now (); 00176 } 00177 00178 void 00179 input_callback (const sensor_msgs::PointCloud2ConstPtr &cloud) 00180 { 00181 if (msg_count_ >= total_msgs_) 00182 ros::shutdown (); 00183 00184 pub_output_.publish (cloud); 00185 msg_count_++; 00186 byte_count_ += cloud->data.size (); 00187 } 00188 }; 00189 } 00190 00191 typedef pcl_ros::TestTalker TestTalker; 00192 typedef pcl_ros::TestListener TestListener; 00193 typedef pcl_ros::TestPingPong TestPingPong; 00194 PLUGINLIB_DECLARE_CLASS (pcl, TestTalker, TestTalker, nodelet::Nodelet); 00195 PLUGINLIB_DECLARE_CLASS (pcl, TestListener, TestListener, nodelet::Nodelet); 00196 PLUGINLIB_DECLARE_CLASS (pcl, TestPingPong, TestPingPong, nodelet::Nodelet); 00197