Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
53namespace 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 */
59 class PCL_EXPORTS DinastGrabber: public Grabber
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.
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.
pcl::PointCloud< pcl::PointXYZI >::Ptr getXYZIPointCloud()
Obtains XYZI Point Cloud from the image of the camera.
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
shared_ptr< const PointCloud< PointT > > ConstPtr
Defines all the PCL implemented PointT point type structures.
Define methods for measuring time spent in code blocks.
#define M_PI
Definition pcl_macros.h:203