Point Cloud Library (PCL)  1.11.0
robot_eye_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  */
37 
38 #pragma once
39 
40 #include "pcl/pcl_config.h"
41 
42 #include <pcl/io/grabber.h>
43 #include <pcl/io/impl/synchronized_queue.hpp>
44 #include <pcl/point_types.h>
45 #include <pcl/point_cloud.h>
46 #include <pcl/memory.h>
47 #include <boost/asio.hpp>
48 
49 #include <memory>
50 #include <thread>
51 
52 namespace pcl
53 {
54 
55  /** \brief Grabber for the Ocular Robotics RobotEye sensor.
56  * \ingroup io
57  */
59  {
60  public:
61 
62  /** \brief Signal used for the point cloud callback.
63  * This signal is sent when the accumulated number of points reaches
64  * the limit specified by setSignalPointCloudSize().
65  */
67 
68  /** \brief RobotEyeGrabber default constructor. */
70 
71  /** \brief RobotEyeGrabber constructor taking a specified IP address and data port. */
72  RobotEyeGrabber (const boost::asio::ip::address& ipAddress, unsigned short port=443);
73 
74  /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
75  ~RobotEyeGrabber () noexcept;
76 
77  /** \brief Starts the RobotEye grabber.
78  * The grabber runs on a separate thread, this call will return without blocking. */
79  void start () override;
80 
81  /** \brief Stops the RobotEye grabber. */
82  void stop () override;
83 
84  /** \brief Obtains the name of this I/O Grabber
85  * \return The name of the grabber
86  */
87  std::string getName () const override;
88 
89  /** \brief Check if the grabber is still running.
90  * \return TRUE if the grabber is running, FALSE otherwise
91  */
92  bool isRunning () const override;
93 
94  /** \brief Returns the number of frames per second.
95  */
96  float getFramesPerSecond () const override;
97 
98  /** \brief Set/get ip address of the sensor that sends the data.
99  * The default is address_v4::any ().
100  */
101  void setSensorAddress (const boost::asio::ip::address& ipAddress);
102  const boost::asio::ip::address& getSensorAddress () const;
103 
104  /** \brief Set/get the port number which receives data from the sensor.
105  * The default is 443.
106  */
107  void setDataPort (unsigned short port);
108  unsigned short getDataPort () const;
109 
110  /** \brief Set/get the number of points to accumulate before the grabber
111  * callback is signaled. The default is 1000.
112  */
113  void setSignalPointCloudSize (std::size_t numerOfPoints);
114  std::size_t getSignalPointCloudSize () const;
115 
116  /** \brief Returns the point cloud with point accumulated by the grabber.
117  * It is not safe to access this point cloud except if the grabber is
118  * stopped or during the grabber callback.
119  */
120  pcl::PointCloud<pcl::PointXYZI>::Ptr getPointCloud() const;
121 
122  private:
123 
124  bool terminate_thread_;
125  std::size_t signal_point_cloud_size_;
126  unsigned short data_port_;
127  enum { MAX_LENGTH = 65535 };
128  unsigned char receive_buffer_[MAX_LENGTH];
129  unsigned int data_size_;
130 
131  boost::asio::ip::address sensor_address_;
132  boost::asio::ip::udp::endpoint sender_endpoint_;
133  boost::asio::io_service io_service_;
134  std::shared_ptr<boost::asio::ip::udp::socket> socket_;
135  std::shared_ptr<std::thread> socket_thread_;
136  std::shared_ptr<std::thread> consumer_thread_;
137 
139  pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_xyzi_;
140  boost::signals2::signal<sig_cb_robot_eye_point_cloud_xyzi>* point_cloud_signal_;
141 
142  void consumerThreadLoop ();
143  void socketThreadLoop ();
144  void asyncSocketReceive ();
145  void resetPointCloud ();
146  void socketCallback (const boost::system::error_code& error, std::size_t number_of_bytes);
147  void convertPacketData (unsigned char *data_packet, std::size_t length);
148  void computeXYZI (pcl::PointXYZI& point_XYZI, unsigned char* point_data);
149  void computeTimestamp (std::uint32_t& timestamp, unsigned char* point_data);
150  };
151 }
Grabber interface for PCL 1.x device drivers.
Definition: grabber.h:60
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:180
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
Grabber for the Ocular Robotics RobotEye sensor.
RobotEyeGrabber(const boost::asio::ip::address &ipAddress, unsigned short port=443)
RobotEyeGrabber constructor taking a specified IP address and data port.
~RobotEyeGrabber() noexcept
virtual Destructor inherited from the Grabber interface.
void(const pcl::PointCloud< pcl::PointXYZI >::ConstPtr &) sig_cb_robot_eye_point_cloud_xyzi
Signal used for the point cloud callback.
RobotEyeGrabber()
RobotEyeGrabber default constructor.
Defines all the PCL implemented PointT point type structures.
Defines functions, macros and traits for allocating and using memory.
std::uint32_t uint32_t
Definition: types.h:58
#define PCL_EXPORTS
Definition: pcl_macros.h:331