openni_mobile_server.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, 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  */
00035 
00036 
00037 #include <pcl/point_cloud.h>
00038 #include <pcl/point_types.h>
00039 #include <pcl/io/openni_grabber.h>
00040 #include <pcl/visualization/cloud_viewer.h>
00041 #include <pcl/io/openni_camera/openni_driver.h>
00042 #include <pcl/filters/voxel_grid.h>
00043 #include <pcl/console/parse.h>
00044 #include <pcl/common/time.h>
00045 
00046 #include <boost/asio.hpp>
00047 #include <boost/thread/thread.hpp>
00048 
00049 using boost::asio::ip::tcp;
00050 
00051 
00052 struct PointCloudBuffers
00053 {
00054   typedef boost::shared_ptr<PointCloudBuffers> Ptr;
00055   std::vector<short> points;
00056   std::vector<unsigned char> rgb;
00057 };
00058 
00059 void
00060 CopyPointCloudToBuffers (pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud, PointCloudBuffers& cloud_buffers)
00061 {
00062   const size_t nr_points = cloud->points.size ();
00063 
00064   cloud_buffers.points.resize (nr_points*3);
00065   cloud_buffers.rgb.resize (nr_points*3);
00066 
00067   const pcl::PointXYZ  bounds_min (-0.9f, -0.8f, 1.0f);
00068   const pcl::PointXYZ  bounds_max (0.9f, 3.0f, 3.3f);
00069 
00070   size_t j = 0;
00071   for (size_t i = 0; i < nr_points; ++i)
00072   {
00073 
00074     const pcl::PointXYZRGBA& point = cloud->points[i];
00075 
00076     if (!pcl_isfinite (point.x) || 
00077         !pcl_isfinite (point.y) || 
00078         !pcl_isfinite (point.z))
00079       continue;
00080 
00081     if (point.x < bounds_min.x ||
00082         point.y < bounds_min.y ||
00083         point.z < bounds_min.z ||
00084         point.x > bounds_max.x ||
00085         point.y > bounds_max.y ||
00086         point.z > bounds_max.z)
00087       continue;
00088 
00089     const int conversion_factor = 500;
00090 
00091     cloud_buffers.points[j*3 + 0] = static_cast<short> (point.x * conversion_factor);
00092     cloud_buffers.points[j*3 + 1] = static_cast<short> (point.y * conversion_factor);
00093     cloud_buffers.points[j*3 + 2] = static_cast<short> (point.z * conversion_factor);
00094 
00095     cloud_buffers.rgb[j*3 + 0] = point.r;
00096     cloud_buffers.rgb[j*3 + 1] = point.g;
00097     cloud_buffers.rgb[j*3 + 2] = point.b;
00098 
00099     j++;
00100   }
00101 
00102   cloud_buffers.points.resize (j * 3);
00103   cloud_buffers.rgb.resize (j * 3);
00104 }
00105 
00106 
00107 template <typename PointType>
00108 class PCLMobileServer
00109 {
00110   public:
00111 
00112     typedef pcl::PointCloud<PointType> Cloud;
00113     typedef typename Cloud::Ptr CloudPtr;
00114     typedef typename Cloud::ConstPtr CloudConstPtr;
00115 
00116     PCLMobileServer (const std::string& device_id = "", int port = 11111,
00117                      float leaf_size_x = 0.01, float leaf_size_y = 0.01, float leaf_size_z = 0.01)
00118     : port_ (port),
00119       device_id_ (device_id),
00120       viewer_ ("PCL OpenNI Mobile Server")
00121     {
00122       voxel_grid_filter_.setLeafSize (leaf_size_x, leaf_size_y, leaf_size_z);
00123     }
00124     
00125     void
00126     handleIncomingCloud (const CloudConstPtr& new_cloud)
00127     {
00128       CloudPtr temp_cloud (new Cloud);
00129       voxel_grid_filter_.setInputCloud (new_cloud);
00130       voxel_grid_filter_.filter (*temp_cloud);
00131 
00132       PointCloudBuffers::Ptr new_buffers = PointCloudBuffers::Ptr (new PointCloudBuffers);
00133       CopyPointCloudToBuffers (temp_cloud, *new_buffers);
00134 
00135       boost::mutex::scoped_lock lock (mutex_);
00136       filtered_cloud_ = temp_cloud;
00137       buffers_ = new_buffers;
00138     }
00139 
00140     PointCloudBuffers::Ptr
00141     getLatestBuffers ()
00142     {
00143       boost::mutex::scoped_lock lock (mutex_);
00144       return (buffers_);
00145     }
00146 
00147     CloudPtr
00148     getLatestPointCloud ()
00149     {
00150       boost::mutex::scoped_lock lock (mutex_);
00151       return (filtered_cloud_);
00152     }
00153 
00154     void
00155     run ()
00156     {
00157       pcl::OpenNIGrabber grabber (device_id_);
00158       boost::function<void (const CloudConstPtr&)> handler_function = boost::bind (&PCLMobileServer::handleIncomingCloud, this, _1);
00159       grabber.registerCallback (handler_function);
00160       grabber.start ();
00161 
00162       // wait for first cloud
00163       while (!getLatestPointCloud ())
00164         boost::this_thread::sleep (boost::posix_time::milliseconds (10));
00165 
00166       viewer_.showCloud (getLatestPointCloud ());
00167 
00168 
00169       boost::asio::io_service io_service;
00170       tcp::endpoint endpoint (tcp::v4 (), static_cast<unsigned short> (port_));
00171       tcp::acceptor acceptor (io_service, endpoint);
00172       tcp::socket socket (io_service);
00173 
00174       std::cout << "Listening on port " << port_ << "..." << std::endl;
00175       acceptor.accept (socket);
00176 
00177       std::cout << "Client connected." << std::endl;
00178 
00179       double start_time = pcl::getTime ();
00180       int counter = 0;
00181 
00182       while (!viewer_.wasStopped ())
00183       {
00184 
00185         // wait for client
00186         unsigned int nr_points = 0;
00187         boost::asio::read (socket, boost::asio::buffer (&nr_points, sizeof (nr_points)));
00188 
00189         PointCloudBuffers::Ptr buffers_to_send = getLatestBuffers ();
00190 
00191         nr_points = static_cast<unsigned int> (buffers_to_send->points.size()/3);
00192         boost::asio::write (socket, boost::asio::buffer (&nr_points, sizeof (nr_points)));
00193 
00194         if (nr_points)
00195         {
00196           boost::asio::write (socket, boost::asio::buffer (&buffers_to_send->points.front(), nr_points * 3 * sizeof (short)));
00197           boost::asio::write (socket, boost::asio::buffer (&buffers_to_send->rgb.front(), nr_points * 3 * sizeof (unsigned char)));
00198         }
00199 
00200         counter++;
00201 
00202         double new_time = pcl::getTime ();
00203         double elapsed_time = new_time - start_time;
00204         if (elapsed_time > 1.0)
00205         {
00206           double frames_per_second = counter / elapsed_time;
00207           start_time = new_time;
00208           counter = 0;
00209           std::cout << "fps: " << frames_per_second << std::endl;
00210         }
00211 
00212         viewer_.showCloud (getLatestPointCloud ());
00213       }
00214 
00215       grabber.stop ();
00216     }
00217 
00218     int port_;
00219     std::string device_id_;
00220     boost::mutex mutex_;
00221 
00222     pcl::VoxelGrid<PointType> voxel_grid_filter_;
00223     pcl::visualization::CloudViewer viewer_;
00224 
00225     CloudPtr filtered_cloud_;
00226     PointCloudBuffers::Ptr buffers_;
00227 };
00228 
00229 void
00230 usage (char ** argv)
00231 {
00232   std::cout << "usage: " << argv[0] << " <options>\n"
00233             << "where options are:\n"
00234             << "  -port p :: set the server port (default: 11111)\n"
00235             << "  -leaf x, y, z  :: set the voxel grid leaf size (default: 0.01)\n";
00236 }
00237 
00238 
00239 int 
00240 main (int argc, char ** argv)
00241 {
00242   if (pcl::console::find_argument (argc, argv, "-h") != -1)
00243   {
00244     usage (argv);
00245     return (0);
00246   }
00247 
00248   int port = 11111;
00249   float leaf_x = 0.01f, leaf_y = 0.01f, leaf_z = 0.01f;
00250   std::string device_id = "";
00251 
00252   pcl::console::parse_argument (argc, argv, "-port", port);
00253   pcl::console::parse_3x_arguments (argc, argv, "-leaf", leaf_x, leaf_y, leaf_z, false);
00254 
00255   pcl::OpenNIGrabber grabber ("");
00256   if (!grabber.providesCallback<pcl::OpenNIGrabber::sig_cb_openni_point_cloud_rgba> ())
00257   {
00258     std::cout << "OpenNI grabber does not provide the rgba cloud format." << std::endl;
00259     return (1);
00260   }
00261 
00262   PCL_INFO ("Using %f, %f, %f as a leaf size for VoxelGrid.\n", leaf_x, leaf_y, leaf_z);
00263 
00264   PCLMobileServer<pcl::PointXYZRGBA> server (device_id, port, leaf_x, leaf_y, leaf_z);
00265   server.run ();
00266 
00267   return (0);
00268 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:26:31