Apollo  6.0
Open source self driving car software
driver.h
Go to the documentation of this file.
1 /******************************************************************************
2  * Copyright 2017 The Apollo Authors. All Rights Reserved.
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *****************************************************************************/
16 
17 #pragma once
18 
19 #include <memory>
20 #include <string>
21 
22 #include "modules/drivers/lidar/proto/config.pb.h"
23 #include "modules/drivers/lidar/proto/velodyne.pb.h"
24 #include "modules/drivers/lidar/proto/velodyne_config.pb.h"
25 
28 
29 namespace apollo {
30 namespace drivers {
31 namespace velodyne {
32 
33 constexpr int BLOCKS_PER_PACKET = 12;
34 constexpr int BLOCK_SIZE = 100;
35 
36 constexpr double PACKET_RATE_VLP16 = 754;
37 constexpr double PACKET_RATE_HDL32E = 1808.0;
38 constexpr double PACKET_RATE_HDL64E_S2 = 3472.17;
39 constexpr double PACKET_RATE_HDL64E_S3S = 3472.17;
40 constexpr double PACKET_RATE_HDL64E_S3D = 5789;
41 constexpr double PACKET_RATE_VLS128 = 6250.0;
42 constexpr double PACKET_RATE_VLP32C = 1507.0;
43 
45  public:
47  explicit VelodyneDriver(const Config &config) : config_(config) {}
48  VelodyneDriver(const std::shared_ptr<cyber::Node> &node,
49  const Config &config)
50  : config_(config) {
51  node_ = node;
52  }
53  virtual ~VelodyneDriver();
54 
55  virtual bool Poll(const std::shared_ptr<VelodyneScan> &scan);
56  bool Init() override;
57  virtual void PollPositioningPacket();
58  void SetPacketRate(const double packet_rate) { packet_rate_ = packet_rate; }
59  void DevicePoll();
60 
61  protected:
62  std::thread poll_thread_;
63  Config config_;
64  std::shared_ptr<apollo::cyber::Writer<VelodyneScan>> writer_;
65  std::unique_ptr<Input> input_ = nullptr;
66  std::unique_ptr<Input> positioning_input_ = nullptr;
67  std::string topic_;
68  double packet_rate_ = 0.0;
69 
70  uint64_t basetime_ = 0;
71  uint32_t last_gps_time_ = 0;
72  uint64_t last_count_ = 0;
73  static uint64_t sync_counter;
74 
75  std::thread positioning_thread_;
76  virtual int PollStandard(std::shared_ptr<VelodyneScan> scan);
77  bool SetBaseTime();
78  void SetBaseTimeFromNmeaTime(NMEATimePtr nmea_time, uint64_t *basetime);
79  void UpdateGpsTopHour(uint32_t current_time);
80 };
81 
83  public:
84  explicit Velodyne64Driver(const Config &config) : VelodyneDriver(config) {}
85  Velodyne64Driver(const std::shared_ptr<cyber::Node> &node,
86  const Config &config) {
87  node_ = node;
88  config_ = config;
89  }
91 
92  bool Init() override;
93  bool Poll(const std::shared_ptr<VelodyneScan> &scan) override;
94  void DevicePoll();
95 
96  private:
97  bool CheckAngle(const VelodynePacket &packet);
98  int PollStandardSync(std::shared_ptr<VelodyneScan> scan);
99 };
100 
102  public:
103  static VelodyneDriver *CreateDriver(
104  const std::shared_ptr<::apollo::cyber::Node> &node, const Config &config);
105 };
106 
107 } // namespace velodyne
108 } // namespace drivers
109 } // namespace apollo
uint64_t last_count_
Definition: driver.h:72
std::thread positioning_thread_
Definition: driver.h:75
uint64_t basetime_
Definition: driver.h:70
std::shared_ptr< cyber::Node > node_
Definition: driver_base.h:66
uint32_t last_gps_time_
Definition: driver.h:71
std::unique_ptr< Input > positioning_input_
Definition: driver.h:66
VelodyneDriver(const Config &config)
Definition: driver.h:47
constexpr double PACKET_RATE_HDL64E_S3D
Definition: driver.h:40
constexpr double PACKET_RATE_HDL32E
Definition: driver.h:37
void UpdateGpsTopHour(uint32_t current_time)
PlanningContext is the runtime context in planning. It is persistent across multiple frames...
Definition: atomic_hash_map.h:25
void SetBaseTimeFromNmeaTime(NMEATimePtr nmea_time, uint64_t *basetime)
static uint64_t sync_counter
Definition: driver.h:73
constexpr double PACKET_RATE_VLS128
Definition: driver.h:41
constexpr double PACKET_RATE_VLP16
Definition: driver.h:36
constexpr int BLOCKS_PER_PACKET
Definition: driver.h:33
The class which defines the lidar driver .
Definition: driver_base.h:45
std::unique_ptr< Input > input_
Definition: driver.h:65
Defines the CanFrame struct and CanClient interface.
virtual bool Poll(const std::shared_ptr< VelodyneScan > &scan)
VelodyneDriver()
Definition: driver.h:46
constexpr int BLOCK_SIZE
Definition: driver.h:34
std::thread poll_thread_
Definition: driver.h:62
std::shared_ptr< NMEATime > NMEATimePtr
Definition: input.h:46
constexpr double PACKET_RATE_HDL64E_S3S
Definition: driver.h:39
std::shared_ptr< apollo::cyber::Writer< VelodyneScan > > writer_
Definition: driver.h:64
constexpr double PACKET_RATE_VLP32C
Definition: driver.h:42
constexpr double PACKET_RATE_HDL64E_S2
Definition: driver.h:38
Config config_
Definition: driver.h:63
Velodyne64Driver(const Config &config)
Definition: driver.h:84
bool Init() override
Initialize the lidar driver.
VelodyneDriver(const std::shared_ptr< cyber::Node > &node, const Config &config)
Definition: driver.h:48
virtual int PollStandard(std::shared_ptr< VelodyneScan > scan)
double packet_rate_
Definition: driver.h:68
void SetPacketRate(const double packet_rate)
Definition: driver.h:58
Velodyne64Driver(const std::shared_ptr< cyber::Node > &node, const Config &config)
Definition: driver.h:85
std::string topic_
Definition: driver.h:67