1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
|
/*
* Slamtec LIDAR SDK
*
* Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
* http://www.slamtec.com
*
*/
/*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#include "sl_lidar_driver.h"
#include "hal/abs_rxtx.h"
#include "hal/socket.h"
namespace sl {
class UdpChannel : public IChannel
{
public:
UdpChannel(const std::string& ip, int port) : _binded_socket(rp::net::DGramSocket::CreateSocket()) {
_ip = ip;
_port = port;
}
bool bind(const std::string & ip, sl_s32 port)
{
_socket = rp::net::SocketAddress(ip.c_str(), port);
return true;
}
bool open()
{
if(!bind(_ip, _port))
return false;
return SL_IS_OK(_binded_socket->setPairAddress(&_socket));
}
void close()
{
_binded_socket->dispose();
_binded_socket = NULL;
}
void flush()
{
clearReadCache();
}
sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs)
{
u_result ans;
size_hint = 0;
ans = _binded_socket->waitforData(timeoutInMs);
switch (ans) {
case RESULT_OK:
size_hint = 1024; //dummy value
break;
}
return ans;
}
bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady)
{
if (actualReady)
*actualReady = size;
return (_binded_socket->waitforData(timeoutInMs) == RESULT_OK);
}
int write(const void* data, size_t size)
{
return _binded_socket->sendTo(nullptr, data, size);
}
int read(void* buffer, size_t size)
{
size_t actualGet;
u_result ans = _binded_socket->recvFrom(buffer, size, actualGet);
if (IS_FAIL(ans)) return 0;
return actualGet;
}
void clearReadCache() {
_binded_socket->clearRxCache();
}
void setStatus(_u32 flag){}
int getChannelType() {
return CHANNEL_TYPE_UDP;
}
private:
rp::net::DGramSocket * _binded_socket;
rp::net::SocketAddress _socket;
std::string _ip;
int _port;
};
Result<IChannel*> createUdpChannel(const std::string& ip, int port)
{
return new UdpChannel(ip, port);
}
}
|
