Loading...
Searching...
No Matches
dinast_grabber.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012-, Open Perception, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#pragma once
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/grabber.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <libusb-1.0/libusb.h>
#include <boost/circular_buffer.hpp>
#include <mutex>
#include <thread>
namespace pcl
{
/** \brief Grabber for DINAST devices (i.e., IPA-1002, IPA-1110, IPA-2001)
* \author Marco A. Gutierrez <marcog@unex.es>
* \ingroup io
*/
class PCL_EXPORTS DinastGrabber: public Grabber
{
// Define callback signature typedefs
using sig_cb_dinast_point_cloud = void (const pcl::PointCloud<pcl::PointXYZI>::ConstPtr &);
public:
/** \brief Constructor that sets up the grabber constants.
* \param[in] device_position Number corresponding the device to grab
*/
DinastGrabber (const int device_position=1);
/** \brief Destructor. It never throws. */
~DinastGrabber () noexcept override;
/** \brief Check if the grabber is running
* \return true if grabber is running / streaming. False otherwise.
*/
bool
isRunning () const override;
/** \brief Returns the name of the concrete subclass, DinastGrabber.
* \return DinastGrabber.
*/
std::string
getName () const override
{ return {"DinastGrabber"}; }
/** \brief Start the data acquisition process.
*/
void
start () override;
/** \brief Stop the data acquisition process.
*/
void
stop () override;
/** \brief Obtain the number of frames per second (FPS). */
float
getFramesPerSecond () const override;
/** \brief Get the version number of the currently opened device
*/
std::string
getDeviceVersion ();
protected:
/** \brief On initialization processing. */
void
onInit (const int device_id);
/** \brief Setup a Dinast 3D camera device
* \param[in] device_position Number corresponding the device to grab
* \param[in] id_vendor The ID of the camera vendor (should be 0x18d1)
* \param[in] id_product The ID of the product (should be 0x1402)
*/
void
setupDevice (int device_position,
const int id_vendor = 0x18d1,
const int id_product = 0x1402);
/** \brief Send a RX data packet request
* \param[in] req_code the request to send (the request field for the setup packet)
* \param buffer
* \param[in] length the length field for the setup packet. The data buffer should be at least this size.
*/
bool
USBRxControlData (const unsigned char req_code,
unsigned char *buffer,
int length);
/** \brief Send a TX data packet request
* \param[in] req_code the request to send (the request field for the setup packet)
* \param buffer
* \param[in] length the length field for the setup packet. The data buffer should be at least this size.
*/
bool
USBTxControlData (const unsigned char req_code,
unsigned char *buffer,
int length);
/** \brief Check if we have a header in the global buffer, and return the position of the next valid image.
* \note If the image in the buffer is partial, return -1, as we have to wait until we add more data to it.
* \return the position of the next valid image (i.e., right after a valid header) or -1 in case the buffer
* either doesn't have an image or has a partial image
*/
int
checkHeader ();
/** \brief Read image data and leaves it on image_
*/
void
readImage ();
/** \brief Obtains XYZI Point Cloud from the image of the camera
* \return the point cloud from the image data
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr
getXYZIPointCloud ();
/** \brief The function in charge of getting the data from the camera
*/
void
captureThreadFunction ();
/** \brief Width of image */
int image_width_{320};
/** \brief Height of image */
int image_height_{240};
/** \brief Total size of image */
int image_size_{image_width_ * image_height_};
/** \brief Length of a sync packet */
int sync_packet_size_{512};
double dist_max_2d_{1. / (image_width_ / 2.)};
/** \brief diagonal Field of View*/
double fov_{64. * M_PI / 180.};
/** \brief Size of pixel */
enum pixel_size { RAW8=1, RGB16=2, RGB24=3, RGB32=4 };
/** \brief The libusb context*/
libusb_context *context_{nullptr};
/** \brief the actual device_handle for the camera */
struct libusb_device_handle *device_handle_{nullptr};
/** \brief Temporary USB read buffer, since we read two RGB16 images at a time size is the double of two images
* plus a sync packet.
*/
unsigned char *raw_buffer_{nullptr} ;
/** \brief Global circular buffer */
boost::circular_buffer<unsigned char> g_buffer_;
/** \brief Bulk endpoint address value */
unsigned char bulk_ep_{std::numeric_limits<unsigned char>::max ()};
/** \brief Device command values */
enum { CMD_READ_START=0xC7, CMD_READ_STOP=0xC8, CMD_GET_VERSION=0xDC, CMD_SEND_DATA=0xDE };
unsigned char *image_{nullptr};
/** \brief Since there is no header after the first image, we need to save the state */
bool second_image_{false};
bool running_{false};
std::thread capture_thread_;
mutable std::mutex capture_mutex_;
boost::signals2::signal<sig_cb_dinast_point_cloud>* point_cloud_signal_;
};
} //namespace pcl
pcl::DinastGrabber::DinastGrabber
DinastGrabber(const int device_position=1)
Constructor that sets up the grabber constants.
pcl::DinastGrabber::getFramesPerSecond
float getFramesPerSecond() const override
Obtain the number of frames per second (FPS).
pcl::DinastGrabber::fov_
double fov_
diagonal Field of View
Definition dinast_grabber.h:180
pcl::DinastGrabber::capture_mutex_
std::mutex capture_mutex_
Definition dinast_grabber.h:214
pcl::DinastGrabber::image_height_
int image_height_
Height of image.
Definition dinast_grabber.h:169
pcl::DinastGrabber::captureThreadFunction
void captureThreadFunction()
The function in charge of getting the data from the camera.
pcl::DinastGrabber::dist_max_2d_
double dist_max_2d_
Definition dinast_grabber.h:177
pcl::DinastGrabber::raw_buffer_
unsigned char * raw_buffer_
Temporary USB read buffer, since we read two RGB16 images at a time size is the double of two images ...
Definition dinast_grabber.h:194
pcl::DinastGrabber::USBRxControlData
bool USBRxControlData(const unsigned char req_code, unsigned char *buffer, int length)
Send a RX data packet request.
pcl::DinastGrabber::stop
void stop() override
Stop the data acquisition process.
pcl::DinastGrabber::getXYZIPointCloud
pcl::PointCloud< pcl::PointXYZI >::Ptr getXYZIPointCloud()
Obtains XYZI Point Cloud from the image of the camera.
pcl::DinastGrabber::start
void start() override
Start the data acquisition process.
pcl::DinastGrabber::checkHeader
int checkHeader()
Check if we have a header in the global buffer, and return the position of the next valid image.
pcl::DinastGrabber::running_
bool running_
Definition dinast_grabber.h:210
pcl::DinastGrabber::CMD_SEND_DATA
@ CMD_SEND_DATA
Definition dinast_grabber.h:203
pcl::DinastGrabber::CMD_GET_VERSION
@ CMD_GET_VERSION
Definition dinast_grabber.h:203
pcl::DinastGrabber::CMD_READ_START
@ CMD_READ_START
Definition dinast_grabber.h:203
pcl::DinastGrabber::CMD_READ_STOP
@ CMD_READ_STOP
Definition dinast_grabber.h:203
pcl::DinastGrabber::image_size_
int image_size_
Total size of image.
Definition dinast_grabber.h:172
pcl::DinastGrabber::device_handle_
struct libusb_device_handle * device_handle_
the actual device_handle for the camera
Definition dinast_grabber.h:189
pcl::DinastGrabber::context_
libusb_context * context_
The libusb context.
Definition dinast_grabber.h:186
pcl::DinastGrabber::readImage
void readImage()
Read image data and leaves it on image_.
pcl::DinastGrabber::capture_thread_
std::thread capture_thread_
Definition dinast_grabber.h:212
pcl::DinastGrabber::getName
std::string getName() const override
Returns the name of the concrete subclass, DinastGrabber.
Definition dinast_grabber.h:83
pcl::DinastGrabber::image_width_
int image_width_
Width of image.
Definition dinast_grabber.h:166
pcl::DinastGrabber::bulk_ep_
unsigned char bulk_ep_
Bulk endpoint address value.
Definition dinast_grabber.h:200
pcl::DinastGrabber::pixel_size
pixel_size
Size of pixel.
Definition dinast_grabber.h:183
pcl::DinastGrabber::RAW8
@ RAW8
Definition dinast_grabber.h:183
pcl::DinastGrabber::RGB32
@ RGB32
Definition dinast_grabber.h:183
pcl::DinastGrabber::RGB24
@ RGB24
Definition dinast_grabber.h:183
pcl::DinastGrabber::RGB16
@ RGB16
Definition dinast_grabber.h:183
pcl::DinastGrabber::setupDevice
void setupDevice(int device_position, const int id_vendor=0x18d1, const int id_product=0x1402)
Setup a Dinast 3D camera device.
pcl::DinastGrabber::image_
unsigned char * image_
Definition dinast_grabber.h:205
pcl::DinastGrabber::getDeviceVersion
std::string getDeviceVersion()
Get the version number of the currently opened device.
pcl::DinastGrabber::second_image_
bool second_image_
Since there is no header after the first image, we need to save the state.
Definition dinast_grabber.h:208
pcl::DinastGrabber::sync_packet_size_
int sync_packet_size_
Length of a sync packet.
Definition dinast_grabber.h:175
pcl::DinastGrabber::~DinastGrabber
~DinastGrabber() noexcept override
Destructor.
pcl::DinastGrabber::USBTxControlData
bool USBTxControlData(const unsigned char req_code, unsigned char *buffer, int length)
Send a TX data packet request.
pcl::DinastGrabber::g_buffer_
boost::circular_buffer< unsigned char > g_buffer_
Global circular buffer.
Definition dinast_grabber.h:197
pcl::DinastGrabber::point_cloud_signal_
boost::signals2::signal< sig_cb_dinast_point_cloud > * point_cloud_signal_
Definition dinast_grabber.h:215
pcl::DinastGrabber::onInit
void onInit(const int device_id)
On initialization processing.
pcl::DinastGrabber::isRunning
bool isRunning() const override
Check if the grabber is running.
pcl::Grabber::Grabber
Grabber()=default
Default ctor.
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition point_cloud.h:414
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:415
point_types.h
Defines all the PCL implemented PointT point type structures.
time.h
Define methods for measuring time spent in code blocks.
pcl
Definition convolution.h:46
M_PI
#define M_PI
Definition pcl_macros.h:203