Live Velodyne input from socket.
更多...
#include <socket_input.h>
Live Velodyne input from socket.
在文件 socket_input.h 第 35 行定义.
◆ SocketInput()
apollo::drivers::velodyne::SocketInput::SocketInput |
( |
| ) |
|
constructor
- 参数
-
private_nh | private node handle for driver |
udp_port | UDP port number to connect |
在文件 socket_input.cc 第 51 行定义.
51: sockfd_(-1), port_(0) {}
◆ ~SocketInput()
apollo::drivers::velodyne::SocketInput::~SocketInput |
( |
void |
| ) |
|
|
virtual |
◆ get_firing_data_packet()
int apollo::drivers::velodyne::SocketInput::get_firing_data_packet |
( |
VelodynePacket * |
pkt | ) |
|
|
virtual |
Get one velodyne packet.
实现了 apollo::drivers::velodyne::Input.
在文件 socket_input.cc 第 93 行定义.
93 {
94
96 while (true) {
97 if (!input_available(POLL_TIMEOUT)) {
98 return SOCKET_TIMEOUT;
99 }
100
101
102 uint8_t bytes[FIRING_DATA_PACKET_SIZE];
103 ssize_t nbytes =
104 recvfrom(sockfd_, bytes, FIRING_DATA_PACKET_SIZE, 0, nullptr, nullptr);
105
106 if (nbytes < 0) {
107 if (errno != EWOULDBLOCK) {
108 AERROR <<
"recvfail from port " << port_;
109 return RECEIVE_FAIL;
110 }
111 }
112
113 if ((size_t)nbytes == FIRING_DATA_PACKET_SIZE) {
114
115 pkt->set_data(bytes, FIRING_DATA_PACKET_SIZE);
116 break;
117 }
118
119 AERROR <<
"Incomplete Velodyne rising data packet read: " << nbytes
120 << " bytes from port " << port_;
121 }
124
125 return 0;
126}
Cyber has builtin time type Time.
static Time Now()
get the current time.
double ToSecond() const
convert time to second.
◆ get_positioning_data_packet()
int apollo::drivers::velodyne::SocketInput::get_positioning_data_packet |
( |
NMEATimePtr |
nmea_time | ) |
|
|
virtual |
实现了 apollo::drivers::velodyne::Input.
在文件 socket_input.cc 第 128 行定义.
128 {
129 while (true) {
130 if (!input_available(POLL_TIMEOUT)) {
131 return 1;
132 }
133
134
135
136 uint8_t bytes[POSITIONING_DATA_PACKET_SIZE];
137 ssize_t nbytes = recvfrom(sockfd_, bytes, POSITIONING_DATA_PACKET_SIZE, 0,
138 nullptr, nullptr);
139
140 if (nbytes < 0) {
141 if (errno != EWOULDBLOCK) {
142 AERROR <<
"recvfail from port " << port_;
143 return 1;
144 }
145 }
146
147 if ((size_t)nbytes == POSITIONING_DATA_PACKET_SIZE) {
148
150 break;
151 } else {
152 return 1;
153 }
154 }
155
156 AINFO <<
"incomplete Velodyne packet read: " << nbytes
157 << " bytes from port " << port_;
158 }
159
160 return 0;
161}
◆ init()
void apollo::drivers::velodyne::SocketInput::init |
( |
const int & |
port | ) |
|
|
overridevirtual |
重载 apollo::drivers::velodyne::Input .
在文件 socket_input.cc 第 56 行定义.
56 {
57 if (sockfd_ != -1) {
58 (void)close(sockfd_);
59 }
60
61
62 AINFO <<
"Opening UDP socket: port " << uint16_t(port);
63 port_ = port;
64 sockfd_ = socket(AF_INET, SOCK_DGRAM, 0);
65
66 if (sockfd_ == -1) {
67 AERROR <<
"Init socket failed, UDP port is " << port;
68 return;
69 }
70
71 sockaddr_in my_addr;
72 memset(&my_addr, 0, sizeof(my_addr));
73 my_addr.sin_family = AF_INET;
74 my_addr.sin_port = htons(uint16_t(port));
75 my_addr.sin_addr.s_addr = INADDR_ANY;
76
77
78 if (bind(sockfd_, reinterpret_cast<sockaddr *>(&my_addr), sizeof(sockaddr)) ==
79 -1) {
80 AERROR <<
"Socket bind failed! Port " << port_;
81 return;
82 }
83
84 if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
85 AERROR <<
"non-block! Port " << port_;
86 return;
87 }
88
89 AINFO <<
"Velodyne socket fd is " << sockfd_ <<
", port " << port_;
90}
该类的文档由以下文件生成: