Branch data Line data Source code
1 : : // Copyright 2017-2018 the Autoware Foundation
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 : : //
15 : : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 : :
17 : : #include <common/types.hpp>
18 : :
19 : : #include "lidar_integration/vlp16_integration_spoofer.hpp"
20 : :
21 : : using autoware::common::types::bool8_t;
22 : : using autoware::common::types::char8_t;
23 : : using autoware::common::types::float32_t;
24 : :
25 : : namespace lidar_integration
26 : : {
27 : :
28 : 19 : Vlp16IntegrationSpoofer::Vlp16IntegrationSpoofer(
29 : : const char8_t * const ip,
30 : : const uint16_t port,
31 : 19 : const float32_t rpm)
32 : : : m_running(false),
33 : 19 : m_spoofer(ip, port, rpm, m_running)
34 : : {
35 : 19 : }
36 : :
37 : 38 : Vlp16IntegrationSpoofer::~Vlp16IntegrationSpoofer()
38 : : {
39 : 19 : stop();
40 : 19 : }
41 : :
42 : 7 : void Vlp16IntegrationSpoofer::start()
43 : : {
44 : : m_running.store(true);
45 : 7 : m_spoofer.start();
46 : 7 : }
47 : :
48 [ + + ]: 26 : void Vlp16IntegrationSpoofer::stop()
49 : : {
50 : : m_running.store(false);
51 : : m_spoofer.stop();
52 : 26 : }
53 : :
54 : 19 : Vlp16IntegrationSpoofer::SpoofTask::SpoofTask(
55 : : const char8_t * const ip,
56 : : const uint16_t port,
57 : : const float32_t rpm,
58 : 19 : const std::atomic<bool8_t> & running)
59 : : : m_udp_sender(ip, port),
60 : : m_last_send_time(std::chrono::nanoseconds(0)),
61 : : m_all_flat_ground_packet({}),
62 : : m_flat_ground_wall_packet({}),
63 : : m_running(running),
64 : : m_send_period(std::chrono::microseconds(1000000LL / 754LL)),
65 : : // 754 packets/second, from spec sheet
66 [ + - ]: 19 : m_azimuth_increment(0)
67 : : {
68 [ + - ]: 19 : initialize_packets(rpm);
69 : 19 : const float32_t period_ms = 60.0E3F / rpm;
70 : 19 : const float32_t dth_tic =
71 : : 55.296E-3F * 2.0F * NUM_BLOCKS_PER_PACKET *
72 : : static_cast<float32_t>(AZIMUTH_ROTATION_RESOLUTION) / period_ms;
73 : 19 : m_azimuth_increment = static_cast<uint16_t>(dth_tic);
74 : 19 : }
75 : :
76 : 7 : void Vlp16IntegrationSpoofer::SpoofTask::task_function()
77 : : {
78 : : using namespace std::chrono_literals; // NOLINT
79 : : using std::chrono::steady_clock;
80 : :
81 : 7 : steady_clock::time_point last_send_time(steady_clock::now());
82 : : bool8_t just_sent_all_ground = true;
83 : 7 : update_packet_azimuth(m_all_flat_ground_packet, m_azimuth_increment);
84 [ + + ]: 434455 : while (m_running.load(std::memory_order_relaxed)) {
85 : 434448 : const steady_clock::time_point right_now(steady_clock::now());
86 [ + + ]: 434448 : if ((right_now - last_send_time) > m_send_period) {
87 [ + + ]: 53082 : if (just_sent_all_ground) {
88 : : // update azimuth
89 : 26543 : update_packet_azimuth(
90 : 26543 : m_flat_ground_wall_packet,
91 : 26543 : static_cast<uint16_t>(2U * m_azimuth_increment));
92 : : m_udp_sender.send(m_flat_ground_wall_packet);
93 : : } else {
94 : : // update azimuth
95 : 26539 : update_packet_azimuth(
96 : : m_all_flat_ground_packet,
97 : 26539 : static_cast<uint16_t>(2U * m_azimuth_increment));
98 : : m_udp_sender.send(m_all_flat_ground_packet);
99 : : }
100 : 53082 : just_sent_all_ground = !just_sent_all_ground;
101 : : last_send_time = right_now;
102 : 53082 : m_send_count += 1;
103 : : }
104 : :
105 : 434448 : std::this_thread::sleep_for(100us);
106 : : }
107 : 7 : }
108 : :
109 : 652116 : void Vlp16IntegrationSpoofer::SpoofTask::uint16_to_bytes(const uint16_t val, uint8_t arr[])
110 : : {
111 : 652116 : arr[0U] = static_cast<uint8_t>(val & 0xFFU);
112 : 652116 : arr[1U] = static_cast<uint8_t>(val >> 8U);
113 : 652116 : }
114 : :
115 : 637068 : uint16_t Vlp16IntegrationSpoofer::SpoofTask::bytes_to_uint16(const uint8_t arr[])
116 : : {
117 : 637068 : return static_cast<uint16_t>((arr[1U] << 8U) + arr[0U]);
118 : : }
119 : :
120 : 53089 : void Vlp16IntegrationSpoofer::SpoofTask::update_packet_azimuth(
121 : : Packet & pkt, const uint16_t dth_tic)
122 : : {
123 [ + + ]: 690157 : for (uint32_t idx = 0U; idx < NUM_BLOCKS_PER_PACKET; ++idx) {
124 : : DataBlock & blk = pkt.blocks[idx];
125 : : // azimuth
126 : 637068 : uint16_t th_tic = bytes_to_uint16(blk.azimuth_bytes);
127 : 637068 : th_tic = static_cast<uint16_t>((th_tic + dth_tic) % AZIMUTH_ROTATION_RESOLUTION);
128 : 637068 : uint16_to_bytes(th_tic, blk.azimuth_bytes);
129 : : }
130 : 53089 : }
131 : :
132 : 38 : void Vlp16IntegrationSpoofer::SpoofTask::init_packet(
133 : : Packet & pkt,
134 : : const float32_t rpm,
135 : : const uint16_t (& distances)[RAY_SIZE]) const
136 : : {
137 : : // timestamp is not read atm
138 : : // factory bytes also not read
139 : : // delta azimuth per index based on rpm
140 : 38 : const float32_t period_ms = 60.0E3F / rpm;
141 : 38 : const float32_t dth_tic =
142 : : 2.0F * 55.296E-3F * static_cast<float32_t>(AZIMUTH_ROTATION_RESOLUTION) / period_ms;
143 [ + + ]: 494 : for (uint32_t idx = 0U; idx < NUM_BLOCKS_PER_PACKET; ++idx) {
144 : : DataBlock & blk = pkt.blocks[idx];
145 : : // flag
146 : 456 : blk.flag[0U] = static_cast<uint8_t>(0xFF);
147 : 456 : blk.flag[1U] = static_cast<uint8_t>(0xEE);
148 : : // azimuth
149 : 456 : uint16_to_bytes(
150 : 456 : static_cast<uint16_t>(dth_tic * static_cast<float32_t>(idx)),
151 : 456 : blk.azimuth_bytes);
152 [ + + ]: 15048 : for (uint32_t jdx = 0U; jdx < NUM_POINTS_PER_BLOCK; ++jdx) {
153 : : DataChannel & data = blk.channels[jdx];
154 : 14592 : uint16_to_bytes(distances[jdx % RAY_SIZE], data.data);
155 : : // intensity
156 : 14592 : data.data[2U] = static_cast<uint8_t>(100U);
157 : : }
158 : : }
159 : 38 : }
160 : :
161 : 19 : void Vlp16IntegrationSpoofer::SpoofTask::initialize_packets(const float32_t rpm)
162 : : {
163 : 19 : const float32_t all_flat_ground_distances_m[RAY_SIZE] = {
164 : : 10.3082077832F,
165 : : 70.0047387525F,
166 : : 11.8790393175F,
167 : : 0.0F, // 70.0426680288F,
168 : : 14.0236477175F,
169 : : 0.0F, // 70.1186294229F,
170 : : 17.1245367095F,
171 : : 0.0F, // 70.232829299F,
172 : : 22.0013602226F,
173 : : 0.0F, // 70.3855789151F,
174 : : 30.7852227581F,
175 : : 0.0F, // horizon: > 100m
176 : : 51.290181248F,
177 : : 0.0F, // horizon: > 100m
178 : : 0.0F, // 70.0047387525F,
179 : : 0.0F // horizon: > 100m
180 : : };
181 : : uint16_t all_flat_ground_distance_tic[RAY_SIZE];
182 [ + + ]: 323 : for (uint32_t idx = 0U; idx < RAY_SIZE; ++idx) {
183 : 304 : all_flat_ground_distance_tic[idx] =
184 : 304 : static_cast<uint16_t>(all_flat_ground_distances_m[idx] * 500.0F);
185 : : }
186 : 19 : init_packet(m_all_flat_ground_packet, rpm, all_flat_ground_distance_tic);
187 : 19 : const float32_t flat_ground_wall_distances_m[RAY_SIZE] = {
188 : : 10.3082077832F,
189 : : 20.0013539293F,
190 : : 11.8790393175F,
191 : : 20.0121908654F,
192 : : 14.0236477175F,
193 : : 20.0338941208F,
194 : : 17.1245367095F,
195 : : 20.0665226568F,
196 : : 20.0665226568F,
197 : : 20.1101654043F,
198 : : 20.0338941208F,
199 : : 20.1649418573F,
200 : : 20.0121908654F,
201 : : 20.2310028762F,
202 : : 20.0013539293F,
203 : : 20.3085317098F
204 : : };
205 : : uint16_t flat_ground_wall_distance_tic[RAY_SIZE];
206 [ + + ]: 323 : for (uint32_t idx = 0U; idx < RAY_SIZE; ++idx) {
207 : 304 : flat_ground_wall_distance_tic[idx] =
208 : 304 : static_cast<uint16_t>(flat_ground_wall_distances_m[idx] * 500.0F);
209 : : }
210 : 19 : init_packet(m_flat_ground_wall_packet, rpm, flat_ground_wall_distance_tic);
211 : 19 : }
212 : : } // namespace lidar_integration
|
