Point Cloud Library (PCL)  1.14.0-dev
tim_grabber.h
1 /*
2  * SPDX-License-Identifier: BSD-3-Clause
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2020-, Open Perception
6  * Copyright (c) 2020, ysuzuki19
7  *
8  * All rights reserved
9  */
10 
11 #pragma once
12 
13 #include <pcl/pcl_exports.h>
14 #include <pcl/console/print.h>
15 #include <pcl/common/time.h>
16 #include <pcl/io/grabber.h>
17 #include <pcl/point_types.h>
18 #include <pcl/point_cloud.h>
19 #include <boost/asio.hpp>
20 #include <mutex>
21 #include <vector>
22 #include <string>
23 #include <thread>
24 
25 namespace pcl
26 {
27 
28 //// note: Protocol named CoLaA (used by SICK) has some information.
29 //// In this Grabber, only the amount_of_data is used, so other information is truncated.
30 //// Details of the protocol can be found at the following URL.
31 
32 //// pp.87~89 (table)
33 
34 //// https://cdn.sickcn.com/media/docs/7/27/927/technical_information_telegram_listing_ranging_sensors_lms1xx_lms5xx_tim2xx_tim5xx_tim7xx_lms1000_mrs1000_mrs6000_nav310_ld_oem15xx_ld_lrs36xx_lms4000_en_im0045927.pdf
35 
36 
37 //// By this PDF, the header contains the following information in order
38 
39 /////////////////////////////////////////////////////
40 //// command type
41 //// command
42 //// version number
43 //// device number
44 //// serial number (2 value)
45 //// device status
46 //// Telegram counter
47 //// Scan counter
48 //// Time since start up
49 //// Time of transmission
50 //// Status of digital inputs (2 value)
51 //// Status of digital outputs (2 value)
52 //// Reserved
53 //// scan frequency
54 //// measurement frequency
55 //// Amount of encoder
56 //// Amount of 16 bit channels
57 //// Content
58 //// Scale factor according to IEEE754
59 //// Scale factor offset according to IEEE754
60 //// Start angle
61 //// Size of single angular step
62 //// Amount of data
63 //// distance_1
64 //// distance_2
65 //// distance_3
66 //// ...
67 //// distance_n
68 /////////////////////////////////////////////////////
69 
70 
72 {
73  public:
75 
77  TimGrabber (const boost::asio::ip::address& ipAddress, const std::uint16_t port);
78  ~TimGrabber () noexcept override;
79 
80  void
81  start () override;
82 
83  void
84  stop () override;
85 
86  std::string
87  getName () const override;
88 
89  bool
90  isRunning () const override;
91 
92  protected:
93  pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_xyz_ptr_;
94  boost::signals2::signal<sig_cb_sick_tim_scan_point_cloud_xyz>* point_cloud_xyz_signal_;
95 
96  void
97  publishSignal ();
98 
99  //// parse received packet
100  //// used by GTEST
101  void
102  processTimPacket (std::string const& packet);
103 
104  //// check size of lookup tables
105  //// rebuild if lookup tables have different size
106  void
107  updateLookupTables ();
108 
109  //// convert std::vector (distance) to pcl::PointCloud
110  //// used by GTEST
111  void
112  toPointClouds ();
113 
114  private:
115  constexpr static float angle_start_ = - 1.0 * M_PI / 4.0;
116  constexpr static float angle_range_ = 2.0 * M_PI * 3.0 / 4.0;
117 
118  //// lookup tables for calculaing 2d-coordinate
119  //// reset lookup tables if amount of received data is different
120  std::vector<float> cos_dynamic_lookup_table_;
121  std::vector<float> sin_dynamic_lookup_table_;
122 
123  std::array<char, 4000> received_packet_;
124  std::size_t length_;
125  std::istringstream iss_;
126 
127  std::size_t amount_of_data_ = 811;
128  std::vector<float> distances_;
129 
130  boost::asio::ip::tcp::endpoint tcp_endpoint_;
131  boost::asio::io_service tim_io_service_;
132  boost::asio::ip::tcp::socket tim_socket_;
133  //// wait time for receiving data (on the order of milliseconds)
134  unsigned int wait_time_milliseconds_ = 0;
135 
136  pcl::EventFrequency frequency_;
137  mutable std::mutex frequency_mutex_;
138 
139  std::thread grabber_thread_;
140  bool is_running_ = false;
141 
142  void
143  initialize ();
144 
145  float
146  getFramesPerSecond () const override;
147 
148  void
149  buildLookupTables ();
150 
151  //// check received packet is valid
152  bool
153  isValidPacket () const;
154 
155  //// receive packet (named CoLaA; SICK sensors Communication Language)
156  void
157  receiveTimPacket ();
158 
159  void
160  parsePacketHeader (std::string const& header);
161  void
162  parsePacketBody (std::string const& body);
163 
164  void
165  processGrabbing ();
166 };
167 }
A helper class to measure frequency of a certain event.
Definition: time.h:133
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< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
TimGrabber(const boost::asio::ip::address &ipAddress, const std::uint16_t port)
~TimGrabber() noexcept override
void(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &) sig_cb_sick_tim_scan_point_cloud_xyz
Definition: tim_grabber.h:74
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
A point structure representing Euclidean xyz coordinates.