Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012-, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 */ 00037 00038 #include "pcl/pcl_config.h" 00039 00040 #ifndef PCL_IO_ROBOT_EYE_GRABBER_H_ 00041 #define PCL_IO_ROBOT_EYE_GRABBER_H_ 00042 00043 #include <pcl/io/grabber.h> 00044 #include <pcl/io/impl/synchronized_queue.hpp> 00045 #include <pcl/point_types.h> 00046 #include <pcl/point_cloud.h> 00047 #include <boost/asio.hpp> 00048 #include <boost/thread/thread.hpp> 00049 00050 namespace pcl 00051 { 00052 00053 /** \brief Grabber for the Ocular Robotics RobotEye sensor. 00054 * \ingroup io 00055 */ 00056 class PCL_EXPORTS RobotEyeGrabber : public Grabber 00057 { 00058 public: 00059 00060 /** \brief Signal used for the point cloud callback. 00061 * This signal is sent when the accumulated number of points reaches 00062 * the limit specified by setSignalPointCloudSize(). 00063 */ 00064 typedef void (sig_cb_robot_eye_point_cloud_xyzi) ( 00065 const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&); 00066 00067 /** \brief RobotEyeGrabber default constructor. */ 00068 RobotEyeGrabber (); 00069 00070 /** \brief RobotEyeGrabber constructor taking a specified IP address and data port. */ 00071 RobotEyeGrabber (const boost::asio::ip::address& ipAddress, unsigned short port=443); 00072 00073 /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ 00074 virtual ~RobotEyeGrabber () throw (); 00075 00076 /** \brief Starts the RobotEye grabber. 00077 * The grabber runs on a separate thread, this call will return without blocking. */ 00078 virtual void start (); 00079 00080 /** \brief Stops the RobotEye grabber. */ 00081 virtual void stop (); 00082 00083 /** \brief Obtains the name of this I/O Grabber 00084 * \return The name of the grabber 00085 */ 00086 virtual std::string getName () const; 00087 00088 /** \brief Check if the grabber is still running. 00089 * \return TRUE if the grabber is running, FALSE otherwise 00090 */ 00091 virtual bool isRunning () const; 00092 00093 /** \brief Returns the number of frames per second. 00094 */ 00095 virtual float getFramesPerSecond () const; 00096 00097 /** \brief Set/get ip address of the sensor that sends the data. 00098 * The default is address_v4::any (). 00099 */ 00100 void setSensorAddress (const boost::asio::ip::address& ipAddress); 00101 const boost::asio::ip::address& getSensorAddress () const; 00102 00103 /** \brief Set/get the port number which receives data from the sensor. 00104 * The default is 443. 00105 */ 00106 void setDataPort (unsigned short port); 00107 unsigned short getDataPort () const; 00108 00109 /** \brief Set/get the number of points to accumulate before the grabber 00110 * callback is signaled. The default is 1000. 00111 */ 00112 void setSignalPointCloudSize (std::size_t numerOfPoints); 00113 std::size_t getSignalPointCloudSize () const; 00114 00115 /** \brief Returns the point cloud with point accumulated by the grabber. 00116 * It is not safe to access this point cloud except if the grabber is 00117 * stopped or during the grabber callback. 00118 */ 00119 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > getPointCloud() const; 00120 00121 private: 00122 00123 bool terminate_thread_; 00124 size_t signal_point_cloud_size_; 00125 unsigned short data_port_; 00126 unsigned char receive_buffer_[500]; 00127 00128 boost::asio::ip::address sensor_address_; 00129 boost::asio::ip::udp::endpoint sender_endpoint_; 00130 boost::asio::io_service io_service_; 00131 boost::shared_ptr<boost::asio::ip::udp::socket> socket_; 00132 boost::shared_ptr<boost::thread> socket_thread_; 00133 boost::shared_ptr<boost::thread> consumer_thread_; 00134 00135 pcl::SynchronizedQueue<boost::shared_array<unsigned char> > packet_queue_; 00136 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > point_cloud_xyzi_; 00137 boost::signals2::signal<sig_cb_robot_eye_point_cloud_xyzi>* point_cloud_signal_; 00138 00139 void consumerThreadLoop (); 00140 void socketThreadLoop (); 00141 void asyncSocketReceive (); 00142 void resetPointCloud (); 00143 void socketCallback (const boost::system::error_code& error, std::size_t numberOfBytes); 00144 void convertPacketData (unsigned char *dataPacket, size_t length); 00145 void computeXYZI (pcl::PointXYZI& pointXYZI, unsigned char* pointData); 00146 }; 00147 } 00148 00149 #endif /* PCL_IO_ROBOT_EYE_GRABBER_H_ */