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 * $Id$ 00037 * 00038 */ 00039 00040 #ifndef PCL_IO_DINAST_GRABBER_ 00041 #define PCL_IO_DINAST_GRABBER_ 00042 00043 #include <pcl/point_types.h> 00044 #include <pcl/point_cloud.h> 00045 #include <pcl/io/grabber.h> 00046 #include <pcl/common/time.h> 00047 #include <pcl/console/print.h> 00048 #include <libusb-1.0/libusb.h> 00049 #include <boost/circular_buffer.hpp> 00050 00051 namespace pcl 00052 { 00053 /** \brief Grabber for DINAST devices (i.e., IPA-1002, IPA-1110, IPA-2001) 00054 * \author Marco A. Gutierrez <marcog@unex.es> 00055 * \ingroup io 00056 */ 00057 class PCL_EXPORTS DinastGrabber: public Grabber 00058 { 00059 // Define callback signature typedefs 00060 typedef void (sig_cb_dinast_point_cloud) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&); 00061 00062 public: 00063 /** \brief Constructor that sets up the grabber constants. 00064 * \param[in] device_position Number corresponding the device to grab 00065 */ 00066 DinastGrabber (const int device_position=1); 00067 00068 /** \brief Destructor. It never throws. */ 00069 virtual ~DinastGrabber () throw (); 00070 00071 /** \brief Check if the grabber is running 00072 * \return true if grabber is running / streaming. False otherwise. 00073 */ 00074 virtual bool 00075 isRunning () const; 00076 00077 /** \brief Returns the name of the concrete subclass, DinastGrabber. 00078 * \return DinastGrabber. 00079 */ 00080 virtual std::string 00081 getName () const 00082 { return (std::string ("DinastGrabber")); } 00083 00084 /** \brief Start the data acquisition process. 00085 */ 00086 virtual void 00087 start (); 00088 00089 /** \brief Stop the data acquisition process. 00090 */ 00091 virtual void 00092 stop (); 00093 00094 /** \brief Obtain the number of frames per second (FPS). */ 00095 virtual float 00096 getFramesPerSecond () const; 00097 00098 /** \brief Get the version number of the currently opened device 00099 */ 00100 std::string 00101 getDeviceVersion (); 00102 00103 protected: 00104 00105 /** \brief On initialization processing. */ 00106 void 00107 onInit (const int device_id); 00108 00109 /** \brief Setup a Dinast 3D camera device 00110 * \param[in] device_position Number corresponding the device to grab 00111 * \param[in] id_vendor The ID of the camera vendor (should be 0x18d1) 00112 * \param[in] id_product The ID of the product (should be 0x1402) 00113 */ 00114 void 00115 setupDevice (int device_position, 00116 const int id_vendor = 0x18d1, 00117 const int id_product = 0x1402); 00118 00119 /** \brief Send a RX data packet request 00120 * \param[in] req_code the request to send (the request field for the setup packet) 00121 * \param[in] length the length field for the setup packet. The data buffer should be at least this size. 00122 */ 00123 bool 00124 USBRxControlData (const unsigned char req_code, 00125 unsigned char *buffer, 00126 int length); 00127 00128 /** \brief Send a TX data packet request 00129 * \param[in] req_code the request to send (the request field for the setup packet) 00130 * \param[in] length the length field for the setup packet. The data buffer should be at least this size. 00131 */ 00132 bool 00133 USBTxControlData (const unsigned char req_code, 00134 unsigned char *buffer, 00135 int length); 00136 00137 /** \brief Check if we have a header in the global buffer, and return the position of the next valid image. 00138 * \note If the image in the buffer is partial, return -1, as we have to wait until we add more data to it. 00139 * \return the position of the next valid image (i.e., right after a valid header) or -1 in case the buffer 00140 * either doesn't have an image or has a partial image 00141 */ 00142 int 00143 checkHeader (); 00144 00145 /** \brief Read image data and leaves it on image_ 00146 */ 00147 void 00148 readImage (); 00149 00150 /** \brief Obtains XYZI Point Cloud from the image of the camera 00151 * \param[out] the point cloud from the image data 00152 */ 00153 pcl::PointCloud<pcl::PointXYZI>::Ptr 00154 getXYZIPointCloud (); 00155 00156 /** \brief The function in charge of getting the data from the camera 00157 */ 00158 void 00159 captureThreadFunction (); 00160 00161 /** \brief Width of image */ 00162 int image_width_; 00163 00164 /** \brief Height of image */ 00165 int image_height_; 00166 00167 /** \brief Total size of image */ 00168 int image_size_; 00169 00170 /** \brief Length of a sync packet */ 00171 int sync_packet_size_; 00172 00173 double dist_max_2d_; 00174 00175 /** \brief diagonal Field of View*/ 00176 double fov_; 00177 00178 /** \brief Size of pixel */ 00179 enum pixel_size { RAW8=1, RGB16=2, RGB24=3, RGB32=4 }; 00180 00181 /** \brief The libusb context*/ 00182 libusb_context *context_; 00183 00184 /** \brief the actual device_handle for the camera */ 00185 struct libusb_device_handle *device_handle_; 00186 00187 /** \brief Temporary USB read buffer, since we read two RGB16 images at a time size is the double of two images 00188 * plus a sync packet. 00189 */ 00190 unsigned char *raw_buffer_ ; 00191 00192 /** \brief Global circular buffer */ 00193 boost::circular_buffer<unsigned char> g_buffer_; 00194 00195 /** \brief Bulk endpoint address value */ 00196 unsigned char bulk_ep_; 00197 00198 /** \brief Device command values */ 00199 enum { CMD_READ_START=0xC7, CMD_READ_STOP=0xC8, CMD_GET_VERSION=0xDC, CMD_SEND_DATA=0xDE }; 00200 00201 unsigned char *image_; 00202 00203 /** \brief Since there is no header after the first image, we need to save the state */ 00204 bool second_image_; 00205 00206 bool running_; 00207 00208 boost::thread capture_thread_; 00209 00210 mutable boost::mutex capture_mutex_; 00211 boost::signals2::signal<sig_cb_dinast_point_cloud>* point_cloud_signal_; 00212 }; 00213 } //namespace pcl 00214 00215 #endif // PCL_IO_DINAST_GRABBER_