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

#include <driver.h>

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

Public 成员函数

 VelodyneDriver ()
 
 VelodyneDriver (const Config &config)
 
 VelodyneDriver (const std::shared_ptr< cyber::Node > &node, const Config &config)
 
virtual ~VelodyneDriver ()
 
virtual bool Poll (const std::shared_ptr< VelodyneScan > &scan)
 poll the device
 
bool Init () override
 Initialize the lidar driver.
 
virtual void PollPositioningPacket ()
 
void SetPacketRate (const double packet_rate)
 
void DevicePoll ()
 
- Public 成员函数 继承自 apollo::drivers::lidar::LidarDriver
 LidarDriver ()
 Constructor
 
 LidarDriver (const std::shared_ptr<::apollo::cyber::Node > &node)
 
virtual ~LidarDriver ()=default
 Destructor
 

Protected 成员函数

virtual int PollStandard (std::shared_ptr< VelodyneScan > scan)
 
bool SetBaseTime ()
 
void SetBaseTimeFromNmeaTime (NMEATimePtr nmea_time, uint64_t *basetime)
 
void UpdateGpsTopHour (uint32_t current_time)
 

Protected 属性

std::thread poll_thread_
 
Config config_
 
std::shared_ptr< apollo::cyber::Writer< VelodyneScan > > writer_
 
std::unique_ptr< Inputinput_ = nullptr
 
std::unique_ptr< Inputpositioning_input_ = nullptr
 
std::string topic_
 
double packet_rate_ = 0.0
 
uint64_t basetime_ = 0
 
uint32_t last_gps_time_ = 0
 
uint64_t last_count_ = 0
 
std::thread positioning_thread_
 
- Protected 属性 继承自 apollo::drivers::lidar::LidarDriver
std::shared_ptr< cyber::Nodenode_
 

静态 Protected 属性

static uint64_t sync_counter = 0
 

详细描述

在文件 driver.h44 行定义.

构造及析构函数说明

◆ VelodyneDriver() [1/3]

apollo::drivers::velodyne::VelodyneDriver::VelodyneDriver ( )
inline

在文件 driver.h46 行定义.

46{}

◆ VelodyneDriver() [2/3]

apollo::drivers::velodyne::VelodyneDriver::VelodyneDriver ( const Config config)
inlineexplicit

在文件 driver.h47 行定义.

◆ VelodyneDriver() [3/3]

apollo::drivers::velodyne::VelodyneDriver::VelodyneDriver ( const std::shared_ptr< cyber::Node > &  node,
const Config config 
)
inline

在文件 driver.h48 行定义.

50 : config_(config) {
51 node_ = node;
52 }
std::shared_ptr< cyber::Node > node_
Definition driver_base.h:66

◆ ~VelodyneDriver()

apollo::drivers::velodyne::VelodyneDriver::~VelodyneDriver ( )
virtual

在文件 driver.cc34 行定义.

34 {
35 if (poll_thread_.joinable()) {
36 poll_thread_.join();
37 }
38 if (positioning_thread_.joinable()) {
40 }
41}

成员函数说明

◆ DevicePoll()

void apollo::drivers::velodyne::VelodyneDriver::DevicePoll ( )

在文件 driver.cc288 行定义.

288 {
289 while (!apollo::cyber::IsShutdown()) {
290 // poll device until end of file
291 std::shared_ptr<VelodyneScan> scan = std::make_shared<VelodyneScan>();
292 bool ret = Poll(scan);
293 if (ret) {
294 apollo::common::util::FillHeader("velodyne", scan.get());
295 writer_->Write(scan);
296 } else {
297 AWARN << "device poll failed";
298 }
299 }
300}
virtual bool Poll(const std::shared_ptr< VelodyneScan > &scan)
poll the device
Definition driver.cc:107
std::shared_ptr< apollo::cyber::Writer< VelodyneScan > > writer_
Definition driver.h:64
#define AWARN
Definition log.h:43
bool IsShutdown()
Definition state.h:46

◆ Init()

bool apollo::drivers::velodyne::VelodyneDriver::Init ( )
overridevirtual

Initialize the lidar driver.

返回
If the initialization is successful.

实现了 apollo::drivers::lidar::LidarDriver.

在文件 driver.cc43 行定义.

43 {
44 double frequency = (config_.rpm() / 60.0); // expected Hz rate
45
46 // default number of packets for each scan is a single revolution
47 // (fractions rounded up)
48 config_.set_npackets(static_cast<int>(ceil(packet_rate_ / frequency)));
49 AINFO << "publishing " << config_.npackets() << " packets per scan";
50
51 // open Velodyne input device
52
53 input_.reset(new SocketInput());
54 writer_ = node_->CreateWriter<VelodyneScan>(config_.scan_channel());
55 positioning_input_.reset(new SocketInput());
58
59 // raw data output topic
61 std::thread(&VelodyneDriver::PollPositioningPacket, this);
62 poll_thread_ = std::thread(&VelodyneDriver::DevicePoll, this);
63 return true;
64}
std::unique_ptr< Input > positioning_input_
Definition driver.h:66
std::unique_ptr< Input > input_
Definition driver.h:65
#define AINFO
Definition log.h:42

◆ Poll()

bool apollo::drivers::velodyne::VelodyneDriver::Poll ( const std::shared_ptr< VelodyneScan > &  scan)
virtual

poll the device

返回
true unless end of file reached

apollo::drivers::velodyne::Velodyne64Driver 重载.

在文件 driver.cc107 行定义.

107 {
108 // Allocate a new shared pointer for zero-copy sharing with other nodelets.
109 if (basetime_ == 0) {
110 // waiting for positioning data
111 std::this_thread::sleep_for(std::chrono::microseconds(100));
112 AWARN << "basetime is zero";
113 return false;
114 }
115
116 int poll_result = PollStandard(scan);
117
118 if (poll_result == SOCKET_TIMEOUT || poll_result == RECEIVE_FAIL) {
119 return false; // poll again
120 }
121
122 if (scan->firing_pkts().empty()) {
123 AINFO << "Get an empty scan from port: " << config_.firing_data_port();
124 return false;
125 }
126
127 // publish message using time of last packet read
128 ADEBUG << "Publishing a full Velodyne scan.";
129 scan->mutable_header()->set_timestamp_sec(cyber::Time().Now().ToSecond());
130 scan->mutable_header()->set_frame_id(config_.frame_id());
131 scan->set_model(config_.model());
132 scan->set_mode(config_.mode());
133 // we use first packet gps time update gps base hour
134 // in cloud nodelet, will update base time packet by packet
135 uint32_t current_secs = *(reinterpret_cast<uint32_t*>(
136 const_cast<char*>(scan->firing_pkts(0).data().data() + 1200)));
137
138 UpdateGpsTopHour(current_secs);
139 scan->set_basetime(basetime_);
140
141 return true;
142}
virtual int PollStandard(std::shared_ptr< VelodyneScan > scan)
Definition driver.cc:144
void UpdateGpsTopHour(uint32_t current_time)
Definition driver.cc:213
#define ADEBUG
Definition log.h:41

◆ PollPositioningPacket()

void apollo::drivers::velodyne::VelodyneDriver::PollPositioningPacket ( void  )
virtual

在文件 driver.cc175 行定义.

175 {
176 while (!cyber::IsShutdown()) {
177 NMEATimePtr nmea_time(new NMEATime);
178 bool ret = true;
179 if (config_.has_use_gps_time() && !config_.use_gps_time()) {
180 time_t t = time(NULL);
181 struct tm current_time;
182 localtime_r(&t, &current_time);
183 nmea_time->year = static_cast<uint16_t>(current_time.tm_year - 100);
184 nmea_time->mon = static_cast<uint16_t>(current_time.tm_mon + 1);
185 nmea_time->day = static_cast<uint16_t>(current_time.tm_mday);
186 nmea_time->hour = static_cast<uint16_t>(current_time.tm_hour);
187 nmea_time->min = static_cast<uint16_t>(current_time.tm_min);
188 nmea_time->sec = static_cast<uint16_t>(current_time.tm_sec);
189 AINFO << "Get NMEA Time from local time :"
190 << "year:" << nmea_time->year << "mon:" << nmea_time->mon
191 << "day:" << nmea_time->day << "hour:" << nmea_time->hour
192 << "min:" << nmea_time->min << "sec:" << nmea_time->sec;
193 } else {
194 while (!cyber::IsShutdown()) {
195 int rc = positioning_input_->get_positioning_data_packet(nmea_time);
196 if (rc == 0) {
197 break; // got a full packet
198 }
199 if (rc < 0) {
200 ret = false; // end of file reached
201 }
202 }
203 }
204
205 if (basetime_ == 0 && ret) {
207 } else {
208 std::this_thread::sleep_for(std::chrono::milliseconds(1));
209 }
210 }
211}
void SetBaseTimeFromNmeaTime(NMEATimePtr nmea_time, uint64_t *basetime)
Definition driver.cc:66
std::shared_ptr< NMEATime > NMEATimePtr
Definition input.h:46

◆ PollStandard()

int apollo::drivers::velodyne::VelodyneDriver::PollStandard ( std::shared_ptr< VelodyneScan scan)
protectedvirtual

在文件 driver.cc144 行定义.

144 {
145 // Since the velodyne delivers data at a very high rate, keep reading and
146 // publishing scans as fast as possible.
147 while ((config_.use_poll_sync() &&
149 scan->firing_pkts_size() < config_.npackets()) ||
152 scan->firing_pkts_size() < config_.npackets())) {
153 while (true) {
154 // keep reading until full packet received
155 VelodynePacket* packet = scan->add_firing_pkts();
156 int rc = input_->get_firing_data_packet(packet);
157
158 if (rc == 0) {
159 break; // got a full packet?
160 } else if (rc < 0) {
161 return rc;
162 }
163 }
164 }
165 if (config_.use_poll_sync()) {
166 if (config_.is_main_frame()) {
167 ++sync_counter;
168 } else {
170 }
171 }
172 return 0;
173}

◆ SetBaseTime()

bool apollo::drivers::velodyne::VelodyneDriver::SetBaseTime ( )
protected

在文件 driver.cc86 行定义.

86 {
87 NMEATimePtr nmea_time(new NMEATime);
88 while (true) {
89 int rc = input_->get_positioning_data_packet(nmea_time);
90 if (rc == 0) {
91 break; // got a full packet
92 }
93 if (rc < 0) {
94 return false; // end of file reached
95 }
96 }
97
100 return true;
101}

◆ SetBaseTimeFromNmeaTime()

void apollo::drivers::velodyne::VelodyneDriver::SetBaseTimeFromNmeaTime ( NMEATimePtr  nmea_time,
uint64_t *  basetime 
)
protected

在文件 driver.cc66 行定义.

67 {
68 struct tm time;
69 std::memset(&time, 0, sizeof(tm));
70 time.tm_year = nmea_time->year + (2000 - 1900);
71 time.tm_mon = nmea_time->mon - 1;
72 time.tm_mday = nmea_time->day;
73 time.tm_hour = nmea_time->hour;
74
75 // set last gps time using gps socket packet
77 static_cast<uint32_t>((nmea_time->min * 60 + nmea_time->sec) * 1e6);
78
79 AINFO << "Set base unix time : " << time.tm_year << "-" << time.tm_mon << "-"
80 << time.tm_mday << " " << time.tm_hour << ":" << time.tm_min << ":"
81 << time.tm_sec;
82 uint64_t unix_base = static_cast<uint64_t>(timegm(&time));
83 *basetime = unix_base * static_cast<uint64_t>(1e6);
84}

◆ SetPacketRate()

void apollo::drivers::velodyne::VelodyneDriver::SetPacketRate ( const double  packet_rate)
inline

在文件 driver.h58 行定义.

58{ packet_rate_ = packet_rate; }

◆ UpdateGpsTopHour()

void apollo::drivers::velodyne::VelodyneDriver::UpdateGpsTopHour ( uint32_t  current_time)
protected

在文件 driver.cc213 行定义.

213 {
214 if (last_gps_time_ == 0) {
215 last_gps_time_ = current_time;
216 return;
217 }
218 if (last_gps_time_ > current_time) {
219 if ((last_gps_time_ - current_time) > 3599000000) {
220 basetime_ += static_cast<uint64_t>(3600 * 1e6);
221 AINFO << "Base time plus 3600s. Model: " << config_.model() << std::fixed
222 << ". current:" << current_time << ", last time:" << last_gps_time_;
223 } else {
224 AWARN << "Current stamp:" << std::fixed << current_time
225 << " less than previous stamp:" << last_gps_time_
226 << ". GPS time stamp maybe incorrect!";
227 }
228 }
229 last_gps_time_ = current_time;
230}

类成员变量说明

◆ basetime_

uint64_t apollo::drivers::velodyne::VelodyneDriver::basetime_ = 0
protected

在文件 driver.h70 行定义.

◆ config_

Config apollo::drivers::velodyne::VelodyneDriver::config_
protected

在文件 driver.h63 行定义.

◆ input_

std::unique_ptr<Input> apollo::drivers::velodyne::VelodyneDriver::input_ = nullptr
protected

在文件 driver.h65 行定义.

◆ last_count_

uint64_t apollo::drivers::velodyne::VelodyneDriver::last_count_ = 0
protected

在文件 driver.h72 行定义.

◆ last_gps_time_

uint32_t apollo::drivers::velodyne::VelodyneDriver::last_gps_time_ = 0
protected

在文件 driver.h71 行定义.

◆ packet_rate_

double apollo::drivers::velodyne::VelodyneDriver::packet_rate_ = 0.0
protected

在文件 driver.h68 行定义.

◆ poll_thread_

std::thread apollo::drivers::velodyne::VelodyneDriver::poll_thread_
protected

在文件 driver.h62 行定义.

◆ positioning_input_

std::unique_ptr<Input> apollo::drivers::velodyne::VelodyneDriver::positioning_input_ = nullptr
protected

在文件 driver.h66 行定义.

◆ positioning_thread_

std::thread apollo::drivers::velodyne::VelodyneDriver::positioning_thread_
protected

在文件 driver.h75 行定义.

◆ sync_counter

uint64_t apollo::drivers::velodyne::VelodyneDriver::sync_counter = 0
staticprotected

在文件 driver.h73 行定义.

◆ topic_

std::string apollo::drivers::velodyne::VelodyneDriver::topic_
protected

在文件 driver.h67 行定义.

◆ writer_

std::shared_ptr<apollo::cyber::Writer<VelodyneScan> > apollo::drivers::velodyne::VelodyneDriver::writer_
protected

在文件 driver.h64 行定义.


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