168 lines
4.1 KiB
C++
168 lines
4.1 KiB
C++
/*
|
|
* SPDX-License-Identifier: BSD-3-Clause
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2020-, Open Perception
|
|
* Copyright (c) 2020, ysuzuki19
|
|
*
|
|
* All rights reserved
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <pcl/pcl_exports.h>
|
|
#include <pcl/console/print.h>
|
|
#include <pcl/common/time.h>
|
|
#include <pcl/io/grabber.h>
|
|
#include <pcl/point_types.h>
|
|
#include <pcl/point_cloud.h>
|
|
#include <boost/asio.hpp>
|
|
#include <mutex>
|
|
#include <vector>
|
|
#include <string>
|
|
#include <thread>
|
|
|
|
namespace pcl
|
|
{
|
|
|
|
//// note: Protocol named CoLaA (used by SICK) has some information.
|
|
//// In this Grabber, only the amount_of_data is used, so other information is truncated.
|
|
//// Details of the protocol can be found at the following URL.
|
|
|
|
//// pp.87~89 (table)
|
|
|
|
//// 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
|
|
|
|
|
|
//// By this PDF, the header contains the following information in order
|
|
|
|
/////////////////////////////////////////////////////
|
|
//// command type
|
|
//// command
|
|
//// version number
|
|
//// device number
|
|
//// serial number (2 value)
|
|
//// device status
|
|
//// Telegram counter
|
|
//// Scan counter
|
|
//// Time since start up
|
|
//// Time of transmission
|
|
//// Status of digital inputs (2 value)
|
|
//// Status of digital outputs (2 value)
|
|
//// Reserved
|
|
//// scan frequency
|
|
//// measurement frequency
|
|
//// Amount of encoder
|
|
//// Amount of 16 bit channels
|
|
//// Content
|
|
//// Scale factor according to IEEE754
|
|
//// Scale factor offset according to IEEE754
|
|
//// Start angle
|
|
//// Size of single angular step
|
|
//// Amount of data
|
|
//// distance_1
|
|
//// distance_2
|
|
//// distance_3
|
|
//// ...
|
|
//// distance_n
|
|
/////////////////////////////////////////////////////
|
|
|
|
|
|
class PCL_EXPORTS TimGrabber : public Grabber
|
|
{
|
|
public:
|
|
using sig_cb_sick_tim_scan_point_cloud_xyz = void (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&);
|
|
|
|
TimGrabber ();
|
|
TimGrabber (const boost::asio::ip::address& ipAddress, const std::uint16_t port);
|
|
~TimGrabber () noexcept override;
|
|
|
|
void
|
|
start () override;
|
|
|
|
void
|
|
stop () override;
|
|
|
|
std::string
|
|
getName () const override;
|
|
|
|
bool
|
|
isRunning () const override;
|
|
|
|
protected:
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_xyz_ptr_;
|
|
boost::signals2::signal<sig_cb_sick_tim_scan_point_cloud_xyz>* point_cloud_xyz_signal_;
|
|
|
|
void
|
|
publishSignal ();
|
|
|
|
//// parse received packet
|
|
//// used by GTEST
|
|
void
|
|
processTimPacket (std::string const& packet);
|
|
|
|
//// check size of lookup tables
|
|
//// rebuild if lookup tables have different size
|
|
void
|
|
updateLookupTables ();
|
|
|
|
//// convert std::vector (distance) to pcl::PointCloud
|
|
//// used by GTEST
|
|
void
|
|
toPointClouds ();
|
|
|
|
private:
|
|
constexpr static float angle_start_ = - 1.0 * M_PI / 4.0;
|
|
constexpr static float angle_range_ = 2.0 * M_PI * 3.0 / 4.0;
|
|
|
|
//// lookup tables for calculaing 2d-coordinate
|
|
//// reset lookup tables if amount of received data is different
|
|
std::vector<float> cos_dynamic_lookup_table_;
|
|
std::vector<float> sin_dynamic_lookup_table_;
|
|
|
|
std::array<char, 4000> received_packet_;
|
|
std::size_t length_;
|
|
std::istringstream iss_;
|
|
|
|
std::size_t amount_of_data_ = 811;
|
|
std::vector<float> distances_;
|
|
|
|
boost::asio::ip::tcp::endpoint tcp_endpoint_;
|
|
boost::asio::io_service tim_io_service_;
|
|
boost::asio::ip::tcp::socket tim_socket_;
|
|
//// wait time for receiving data (on the order of milliseconds)
|
|
unsigned int wait_time_milliseconds_ = 0;
|
|
|
|
pcl::EventFrequency frequency_;
|
|
mutable std::mutex frequency_mutex_;
|
|
|
|
std::thread grabber_thread_;
|
|
bool is_running_ = false;
|
|
|
|
void
|
|
initialize ();
|
|
|
|
float
|
|
getFramesPerSecond () const override;
|
|
|
|
void
|
|
buildLookupTables ();
|
|
|
|
//// check received packet is valid
|
|
bool
|
|
isValidPacket () const;
|
|
|
|
//// receive packet (named CoLaA; SICK sensors Communication Language)
|
|
void
|
|
receiveTimPacket ();
|
|
|
|
void
|
|
parsePacketHeader (std::string const& header);
|
|
void
|
|
parsePacketBody (std::string const& body);
|
|
|
|
void
|
|
processGrabbing ();
|
|
};
|
|
}
|