Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
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
25namespace pcl
26{
27
28//// note: Protcol 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
71class PCL_EXPORTS TimGrabber : public Grabber
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.
shared_ptr< const PointCloud< PointT > > ConstPtr
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 M_PI
Definition pcl_macros.h:201
A point structure representing Euclidean xyz coordinates.