Apollo 10.0
自动驾驶开放平台
apollo::drivers::velodyne::SocketInput类 参考

Live Velodyne input from socket. 更多...

#include <socket_input.h>

类 apollo::drivers::velodyne::SocketInput 继承关系图:
apollo::drivers::velodyne::SocketInput 的协作图:

Public 成员函数

 SocketInput ()
 constructor
 
virtual ~SocketInput ()
 destructor
 
void init (const int &port) override
 
int get_firing_data_packet (VelodynePacket *pkt)
 Get one velodyne packet.
 
int get_positioning_data_packet (NMEATimePtr nmea_time)
 
- Public 成员函数 继承自 apollo::drivers::velodyne::Input
 Input ()
 
virtual ~Input ()
 
virtual void init ()
 

额外继承的成员函数

- Protected 成员函数 继承自 apollo::drivers::velodyne::Input
bool exract_nmea_time_from_packet (NMEATimePtr nmea_time, const uint8_t *bytes)
 

详细描述

Live Velodyne input from socket.

在文件 socket_input.h35 行定义.

构造及析构函数说明

◆ SocketInput()

apollo::drivers::velodyne::SocketInput::SocketInput ( )

constructor

参数
private_nhprivate node handle for driver
udp_portUDP port number to connect

在文件 socket_input.cc51 行定义.

51: sockfd_(-1), port_(0) {}

◆ ~SocketInput()

apollo::drivers::velodyne::SocketInput::~SocketInput ( void  )
virtual

destructor

在文件 socket_input.cc54 行定义.

54{ (void)close(sockfd_); }

成员函数说明

◆ 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.cc93 行定义.

93 {
94 // double time1 = ros::Time::now().toSec();
95 double time1 = apollo::cyber::Time().Now().ToSecond();
96 while (true) {
97 if (!input_available(POLL_TIMEOUT)) {
98 return SOCKET_TIMEOUT;
99 }
100 // Receive packets that should now be available from the
101 // socket using a blocking read.
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 // read successful, done now
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 }
122 double time2 = apollo::cyber::Time().Now().ToSecond();
123 pkt->set_stamp(apollo::cyber::Time((time2 + time1) / 2.0).ToNanosecond());
124
125 return 0;
126}
Cyber has builtin time type Time.
Definition time.h:31
static Time Now()
get the current time.
Definition time.cc:57
double ToSecond() const
convert time to second.
Definition time.cc:77
#define AERROR
Definition log.h:44

◆ get_positioning_data_packet()

int apollo::drivers::velodyne::SocketInput::get_positioning_data_packet ( NMEATimePtr  nmea_time)
virtual

实现了 apollo::drivers::velodyne::Input.

在文件 socket_input.cc128 行定义.

128 {
129 while (true) {
130 if (!input_available(POLL_TIMEOUT)) {
131 return 1;
132 }
133 // Receive packets that should now be available from the
134 // socket using a blocking read.
135 // Last 234 bytes not use
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 // read successful, exract nmea time
149 if (exract_nmea_time_from_packet(nmea_time, bytes)) {
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}
bool exract_nmea_time_from_packet(NMEATimePtr nmea_time, const uint8_t *bytes)
Definition input.cc:23
#define AINFO
Definition log.h:42

◆ init()

void apollo::drivers::velodyne::SocketInput::init ( const int &  port)
overridevirtual

重载 apollo::drivers::velodyne::Input .

在文件 socket_input.cc56 行定义.

56 {
57 if (sockfd_ != -1) {
58 (void)close(sockfd_);
59 }
60
61 // connect to Velodyne UDP port
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; // my address information
72 memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros
73 my_addr.sin_family = AF_INET; // host byte order
74 my_addr.sin_port = htons(uint16_t(port)); // short, in network byte order
75 my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP
76 // my_addr.sin_addr.s_addr = inet_addr("192.168.1.100");
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}

该类的文档由以下文件生成: