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_client_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 : : // WriterListener
35 : 32 : DdsClientImpl::WriterListener::WriterListener(NodeImpl* impl) : DdsWriterListener(impl) {}
36 : :
37 : 26 : void DdsClientImpl::WriterListener::on_publication_matched(dds::DataWriter* writer,
38 : : const dds::PublicationMatchedStatus& status) {
39 : 26 : auto* instance = static_cast<DdsClientImpl*>(get_impl());
40 : 26 : auto* message_loop = instance->get_message_loop();
41 : :
42 : 26 : instance->write_session_count_.store(status.current_count, std::memory_order_release);
43 : :
44 : : // std::this_thread::sleep_for(std::chrono::microseconds(1)); //?
45 : :
46 [ + + ]: 26 : if (message_loop) {
47 [ + - + - ]: 1 : message_loop->post_task([instance]() {
48 [ - + ]: 1 : if VUNLIKELY (!instance->get_message_loop()) {
49 : 0 : return;
50 : : }
51 : :
52 : 1 : instance->update_connected();
53 : : });
54 : : } else {
55 : 25 : instance->update_connected();
56 : : }
57 : :
58 : 26 : DdsWriterListener::on_publication_matched(writer, status);
59 : 26 : }
60 : :
61 : : // ReaderListener
62 [ + - ]: 25 : DdsClientImpl::ReaderListener::ReaderListener(NodeImpl* impl) : DdsReaderListener(impl) {
63 : 25 : write_params_.sample_identity().sequence_number() = rtps::SequenceNumber_t(0, 0);
64 : 25 : }
65 : :
66 : 22 : void DdsClientImpl::ReaderListener::on_subscription_matched(dds::DataReader* reader,
67 : : const dds::SubscriptionMatchedStatus& status) {
68 : 22 : auto* instance = static_cast<DdsClientImpl*>(get_impl());
69 : 22 : auto* message_loop = instance->get_message_loop();
70 : :
71 : 22 : instance->read_session_count_.store(status.current_count, std::memory_order_release);
72 : :
73 : : // std::this_thread::sleep_for(std::chrono::microseconds(1)); //?
74 : :
75 [ + + ]: 22 : if (message_loop) {
76 [ + - + - ]: 1 : message_loop->post_task([instance]() {
77 [ - + ]: 1 : if VUNLIKELY (!instance->get_message_loop()) {
78 : 0 : return;
79 : : }
80 : :
81 : 1 : instance->update_connected();
82 : : });
83 : : } else {
84 : 21 : instance->update_connected();
85 : : }
86 : :
87 : 22 : DdsReaderListener::on_subscription_matched(reader, status);
88 : 22 : }
89 : :
90 : 26 : void DdsClientImpl::ReaderListener::on_data_available(dds::DataReader* reader) {
91 : 26 : auto* instance = static_cast<DdsClientImpl*>(get_impl());
92 : 26 : auto* message_loop = instance->get_message_loop();
93 : :
94 [ + + ]: 26 : if (message_loop) {
95 [ + - + - ]: 2 : message_loop->post_task([instance, reader]() {
96 [ - + ]: 2 : if VUNLIKELY (!instance->get_message_loop()) {
97 : 0 : return;
98 : : }
99 : :
100 : 2 : instance->process_message(reader);
101 : : });
102 : : } else {
103 : 24 : instance->process_message(reader);
104 : : }
105 : 26 : }
106 : :
107 : : // DdsClientImpl
108 [ + - + - ]: 36 : DdsClientImpl::DdsClientImpl(const DdsConf& conf) : conf_(conf) {}
109 : :
110 : 26 : void DdsClientImpl::process_message(dds::DataReader* reader) {
111 [ - + ]: 26 : if (is_cdr_type) {
112 : 0 : DdsFactory::ReadCdrMessage msg;
113 : :
114 [ # # # # ]: 0 : while (DdsFactory::take_cdr_data(reader, msg)) {
115 [ # # ]: 0 : if VUNLIKELY (quit_flag_.load(std::memory_order_acquire)) {
116 [ # # ]: 0 : DdsFactory::return_cdr_loan(reader, msg);
117 : 0 : break;
118 : : }
119 : :
120 [ # # ]: 0 : const auto& info = msg.infos[0];
121 : :
122 [ # # ]: 0 : if VUNLIKELY (!info.valid_data) {
123 [ # # ]: 0 : DdsFactory::return_cdr_loan(reader, msg);
124 : 0 : continue;
125 : : }
126 : :
127 : 0 : NodeImpl::MsgCallback cb;
128 : : {
129 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
130 [ # # ]: 0 : auto iter = cdr_callbacks_.find(info.related_sample_identity);
131 : :
132 [ # # ]: 0 : if VLIKELY (iter != cdr_callbacks_.end()) {
133 : 0 : cb = std::move(iter->second);
134 [ # # ]: 0 : cdr_callbacks_.erase(iter);
135 : : }
136 : 0 : }
137 : :
138 [ # # ]: 0 : if VLIKELY (cb) {
139 [ # # # # ]: 0 : cb(msg.samples[0]);
140 : : }
141 : :
142 [ # # ]: 0 : DdsFactory::return_cdr_loan(reader, msg);
143 : 0 : }
144 : 0 : } else {
145 : 26 : DdsFactory::ReadMessage msg;
146 : :
147 [ + - + + ]: 52 : while (DdsFactory::take_data(reader, msg)) {
148 [ - + ]: 26 : if VUNLIKELY (quit_flag_.load(std::memory_order_acquire)) {
149 [ # # ]: 0 : DdsFactory::return_data_loan(reader, msg);
150 : 0 : break;
151 : : }
152 : :
153 [ + - ]: 26 : const auto& info = msg.infos[0];
154 : :
155 [ - + ]: 26 : if VUNLIKELY (!info.valid_data) {
156 [ # # ]: 0 : DdsFactory::return_data_loan(reader, msg);
157 : 0 : continue;
158 : : }
159 : :
160 : 26 : NodeImpl::MsgCallback cb;
161 : : {
162 [ + - ]: 26 : std::lock_guard param_lock(param_mtx_);
163 [ + - ]: 26 : auto iter = callbacks_.find(msg.id);
164 : :
165 [ + + ]: 26 : if VLIKELY (iter != callbacks_.end()) {
166 : 23 : cb = std::move(iter->second);
167 [ + - ]: 23 : callbacks_.erase(iter);
168 : : }
169 : 26 : }
170 : :
171 [ + + ]: 26 : if VLIKELY (cb) {
172 [ + - + - : 23 : cb(msg.samples[0].data());
+ - ]
173 : : }
174 : :
175 [ + - ]: 26 : DdsFactory::return_data_loan(reader, msg);
176 : 26 : }
177 : 26 : }
178 : 26 : }
179 : :
180 : 32 : void DdsClientImpl::init() {
181 [ + + - + : 32 : if VUNLIKELY (is_resp_type && is_cdr_type != is_resp_cdr_type) {
- + ]
182 [ # # # # : 0 : VLOG_F("DdsClient: Request and response must both use raw or CDR serialization.");
# # ]
183 : : }
184 : :
185 [ - + - - : 32 : if VUNLIKELY (is_cdr_type && is_security_type) {
- + ]
186 [ # # # # : 0 : VLOG_F("Cdr type does not support security.");
# # ]
187 : : }
188 : :
189 [ + - ]: 32 : participant_ = DdsFactory::create_participant(kServer | kClient, conf_, get_all_properties());
190 : :
191 [ - + ]: 32 : if VUNLIKELY (!participant_) {
192 [ # # # # : 0 : VLOG_E("DdsClientImpl::init(): participant creation failed; client left uninitialised.");
# # ]
193 : :
194 : 0 : return;
195 : : }
196 : :
197 [ + + ]: 32 : if (is_resp_type) {
198 : 25 : std::tie(topic_req_, topic_resp_) =
199 : 50 : DdsFactory::create_method_topic(kServer | kClient, conf_, participant_.get(), is_cdr_type, ser_type);
200 : :
201 [ + - - + : 25 : if VUNLIKELY (!topic_req_ || !topic_resp_) {
- + ]
202 [ # # # # : 0 : VLOG_E("DdsClientImpl::init(): method topic creation failed; client left uninitialised.");
# # ]
203 : 0 : return;
204 : : }
205 : :
206 [ - + ]: 25 : if (is_cdr_type) {
207 [ # # ]: 0 : ser_type = topic_req_->get_type_name() + "|" + topic_resp_->get_type_name();
208 : : }
209 : :
210 : 25 : publisher_ = DdsFactory::create_publisher(kClient, conf_, participant_.get());
211 : :
212 [ + - ]: 25 : writer_listener_.emplace(this);
213 : :
214 : 50 : writer_ = DdsFactory::create_datawriter(kClient, conf_, publisher_.get(), topic_req_.get(),
215 : 50 : &writer_listener_.value(), is_cdr_type);
216 : :
217 : 25 : subscriber_ = DdsFactory::create_subscriber(kClient, conf_, participant_.get());
218 : :
219 [ + - ]: 25 : reader_listener_.emplace(this);
220 : :
221 : 50 : reader_ = DdsFactory::create_datareader(kClient, conf_, subscriber_.get(), topic_resp_.get(),
222 : 50 : &reader_listener_.value(), is_cdr_type);
223 : : } else {
224 [ + - ]: 7 : topic_req_ = DdsFactory::create_topic(kServer | kClient, conf_, participant_.get(), is_cdr_type, {}, ser_type);
225 : :
226 [ - + ]: 7 : if VUNLIKELY (!topic_req_) {
227 [ # # # # : 0 : VLOG_E("DdsClientImpl::init(): request topic creation failed; client left uninitialised.");
# # ]
228 : 0 : return;
229 : : }
230 : :
231 [ - + ]: 7 : if (is_cdr_type) {
232 : 0 : ser_type = topic_req_->get_type_name();
233 : : }
234 : :
235 : 7 : publisher_ = DdsFactory::create_publisher(kClient, conf_, participant_.get());
236 : :
237 [ + - ]: 7 : writer_listener_.emplace(this);
238 : :
239 : 14 : writer_ = DdsFactory::create_datawriter(kClient, conf_, publisher_.get(), topic_req_.get(),
240 : 14 : &writer_listener_.value(), is_cdr_type);
241 : : }
242 : :
243 : 32 : quit_flag_.store(false, std::memory_order_release);
244 : : }
245 : :
246 : 32 : void DdsClientImpl::deinit() {
247 : 32 : quit_flag_.store(true, std::memory_order_release);
248 : :
249 [ + - ]: 32 : detach();
250 : :
251 : 32 : reader_.reset();
252 : 32 : writer_.reset();
253 : 32 : reader_listener_.reset();
254 : 32 : writer_listener_.reset();
255 : 32 : subscriber_.reset();
256 : 32 : publisher_.reset();
257 : 32 : topic_resp_.reset();
258 : 32 : topic_req_.reset();
259 : 32 : participant_.reset();
260 [ + - ]: 32 : std::lock_guard lock(param_mtx_);
261 : 32 : callbacks_.clear();
262 : 32 : cdr_callbacks_.clear();
263 : 32 : write_session_count_.store(0, std::memory_order_release);
264 : 32 : read_session_count_.store(0, std::memory_order_release);
265 : 32 : }
266 : :
267 : 32 : void DdsClientImpl::interrupt() {
268 : 32 : ClientImpl::interrupt();
269 : 32 : ack_manager_.clear();
270 : 32 : }
271 : :
272 : 0 : const Conf* DdsClientImpl::get_conf() const { return &conf_; }
273 : :
274 : 4 : const AbstractNode* DdsClientImpl::get_abstract_node() const { return this; }
275 : :
276 : 7 : Status::BasePtr DdsClientImpl::get_status(Status::Type type) const {
277 [ + + ]: 7 : if (Status::is_for_writer(type)) {
278 [ + + ]: 3 : if VUNLIKELY (!writer_) {
279 : 1 : return std::make_shared<Status::Unknown>();
280 : : }
281 : :
282 : 2 : return WriterListener::get_status(writer_.get(), type);
283 : : }
284 : :
285 [ + + ]: 4 : if VUNLIKELY (!reader_) {
286 : 2 : return std::make_shared<Status::Unknown>();
287 : : }
288 : :
289 [ + - ]: 2 : if (reader_listener_) {
290 : 2 : return ReaderListener::get_status(reader_.get(), type);
291 : : }
292 : :
293 : 0 : return std::make_shared<Status::Unknown>();
294 : : }
295 : :
296 : 2 : std::any DdsClientImpl::get_native_handle() const { return publisher_; }
297 : :
298 : 100 : bool DdsClientImpl::is_connected() const {
299 [ + + ]: 100 : if (is_resp_type) {
300 [ + + ]: 260 : return write_session_count_.load(std::memory_order_acquire) > 0 &&
301 [ + + ]: 253 : read_session_count_.load(std::memory_order_acquire) > 0;
302 : : } else {
303 : 22 : return write_session_count_.load(std::memory_order_acquire) > 0;
304 : : }
305 : : }
306 : :
307 : 35 : bool DdsClientImpl::call(const Bytes& req_data, MsgCallback&& callback, std::chrono::milliseconds timeout) {
308 [ + - + + : 35 : if VUNLIKELY (!writer_ || (callback && !reader_)) {
- + - + -
+ ]
309 : 0 : return false;
310 : : }
311 : :
312 [ + + ]: 35 : if (!callback) {
313 [ - + ]: 6 : if (is_cdr_type) {
314 [ # # ]: 0 : return DdsFactory::write_cdr_data(writer_.get(), req_data);
315 : : } else {
316 [ + - ]: 6 : return DdsFactory::write_data(writer_.get(), req_data, 0);
317 : : }
318 : : }
319 : :
320 [ + - + - ]: 58 : uint64_t id = DdsFactory::get_guid(writer_->guid(), seq_.fetch_add(1, std::memory_order_relaxed) + 1);
321 : :
322 [ + - ]: 29 : rtps::WriteParams write_params;
323 : 29 : rtps::SampleIdentity sample_identity;
324 : :
325 [ - + ]: 29 : if (is_cdr_type) {
326 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
327 : 0 : write_params = reader_listener_->write_params_;
328 : :
329 : 0 : auto& write_identity = write_params.sample_identity();
330 [ # # ]: 0 : write_identity.writer_guid() = writer_->guid();
331 : 0 : ++write_identity.sequence_number().low;
332 : :
333 [ # # ]: 0 : if VUNLIKELY (write_identity.sequence_number().low == 0) {
334 : 0 : write_identity.sequence_number() = rtps::SequenceNumber_t(0, 1);
335 : : }
336 : :
337 : 0 : sample_identity = write_identity;
338 : 0 : reader_listener_->write_params_.sample_identity(sample_identity);
339 : 0 : }
340 : :
341 : 18 : auto cleanup_callback = [this, &id, &sample_identity]() {
342 [ + - ]: 6 : std::lock_guard param_lock(param_mtx_);
343 : :
344 [ - + ]: 6 : if (is_cdr_type) {
345 [ # # ]: 0 : cdr_callbacks_.erase(sample_identity);
346 : : } else {
347 [ + - ]: 6 : callbacks_.erase(id);
348 : : }
349 : 6 : };
350 : :
351 [ + + ]: 29 : if (timeout.count() != 0) {
352 : 23 : ack_manager_.reset_interrupted();
353 : :
354 : 23 : auto ack_request = ack_manager_.create_request();
355 : :
356 [ - + ]: 23 : if (is_cdr_type) {
357 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
358 [ # # ]: 0 : cdr_callbacks_[sample_identity] = [ack_request, callback = std::move(callback)](const Bytes& resp_data) {
359 [ # # ]: 0 : AckManager::notify(ack_request, [&callback, &resp_data]() { callback(resp_data); });
360 [ # # ]: 0 : };
361 : 0 : } else {
362 [ + - ]: 23 : std::lock_guard param_lock(param_mtx_);
363 [ + - ]: 46 : callbacks_[id] = [ack_request, callback = std::move(callback)](const Bytes& resp_data) {
364 [ + - ]: 34 : AckManager::notify(ack_request, [&callback, &resp_data]() { callback(resp_data); });
365 [ + - ]: 86 : };
366 : 23 : }
367 : :
368 : 23 : ElapsedTimer timer;
369 : 23 : timer.start();
370 : :
371 [ + - + + ]: 23 : if VUNLIKELY (!wait_for_connected(timeout)) {
372 [ + - ]: 2 : cleanup_callback();
373 : 2 : return false;
374 : : }
375 : :
376 : 21 : auto elapsed = timer.get();
377 : :
378 [ + - - + : 21 : if VUNLIKELY (timeout.count() > 0 && elapsed >= timeout.count()) {
- + ]
379 [ # # ]: 0 : cleanup_callback();
380 : 0 : return false;
381 : : }
382 : :
383 [ + - ]: 21 : bool result = ack_manager_.process(ack_request, timeout.count() - elapsed, [this, &req_data, &id, &write_params]() {
384 [ - + ]: 21 : if (is_cdr_type) {
385 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
386 [ # # ]: 0 : return DdsFactory::write_cdr_data(writer_.get(), req_data, &write_params);
387 : 0 : } else {
388 : 21 : return DdsFactory::write_data(writer_.get(), req_data, id);
389 : : }
390 : : });
391 : :
392 [ + + ]: 21 : if VUNLIKELY (!result) {
393 [ + - ]: 4 : cleanup_callback();
394 : : }
395 : :
396 : 21 : return result;
397 : 23 : }
398 : :
399 : 6 : bool result = false;
400 : :
401 [ - + ]: 6 : if (is_cdr_type) {
402 : : {
403 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
404 : :
405 [ # # ]: 0 : cdr_callbacks_[sample_identity] = [callback = std::move(callback)](const Bytes& resp_data) {
406 : 0 : callback(resp_data);
407 [ # # ]: 0 : };
408 : :
409 [ # # ]: 0 : result = DdsFactory::write_cdr_data(writer_.get(), req_data, &write_params);
410 : 0 : }
411 : : } else {
412 : : {
413 [ + - ]: 6 : std::lock_guard param_lock(param_mtx_);
414 [ + - + - ]: 12 : callbacks_[id] = [callback = std::move(callback)](const Bytes& resp_data) { callback(resp_data); };
415 : 6 : }
416 : :
417 [ + - ]: 6 : result = DdsFactory::write_data(writer_.get(), req_data, id);
418 : : }
419 : :
420 [ - + ]: 6 : if VUNLIKELY (!result) {
421 [ # # ]: 0 : cleanup_callback();
422 : : }
423 : :
424 : 6 : return result;
425 : : }
426 : :
427 : : } // namespace vlink
|
