1
+ // Copyright 2024 TIER IV, Inc.
2
+ //
3
+ // Licensed under the Apache License, Version 2.0 (the "License");
4
+ // you may not use this file except in compliance with the License.
5
+ // You may obtain a copy of the License at
6
+ //
7
+ // http://www.apache.org/licenses/LICENSE-2.0
8
+ //
9
+ // Unless required by applicable law or agreed to in writing, software
10
+ // distributed under the License is distributed on an "AS IS" BASIS,
11
+ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12
+ // See the License for the specific language governing permissions and
13
+ // limitations under the License.
14
+
1
15
#pragma once
16
+
2
17
#include " nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_sensor.hpp"
3
18
4
- namespace nebula
5
- {
6
- namespace drivers
19
+ #include < rclcpp/logger.hpp>
20
+ #include < rclcpp/logging.hpp>
21
+
22
+ #include < cmath>
23
+
24
+ namespace nebula ::drivers
7
25
{
8
26
9
27
class VLS128 : public VelodyneSensor
10
28
{
11
29
public:
12
30
// To ignore an empty data blocks in VLS128 case
13
31
// / @brief VLS128 Dual return mode data structure in VLS128 User manual p.57
14
- int getNumPaddingBlocks (bool dual_return)
32
+ int get_num_padding_blocks (bool dual_return) override
15
33
{
16
34
if (dual_return) return 4 ;
17
35
return 0 ;
18
36
}
19
37
20
- // calculate and stack the firing timing for each laser timeing
21
- // / @brief laser timing for VLS128 from VLS128 User manual in p.61
22
- bool fillAzimuthCache ()
38
+ // calculate and stack the firing timing for each laser timeing
39
+ // / @brief laser timing for VLS128 from VLS128 User manual in p.61
40
+ bool fill_azimuth_cache () override
23
41
{
24
42
for (uint8_t i = 0 ; i < 16 ; i++) {
25
- laser_azimuth_cache_[i] = (VLS128_CHANNEL_DURATION / VLS128_SEQ_DURATION ) * (i + i / 8 );
43
+ laser_azimuth_cache_[i] = (vls128_channel_duration / vls128_seq_duration ) * (i + i / 8 );
26
44
}
27
45
return true ;
28
46
}
@@ -32,8 +50,8 @@ class VLS128 : public VelodyneSensor
32
50
// / @param azimuth_diff Azimuth difference
33
51
// / @param firing_order Firing order
34
52
// / @return Corrected azimuth
35
- uint16_t getAzimuthCorrected (
36
- uint16_t azimuth, float azimuth_diff, int /* firing_sequence */ , int firing_order)
53
+ uint16_t get_azimuth_corrected (
54
+ uint16_t azimuth, float azimuth_diff, int /* firing_sequence */ , int firing_order) override
37
55
{
38
56
float azimuth_corrected = azimuth + (azimuth_diff * laser_azimuth_cache_[firing_order]);
39
57
@@ -42,24 +60,26 @@ class VLS128 : public VelodyneSensor
42
60
43
61
// Not succeed nebula_test on only VLP32 so add this function
44
62
// Choose the correct azimuth from the 2 azimuths
45
- uint16_t getTrueRotation (uint16_t azimuth_corrected, uint16_t /* current_block_rotation */ )
63
+ static uint16_t get_true_rotation (
64
+ uint16_t azimuth_corrected, uint16_t /* current_block_rotation */ )
46
65
{
47
66
return azimuth_corrected;
48
67
}
49
68
50
- uint getBank (uint bank, uint header) {
69
+ uint32_t get_bank (uint32_t bank, uint32_t header) override
70
+ {
51
71
// Used to detect which bank of 32 lasers is in this block.
52
72
switch (header) {
53
- case BANK_1 :
73
+ case bank_1 :
54
74
bank = 0 ;
55
75
break ;
56
- case BANK_2 :
76
+ case bank_2 :
57
77
bank = 32 ;
58
78
break ;
59
- case BANK_3 :
79
+ case bank_3 :
60
80
bank = 64 ;
61
81
break ;
62
- case BANK_4 :
82
+ case bank_4 :
63
83
bank = 96 ;
64
84
break ;
65
85
default :
@@ -71,15 +91,12 @@ class VLS128 : public VelodyneSensor
71
91
return bank;
72
92
}
73
93
74
- int getFiringOrder (int channels, int scans_per_firing)
94
+ int get_firing_order (int channels, int scans_per_firing) override
75
95
{
76
96
return channels / scans_per_firing;
77
97
}
78
98
79
- int getChannelNumber (int unit_idx)
80
- {
81
- return unit_idx % channels_per_firing_sequence;
82
- }
99
+ int get_channel_number (int unit_idx) override { return unit_idx % channels_per_firing_sequence; }
83
100
84
101
constexpr static int num_maintenance_periods = 1 ;
85
102
@@ -98,19 +115,20 @@ class VLS128 : public VelodyneSensor
98
115
constexpr static double offset_packet_time = 8.7 * 1e-6 ;
99
116
100
117
/* * Special Definitions for VLS128 support **/
101
- constexpr static const float VLP128_DISTANCE_RESOLUTION = 0 .004f ; // [m]
118
+ constexpr static const float vls128_distance_resolution = 0 .004f ; // [m]
102
119
103
- constexpr static const float VLS128_CHANNEL_DURATION = 2 .665f ; // [µs] Channels corresponds to one laser firing
120
+ constexpr static const float vls128_channel_duration =
121
+ 2 .665f ; // [µs] Channels corresponds to one laser firing
104
122
105
- constexpr static const float VLS128_SEQ_DURATION = 53 .3f ; // [µs] Sequence is a set of laser firings including recharging
123
+ constexpr static const float vls128_seq_duration =
124
+ 53 .3f ; // [µs] Sequence is a set of laser firings including recharging
106
125
// These are used to detect which bank of 32 lasers is in this block
107
- constexpr static const uint16_t BANK_1 = 0xeeff ;
108
- constexpr static const uint16_t BANK_2 = 0xddff ;
109
- constexpr static const uint16_t BANK_3 = 0xccff ;
110
- constexpr static const uint16_t BANK_4 = 0xbbff ;
126
+ constexpr static const uint16_t bank_1 = 0xeeff ;
127
+ constexpr static const uint16_t bank_2 = 0xddff ;
128
+ constexpr static const uint16_t bank_3 = 0xccff ;
129
+ constexpr static const uint16_t bank_4 = 0xbbff ;
111
130
112
131
private:
113
- float laser_azimuth_cache_[16 ];
132
+ float laser_azimuth_cache_[16 ]{} ;
114
133
};
115
- } // namespace drivers
116
- } // namespace nebula
134
+ } // namespace nebula::drivers
0 commit comments