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 : 31 : DdsClientImpl::WriterListener::WriterListener(NodeImpl* impl) : DdsWriterListener(impl) {}
36 : :
37 : 25 : void DdsClientImpl::WriterListener::on_publication_matched(dds::DataWriter* writer,
38 : : const dds::PublicationMatchedStatus& status) {
39 : 25 : auto* instance = static_cast<DdsClientImpl*>(get_impl());
40 : 25 : auto* message_loop = instance->get_message_loop();
41 : :
42 : 25 : 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 [ + + ]: 25 : 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 : 24 : instance->update_connected();
56 : : }
57 : :
58 : 25 : DdsWriterListener::on_publication_matched(writer, status);
59 : 25 : }
60 : :
61 : : // ReaderListener
62 [ + - ]: 24 : DdsClientImpl::ReaderListener::ReaderListener(NodeImpl* impl) : DdsReaderListener(impl) {
63 : 24 : write_params_.sample_identity().sequence_number() = rtps::SequenceNumber_t(0, 0);
64 : 24 : }
65 : :
66 : 21 : void DdsClientImpl::ReaderListener::on_subscription_matched(dds::DataReader* reader,
67 : : const dds::SubscriptionMatchedStatus& status) {
68 : 21 : auto* instance = static_cast<DdsClientImpl*>(get_impl());
69 : 21 : auto* message_loop = instance->get_message_loop();
70 : :
71 : 21 : 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 [ + + ]: 21 : 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 : 20 : instance->update_connected();
85 : : }
86 : :
87 : 21 : DdsReaderListener::on_subscription_matched(reader, status);
88 : 21 : }
89 : :
90 : 24 : void DdsClientImpl::ReaderListener::on_data_available(dds::DataReader* reader) {
91 : 24 : auto* instance = static_cast<DdsClientImpl*>(get_impl());
92 : 24 : auto* message_loop = instance->get_message_loop();
93 : :
94 [ + + ]: 24 : 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 : 22 : instance->process_message(reader);
104 : : }
105 : 24 : }
106 : :
107 : : // DdsClientImpl
108 [ + - + - ]: 35 : DdsClientImpl::DdsClientImpl(const DdsConf& conf) : conf_(conf) {}
109 : :
110 : 24 : void DdsClientImpl::process_message(dds::DataReader* reader) {
111 [ - + ]: 24 : 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 : 24 : DdsFactory::ReadMessage msg;
146 : :
147 [ + - + + ]: 48 : while (DdsFactory::take_data(reader, msg)) {
148 [ - + ]: 24 : if VUNLIKELY (quit_flag_.load(std::memory_order_acquire)) {
149 [ # # ]: 0 : DdsFactory::return_data_loan(reader, msg);
150 : 0 : break;
151 : : }
152 : :
153 [ + - ]: 24 : const auto& info = msg.infos[0];
154 : :
155 [ - + ]: 24 : if VUNLIKELY (!info.valid_data) {
156 [ # # ]: 0 : DdsFactory::return_data_loan(reader, msg);
157 : 0 : continue;
158 : : }
159 : :
160 : 24 : NodeImpl::MsgCallback cb;
161 : : {
162 [ + - ]: 24 : std::lock_guard param_lock(param_mtx_);
163 [ + - ]: 24 : auto iter = callbacks_.find(msg.id);
164 : :
165 [ + + ]: 24 : if VLIKELY (iter != callbacks_.end()) {
166 : 21 : cb = std::move(iter->second);
167 [ + - ]: 21 : callbacks_.erase(iter);
168 : : }
169 : 24 : }
170 : :
171 [ + + ]: 24 : if VLIKELY (cb) {
172 [ + - + - : 21 : cb(msg.samples[0].data());
+ - ]
173 : : }
174 : :
175 [ + - ]: 24 : DdsFactory::return_data_loan(reader, msg);
176 : 24 : }
177 : 24 : }
178 : 24 : }
179 : :
180 : 31 : void DdsClientImpl::init() {
181 [ + + - + : 31 : 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 [ - + - - : 31 : if VUNLIKELY (is_cdr_type && is_security_type) {
- + ]
186 [ # # # # ]: 0 : VLOG_F("Cdr type does not support security.");
187 : : }
188 : :
189 [ + - ]: 31 : participant_ = DdsFactory::create_participant(kServer | kClient, conf_, get_all_properties());
190 : :
191 [ - + ]: 31 : if VUNLIKELY (!participant_) {
192 [ # # # # ]: 0 : VLOG_E("DdsClientImpl::init(): participant creation failed; client left uninitialised.");
193 : :
194 : 0 : return;
195 : : }
196 : :
197 [ + + ]: 31 : if (is_resp_type) {
198 : 24 : std::tie(topic_req_, topic_resp_) =
199 : 48 : DdsFactory::create_method_topic(kServer | kClient, conf_, participant_.get(), is_cdr_type, ser_type);
200 : :
201 [ + - - + : 24 : 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 [ - + ]: 24 : if (is_cdr_type) {
207 [ # # ]: 0 : ser_type = topic_req_->get_type_name() + "|" + topic_resp_->get_type_name();
208 : : }
209 : :
210 : 24 : publisher_ = DdsFactory::create_publisher(kClient, conf_, participant_.get());
211 : :
212 [ + - ]: 24 : writer_listener_.emplace(this);
213 : :
214 : 48 : writer_ = DdsFactory::create_datawriter(kClient, conf_, publisher_.get(), topic_req_.get(),
215 : 48 : &writer_listener_.value(), is_cdr_type);
216 : :
217 : 24 : subscriber_ = DdsFactory::create_subscriber(kClient, conf_, participant_.get());
218 : :
219 [ + - ]: 24 : reader_listener_.emplace(this);
220 : :
221 : 48 : reader_ = DdsFactory::create_datareader(kClient, conf_, subscriber_.get(), topic_resp_.get(),
222 : 48 : &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 : 31 : quit_flag_.store(false, std::memory_order_release);
244 : : }
245 : :
246 : 31 : void DdsClientImpl::deinit() {
247 : 31 : quit_flag_.store(true, std::memory_order_release);
248 : :
249 [ + - ]: 31 : detach();
250 : :
251 : 31 : reader_.reset();
252 : 31 : writer_.reset();
253 : 31 : reader_listener_.reset();
254 : 31 : writer_listener_.reset();
255 : 31 : subscriber_.reset();
256 : 31 : publisher_.reset();
257 : 31 : topic_resp_.reset();
258 : 31 : topic_req_.reset();
259 : 31 : participant_.reset();
260 [ + - ]: 31 : std::lock_guard lock(param_mtx_);
261 : 31 : callbacks_.clear();
262 : 31 : cdr_callbacks_.clear();
263 : 31 : write_session_count_.store(0, std::memory_order_release);
264 : 31 : read_session_count_.store(0, std::memory_order_release);
265 : 31 : }
266 : :
267 : 31 : void DdsClientImpl::interrupt() {
268 : 31 : ClientImpl::interrupt();
269 : 31 : ack_manager_.clear();
270 : 31 : }
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 : 95 : bool DdsClientImpl::is_connected() const {
299 [ + + ]: 95 : if (is_resp_type) {
300 [ + + ]: 245 : return write_session_count_.load(std::memory_order_acquire) > 0 &&
301 [ + + ]: 238 : 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 : 33 : bool DdsClientImpl::call(const Bytes& req_data, MsgCallback&& callback, std::chrono::milliseconds timeout) {
308 [ + + ]: 33 : if (!callback) {
309 [ - + ]: 6 : if (is_cdr_type) {
310 [ # # ]: 0 : return DdsFactory::write_cdr_data(writer_.get(), req_data);
311 : : } else {
312 [ + - ]: 6 : return DdsFactory::write_data(writer_.get(), req_data, 0);
313 : : }
314 : : }
315 : :
316 [ + - + - ]: 54 : uint64_t id = DdsFactory::get_guid(writer_->guid(), seq_.fetch_add(1, std::memory_order_relaxed) + 1);
317 : :
318 [ + - ]: 27 : rtps::WriteParams write_params;
319 : 27 : rtps::SampleIdentity sample_identity;
320 : :
321 [ - + ]: 27 : if (is_cdr_type) {
322 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
323 : 0 : write_params = reader_listener_->write_params_;
324 : :
325 : 0 : auto& write_identity = write_params.sample_identity();
326 [ # # ]: 0 : write_identity.writer_guid() = writer_->guid();
327 : 0 : ++write_identity.sequence_number().low;
328 : :
329 [ # # ]: 0 : if VUNLIKELY (write_identity.sequence_number().low == 0) {
330 : 0 : write_identity.sequence_number() = rtps::SequenceNumber_t(0, 1);
331 : : }
332 : :
333 : 0 : sample_identity = write_identity;
334 : 0 : reader_listener_->write_params_.sample_identity(sample_identity);
335 : 0 : }
336 : :
337 : 18 : auto cleanup_callback = [this, &id, &sample_identity]() {
338 [ + - ]: 6 : std::lock_guard param_lock(param_mtx_);
339 : :
340 [ - + ]: 6 : if (is_cdr_type) {
341 [ # # ]: 0 : cdr_callbacks_.erase(sample_identity);
342 : : } else {
343 [ + - ]: 6 : callbacks_.erase(id);
344 : : }
345 : 6 : };
346 : :
347 [ + + ]: 27 : if (timeout.count() != 0) {
348 : 21 : ack_manager_.reset_interrupted();
349 : :
350 : 21 : auto ack_request = ack_manager_.create_request();
351 : :
352 [ - + ]: 21 : if (is_cdr_type) {
353 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
354 [ # # ]: 0 : cdr_callbacks_[sample_identity] = [this, ack_request, callback = std::move(callback)](const Bytes& resp_data) {
355 [ # # ]: 0 : ack_manager_.notify(ack_request, [&callback, &resp_data]() { callback(resp_data); });
356 [ # # ]: 0 : };
357 : 0 : } else {
358 [ + - ]: 21 : std::lock_guard param_lock(param_mtx_);
359 [ + - ]: 42 : callbacks_[id] = [this, ack_request, callback = std::move(callback)](const Bytes& resp_data) {
360 [ + - ]: 30 : ack_manager_.notify(ack_request, [&callback, &resp_data]() { callback(resp_data); });
361 [ + - ]: 78 : };
362 : 21 : }
363 : :
364 : 21 : ElapsedTimer timer;
365 : 21 : timer.start();
366 : :
367 [ + - + + ]: 21 : if VUNLIKELY (!wait_for_connected(timeout)) {
368 [ + - ]: 2 : cleanup_callback();
369 : 2 : return false;
370 : : }
371 : :
372 : 19 : auto elapsed = timer.get();
373 : :
374 [ + - - + : 19 : if VUNLIKELY (timeout.count() > 0 && elapsed >= timeout.count()) {
- + ]
375 [ # # ]: 0 : cleanup_callback();
376 : 0 : return false;
377 : : }
378 : :
379 [ + - ]: 19 : bool result = ack_manager_.process(ack_request, timeout.count() - elapsed, [this, &req_data, &id, &write_params]() {
380 [ - + ]: 19 : if (is_cdr_type) {
381 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
382 [ # # ]: 0 : return DdsFactory::write_cdr_data(writer_.get(), req_data, &write_params);
383 : 0 : } else {
384 : 19 : return DdsFactory::write_data(writer_.get(), req_data, id);
385 : : }
386 : : });
387 : :
388 [ + + ]: 19 : if VUNLIKELY (!result) {
389 [ + - ]: 4 : cleanup_callback();
390 : : }
391 : :
392 : 19 : return result;
393 : 21 : }
394 : :
395 : 6 : bool result = false;
396 : :
397 [ - + ]: 6 : if (is_cdr_type) {
398 : : {
399 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
400 : :
401 [ # # ]: 0 : cdr_callbacks_[sample_identity] = [callback = std::move(callback)](const Bytes& resp_data) {
402 : 0 : callback(resp_data);
403 [ # # ]: 0 : };
404 : :
405 [ # # ]: 0 : result = DdsFactory::write_cdr_data(writer_.get(), req_data, &write_params);
406 : 0 : }
407 : : } else {
408 : : {
409 [ + - ]: 6 : std::lock_guard param_lock(param_mtx_);
410 [ + - + - ]: 12 : callbacks_[id] = [callback = std::move(callback)](const Bytes& resp_data) { callback(resp_data); };
411 : 6 : }
412 : :
413 [ + - ]: 6 : result = DdsFactory::write_data(writer_.get(), req_data, id);
414 : : }
415 : :
416 [ - + ]: 6 : if VUNLIKELY (!result) {
417 [ # # ]: 0 : cleanup_callback();
418 : : }
419 : :
420 : 6 : return result;
421 : : }
422 : :
423 : : } // namespace vlink
|
