Point Cloud Library (PCL)  1.14.1-dev
dinast_grabber.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #pragma once
41 
42 #include <pcl/point_types.h>
43 #include <pcl/point_cloud.h>
44 #include <pcl/io/grabber.h>
45 #include <pcl/common/time.h>
46 #include <pcl/console/print.h>
47 #include <libusb-1.0/libusb.h>
48 #include <boost/circular_buffer.hpp>
49 
50 #include <mutex>
51 #include <thread>
52 
53 namespace pcl
54 {
55  /** \brief Grabber for DINAST devices (i.e., IPA-1002, IPA-1110, IPA-2001)
56  * \author Marco A. Gutierrez <marcog@unex.es>
57  * \ingroup io
58  */
60  {
61  // Define callback signature typedefs
62  using sig_cb_dinast_point_cloud = void (const pcl::PointCloud<pcl::PointXYZI>::ConstPtr &);
63 
64  public:
65  /** \brief Constructor that sets up the grabber constants.
66  * \param[in] device_position Number corresponding the device to grab
67  */
68  DinastGrabber (const int device_position=1);
69 
70  /** \brief Destructor. It never throws. */
71  ~DinastGrabber () noexcept override;
72 
73  /** \brief Check if the grabber is running
74  * \return true if grabber is running / streaming. False otherwise.
75  */
76  bool
77  isRunning () const override;
78 
79  /** \brief Returns the name of the concrete subclass, DinastGrabber.
80  * \return DinastGrabber.
81  */
82  std::string
83  getName () const override
84  { return {"DinastGrabber"}; }
85 
86  /** \brief Start the data acquisition process.
87  */
88  void
89  start () override;
90 
91  /** \brief Stop the data acquisition process.
92  */
93  void
94  stop () override;
95 
96  /** \brief Obtain the number of frames per second (FPS). */
97  float
98  getFramesPerSecond () const override;
99 
100  /** \brief Get the version number of the currently opened device
101  */
102  std::string
104 
105  protected:
106 
107  /** \brief On initialization processing. */
108  void
109  onInit (const int device_id);
110 
111  /** \brief Setup a Dinast 3D camera device
112  * \param[in] device_position Number corresponding the device to grab
113  * \param[in] id_vendor The ID of the camera vendor (should be 0x18d1)
114  * \param[in] id_product The ID of the product (should be 0x1402)
115  */
116  void
117  setupDevice (int device_position,
118  const int id_vendor = 0x18d1,
119  const int id_product = 0x1402);
120 
121  /** \brief Send a RX data packet request
122  * \param[in] req_code the request to send (the request field for the setup packet)
123  * \param buffer
124  * \param[in] length the length field for the setup packet. The data buffer should be at least this size.
125  */
126  bool
127  USBRxControlData (const unsigned char req_code,
128  unsigned char *buffer,
129  int length);
130 
131  /** \brief Send a TX data packet request
132  * \param[in] req_code the request to send (the request field for the setup packet)
133  * \param buffer
134  * \param[in] length the length field for the setup packet. The data buffer should be at least this size.
135  */
136  bool
137  USBTxControlData (const unsigned char req_code,
138  unsigned char *buffer,
139  int length);
140 
141  /** \brief Check if we have a header in the global buffer, and return the position of the next valid image.
142  * \note If the image in the buffer is partial, return -1, as we have to wait until we add more data to it.
143  * \return the position of the next valid image (i.e., right after a valid header) or -1 in case the buffer
144  * either doesn't have an image or has a partial image
145  */
146  int
148 
149  /** \brief Read image data and leaves it on image_
150  */
151  void
153 
154  /** \brief Obtains XYZI Point Cloud from the image of the camera
155  * \return the point cloud from the image data
156  */
159 
160  /** \brief The function in charge of getting the data from the camera
161  */
162  void
164 
165  /** \brief Width of image */
166  int image_width_{320};
167 
168  /** \brief Height of image */
169  int image_height_{240};
170 
171  /** \brief Total size of image */
172  int image_size_{image_width_ * image_height_};
173 
174  /** \brief Length of a sync packet */
175  int sync_packet_size_{512};
176 
177  double dist_max_2d_{1. / (image_width_ / 2.)};
178 
179  /** \brief diagonal Field of View*/
180  double fov_{64. * M_PI / 180.};
181 
182  /** \brief Size of pixel */
183  enum pixel_size { RAW8=1, RGB16=2, RGB24=3, RGB32=4 };
184 
185  /** \brief The libusb context*/
186  libusb_context *context_{nullptr};
187 
188  /** \brief the actual device_handle for the camera */
189  struct libusb_device_handle *device_handle_{nullptr};
190 
191  /** \brief Temporary USB read buffer, since we read two RGB16 images at a time size is the double of two images
192  * plus a sync packet.
193  */
194  unsigned char *raw_buffer_{nullptr} ;
195 
196  /** \brief Global circular buffer */
197  boost::circular_buffer<unsigned char> g_buffer_;
198 
199  /** \brief Bulk endpoint address value */
200  unsigned char bulk_ep_{std::numeric_limits<unsigned char>::max ()};
201 
202  /** \brief Device command values */
203  enum { CMD_READ_START=0xC7, CMD_READ_STOP=0xC8, CMD_GET_VERSION=0xDC, CMD_SEND_DATA=0xDE };
204 
205  unsigned char *image_{nullptr};
206 
207  /** \brief Since there is no header after the first image, we need to save the state */
208  bool second_image_{false};
209 
210  bool running_{false};
211 
212  std::thread capture_thread_;
213 
214  mutable std::mutex capture_mutex_;
215  boost::signals2::signal<sig_cb_dinast_point_cloud>* point_cloud_signal_;
216  };
217 } //namespace pcl
Grabber for DINAST devices (i.e., IPA-1002, IPA-1110, IPA-2001)
DinastGrabber(const int device_position=1)
Constructor that sets up the grabber constants.
float getFramesPerSecond() const override
Obtain the number of frames per second (FPS).
std::mutex capture_mutex_
void captureThreadFunction()
The function in charge of getting the data from the camera.
pcl::PointCloud< pcl::PointXYZI >::Ptr getXYZIPointCloud()
Obtains XYZI Point Cloud from the image of the camera.
bool USBRxControlData(const unsigned char req_code, unsigned char *buffer, int length)
Send a RX data packet request.
void stop() override
Stop the data acquisition process.
void start() override
Start the data acquisition process.
int checkHeader()
Check if we have a header in the global buffer, and return the position of the next valid image.
void readImage()
Read image data and leaves it on image_.
std::thread capture_thread_
pixel_size
Size of pixel.
void setupDevice(int device_position, const int id_vendor=0x18d1, const int id_product=0x1402)
Setup a Dinast 3D camera device.
std::string getDeviceVersion()
Get the version number of the currently opened device.
~DinastGrabber() noexcept override
Destructor.
bool USBTxControlData(const unsigned char req_code, unsigned char *buffer, int length)
Send a TX data packet request.
boost::circular_buffer< unsigned char > g_buffer_
Global circular buffer.
boost::signals2::signal< sig_cb_dinast_point_cloud > * point_cloud_signal_
void onInit(const int device_id)
On initialization processing.
Grabber interface for PCL 1.x device drivers.
Definition: grabber.h:60
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
Defines all the PCL implemented PointT point type structures.
Define methods for measuring time spent in code blocks.
#define PCL_EXPORTS
Definition: pcl_macros.h:323
#define M_PI
Definition: pcl_macros.h:201