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