Branch data Line data Source code
1 : : /*
2 : : * Copyright (C) 2026 by Thun Lu. All rights reserved.
3 : : * Author: Thun Lu <thun.lu@zohomail.cn>
4 : : * Repo: https://github.com/thun-res/vlink
5 : : * _ __ __ _ __
6 : : * | | / / / / (_) ____ / /__
7 : : * | | / / / / / / / __ \ / //_/
8 : : * | |/ / / /___ / / / / / / / ,<
9 : : * |___/ /_____/ /_/ /_/ /_/ /_/|_|
10 : : *
11 : : * Licensed under the Apache License, Version 2.0 (the "License");
12 : : * you may not use this file except in compliance with the License.
13 : : * You may obtain a copy of the License at
14 : : *
15 : : * http://www.apache.org/licenses/LICENSE-2.0
16 : : *
17 : : * Unless required by applicable law or agreed to in writing, software
18 : : * distributed under the License is distributed on an "AS IS" BASIS,
19 : : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
20 : : * See the License for the specific language governing permissions and
21 : : * limitations under the License.
22 : : */
23 : :
24 : : #include "./dds_getter_impl.h"
25 : :
26 : : #include <memory>
27 : : #include <utility>
28 : :
29 : : #include "./base/elapsed_timer.h"
30 : : #include "./base/message_loop.h"
31 : :
32 : : namespace vlink {
33 : :
34 : : // ReaderListener
35 : 23 : DdsGetterImpl::ReaderListener::ReaderListener(NodeImpl* impl) : DdsReaderListener(impl) {}
36 : :
37 : 28 : void DdsGetterImpl::ReaderListener::on_data_available(dds::DataReader* reader) {
38 : 28 : auto* instance = static_cast<DdsGetterImpl*>(get_impl());
39 : 28 : auto* message_loop = instance->get_message_loop();
40 : :
41 [ + + ]: 28 : if VUNLIKELY (instance->has_suspend.load(std::memory_order_acquire)) {
42 [ - + ]: 1 : if (instance->is_cdr_type) {
43 : 0 : DdsFactory::ReadCdrMessage msg;
44 : :
45 [ # # # # ]: 0 : while (DdsFactory::take_cdr_data(reader, msg)) {
46 : 0 : const bool should_quit = instance->quit_flag_.load(std::memory_order_acquire);
47 [ # # ]: 0 : DdsFactory::return_cdr_loan(reader, msg);
48 : :
49 [ # # ]: 0 : if VUNLIKELY (should_quit) {
50 : 0 : break;
51 : : }
52 : : }
53 : 0 : } else {
54 : 1 : DdsFactory::ReadMessage msg;
55 : :
56 [ + - + + ]: 2 : while (DdsFactory::take_data(reader, msg)) {
57 [ + - ]: 1 : DdsFactory::return_data_loan(reader, msg);
58 : :
59 [ - + ]: 1 : if VUNLIKELY (instance->quit_flag_.load(std::memory_order_acquire)) {
60 : 0 : break;
61 : : }
62 : : }
63 : 1 : }
64 : :
65 : 1 : return;
66 : : }
67 : :
68 [ - + ]: 27 : if VUNLIKELY (!instance->callback_) {
69 : 0 : return;
70 : : }
71 : :
72 [ + + ]: 27 : if (message_loop) {
73 [ + - + - ]: 1 : message_loop->post_task([instance, reader]() {
74 [ - + ]: 1 : if VUNLIKELY (!instance->get_message_loop()) {
75 : 0 : return;
76 : : }
77 : :
78 : 1 : instance->process_message(reader);
79 : : });
80 : : } else {
81 : 26 : instance->process_message(reader);
82 : : }
83 : : }
84 : :
85 : : // DdsGetterImpl
86 [ + - + - ]: 27 : DdsGetterImpl::DdsGetterImpl(const DdsConf& conf) : conf_(conf) {}
87 : :
88 : 27 : void DdsGetterImpl::process_message(dds::DataReader* reader) {
89 [ - + ]: 27 : if (is_cdr_type) {
90 : 0 : DdsFactory::ReadCdrMessage msg;
91 : :
92 [ # # # # ]: 0 : while (DdsFactory::take_cdr_data(reader, msg)) {
93 [ # # ]: 0 : if VUNLIKELY (quit_flag_.load(std::memory_order_acquire)) {
94 [ # # ]: 0 : DdsFactory::return_cdr_loan(reader, msg);
95 : 0 : break;
96 : : }
97 : :
98 [ # # ]: 0 : const auto& info = msg.infos[0];
99 : :
100 [ # # ]: 0 : if VUNLIKELY (!info.valid_data) {
101 [ # # ]: 0 : DdsFactory::return_cdr_loan(reader, msg);
102 : 0 : continue;
103 : : }
104 : :
105 [ # # ]: 0 : if VUNLIKELY (is_latency_and_lost_enabled_.load(std::memory_order_acquire)) {
106 : 0 : last_latency_.store(ElapsedTimer::get_sys_timestamp(ElapsedTimer::kNano, false) - msg.timestamp,
107 : : std::memory_order_relaxed);
108 : :
109 : 0 : uint64_t part1 = 0;
110 : 0 : uint64_t part2 = 0;
111 : :
112 : 0 : std::memcpy(&part1, info.publication_handle.value, sizeof(uint64_t));
113 : 0 : std::memcpy(&part2, info.publication_handle.value + 8, sizeof(uint64_t));
114 : :
115 : 0 : calc_sample_.update(msg.id, part1 ^ part2);
116 : : }
117 : :
118 [ # # # # ]: 0 : callback_(msg.samples[0]);
119 [ # # ]: 0 : DdsFactory::return_cdr_loan(reader, msg);
120 : : }
121 : 0 : } else {
122 : 27 : DdsFactory::ReadMessage msg;
123 : :
124 [ + - + + ]: 54 : while (DdsFactory::take_data(reader, msg)) {
125 [ - + ]: 27 : if VUNLIKELY (quit_flag_.load(std::memory_order_acquire)) {
126 [ # # ]: 0 : DdsFactory::return_data_loan(reader, msg);
127 : 0 : break;
128 : : }
129 : :
130 [ + - ]: 27 : const auto& info = msg.infos[0];
131 : :
132 [ - + ]: 27 : if VUNLIKELY (!info.valid_data) {
133 [ # # ]: 0 : DdsFactory::return_data_loan(reader, msg);
134 : 0 : continue;
135 : : }
136 : :
137 [ + + ]: 27 : if VUNLIKELY (is_latency_and_lost_enabled_.load(std::memory_order_acquire)) {
138 : 5 : last_latency_.store(ElapsedTimer::get_sys_timestamp(ElapsedTimer::kNano, false) - msg.timestamp,
139 : : std::memory_order_relaxed);
140 : :
141 : 5 : uint64_t part1 = 0;
142 : 5 : uint64_t part2 = 0;
143 : :
144 : 5 : std::memcpy(&part1, info.publication_handle.value, sizeof(uint64_t));
145 : 5 : std::memcpy(&part2, info.publication_handle.value + 8, sizeof(uint64_t));
146 : :
147 : 5 : calc_sample_.update(msg.id, part1 ^ part2);
148 : : }
149 : :
150 [ + - + - : 27 : callback_(msg.samples[0].data());
+ - ]
151 [ + - ]: 27 : DdsFactory::return_data_loan(reader, msg);
152 : : }
153 : 27 : }
154 : 27 : }
155 : :
156 : 23 : void DdsGetterImpl::init() {
157 [ - + - - : 23 : if VUNLIKELY (is_cdr_type && is_security_type) {
- + ]
158 [ # # # # ]: 0 : VLOG_F("Cdr type does not support security.");
159 : : }
160 : :
161 [ + - ]: 23 : participant_ = DdsFactory::create_participant(kPublisher | kSubscriber, conf_, get_all_properties());
162 : :
163 [ + - ]: 23 : topic_ = DdsFactory::create_topic(kPublisher | kSubscriber, conf_, participant_.get(), is_cdr_type, {}, ser_type);
164 : :
165 : 23 : subscriber_ = DdsFactory::create_subscriber(kSubscriber, conf_, participant_.get());
166 : :
167 [ + - - + : 23 : if VUNLIKELY (!participant_ || !topic_) {
- + ]
168 [ # # # # ]: 0 : VLOG_E("DdsGetterImpl::init(): participant/topic creation failed; getter left uninitialised.");
169 : :
170 : 0 : return;
171 : : }
172 : :
173 [ - + ]: 23 : if (is_cdr_type) {
174 : 0 : ser_type = topic_->get_type_name();
175 : : }
176 : :
177 : 23 : quit_flag_.store(false, std::memory_order_release);
178 : : }
179 : :
180 : 23 : void DdsGetterImpl::deinit() {
181 : 23 : quit_flag_.store(true, std::memory_order_release);
182 : :
183 : 23 : detach();
184 : :
185 : 23 : reader_.reset();
186 : 23 : listener_.reset();
187 : 23 : subscriber_.reset();
188 : 23 : topic_.reset();
189 : 23 : participant_.reset();
190 : 23 : callback_ = {};
191 : 23 : }
192 : :
193 : 2 : bool DdsGetterImpl::suspend() {
194 : 2 : has_suspend.store(true, std::memory_order_release);
195 : :
196 : 2 : return true;
197 : : }
198 : :
199 : 2 : bool DdsGetterImpl::resume() {
200 : 2 : has_suspend.store(false, std::memory_order_release);
201 : :
202 : 2 : return true;
203 : : }
204 : :
205 : 3 : bool DdsGetterImpl::is_suspend() const { return has_suspend.load(std::memory_order_acquire); }
206 : :
207 : 0 : const Conf* DdsGetterImpl::get_conf() const { return &conf_; }
208 : :
209 : 2 : const AbstractNode* DdsGetterImpl::get_abstract_node() const { return this; }
210 : :
211 : 4 : Status::BasePtr DdsGetterImpl::get_status(Status::Type type) const {
212 [ + + ]: 4 : if VUNLIKELY (!reader_) {
213 : 1 : return std::make_shared<Status::Unknown>();
214 : : }
215 : :
216 : 3 : return ReaderListener::get_status(reader_.get(), type);
217 : : }
218 : :
219 : 1 : std::any DdsGetterImpl::get_native_handle() const { return subscriber_; }
220 : :
221 : 23 : bool DdsGetterImpl::listen(MsgCallback&& callback) {
222 [ - + ]: 23 : if VUNLIKELY (callback_) {
223 : 0 : return false;
224 : : }
225 : :
226 : 23 : callback_ = std::move(callback);
227 : :
228 [ + - ]: 23 : listener_.emplace(this);
229 : :
230 : : reader_ =
231 : 23 : DdsFactory::create_datareader(kGetter, conf_, subscriber_.get(), topic_.get(), &listener_.value(), is_cdr_type);
232 : :
233 : 23 : return true;
234 : : }
235 : :
236 : 4 : void DdsGetterImpl::set_latency_and_lost_enabled(bool enable) {
237 : 4 : is_latency_and_lost_enabled_.store(enable, std::memory_order_release);
238 : 4 : }
239 : :
240 : 5 : bool DdsGetterImpl::is_latency_and_lost_enabled() const {
241 : 5 : return is_latency_and_lost_enabled_.load(std::memory_order_acquire);
242 : : }
243 : :
244 : 3 : int64_t DdsGetterImpl::get_latency() const {
245 [ + + ]: 3 : if (!is_latency_and_lost_enabled_.load(std::memory_order_acquire)) {
246 : 1 : return 0;
247 : : }
248 : :
249 : 4 : return last_latency_.load(std::memory_order_relaxed);
250 : : }
251 : :
252 : 2 : SampleLostInfo DdsGetterImpl::get_lost() const {
253 [ + + ]: 2 : if (!is_latency_and_lost_enabled_.load(std::memory_order_acquire)) {
254 : 1 : return SampleLostInfo();
255 : : }
256 : :
257 : 1 : return SampleLostInfo{calc_sample_.get_total(), calc_sample_.get_lost()};
258 : : }
259 : :
260 : : } // namespace vlink
|
