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 : 23 : void DdsClientImpl::ReaderListener::on_subscription_matched(dds::DataReader* reader,
67 : : const dds::SubscriptionMatchedStatus& status) {
68 : 23 : auto* instance = static_cast<DdsClientImpl*>(get_impl());
69 : 23 : auto* message_loop = instance->get_message_loop();
70 : :
71 : 23 : 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 [ + + ]: 23 : 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 : 22 : instance->update_connected();
85 : : }
86 : :
87 : 23 : DdsReaderListener::on_subscription_matched(reader, status);
88 : 23 : }
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 : if VUNLIKELY (!type_support_resp_) {
113 : 0 : return;
114 : : }
115 : :
116 : 0 : DdsFactory::ReadCdrMessage msg;
117 : :
118 [ # # ]: 0 : msg.sample = type_support_resp_.create_data();
119 : :
120 [ # # # # ]: 0 : while (DdsFactory::take_cdr_data(reader, msg)) {
121 [ # # ]: 0 : if VUNLIKELY (quit_flag_.load(std::memory_order_acquire)) {
122 : 0 : break;
123 : : }
124 : :
125 [ # # ]: 0 : if VUNLIKELY (!msg.info.valid_data) {
126 : 0 : continue;
127 : : }
128 : :
129 : 0 : NodeImpl::MsgCallback cb;
130 : : {
131 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
132 [ # # ]: 0 : auto iter = cdr_callbacks_.find(msg.info.related_sample_identity);
133 : :
134 [ # # ]: 0 : if VUNLIKELY (iter == cdr_callbacks_.end()) {
135 : 0 : continue;
136 : : }
137 : :
138 : 0 : cb = std::move(iter->second);
139 [ # # ]: 0 : cdr_callbacks_.erase(iter);
140 [ # # ]: 0 : }
141 : :
142 [ # # ]: 0 : if VLIKELY (cb) {
143 [ # # ]: 0 : cb(msg.bytes);
144 : : }
145 [ # # ]: 0 : }
146 : :
147 [ # # ]: 0 : type_support_resp_.delete_data(msg.sample);
148 : 0 : } else {
149 [ + - ]: 24 : DdsFactory::ReadMessage msg;
150 : :
151 [ + - + + ]: 48 : while (DdsFactory::take_data(reader, msg)) {
152 [ - + ]: 24 : if VUNLIKELY (quit_flag_.load(std::memory_order_acquire)) {
153 : 0 : break;
154 : : }
155 : :
156 [ - + ]: 24 : if VUNLIKELY (!msg.info.valid_data) {
157 : 3 : 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 VUNLIKELY (iter == callbacks_.end()) {
166 : 3 : continue;
167 : : }
168 : :
169 : 21 : cb = std::move(iter->second);
170 [ + - ]: 21 : callbacks_.erase(iter);
171 [ + + ]: 24 : }
172 : :
173 [ + - ]: 21 : if VLIKELY (cb) {
174 [ + - ]: 21 : cb(msg.bytes);
175 : : }
176 [ + + ]: 24 : }
177 : 24 : }
178 : : }
179 : :
180 : 31 : void DdsClientImpl::init() {
181 [ - + - - : 31 : if VUNLIKELY (is_cdr_type && is_security_type) {
- + ]
182 [ # # # # ]: 0 : VLOG_F("Cdr type does not support security.");
183 : : }
184 : :
185 [ + - ]: 31 : participant_ = DdsFactory::create_participant(kServer | kClient, conf_, get_all_properties());
186 : :
187 [ - + ]: 31 : if VUNLIKELY (!participant_) {
188 [ # # # # ]: 0 : VLOG_E("DdsClientImpl::init(): participant creation failed; client left uninitialised.");
189 : :
190 : 0 : return;
191 : : }
192 : :
193 [ + + ]: 31 : if (is_resp_type) {
194 : 24 : std::tie(topic_req_, topic_resp_) =
195 : 48 : DdsFactory::create_method_topic(kServer | kClient, conf_, participant_.get(), is_cdr_type);
196 : :
197 : 24 : publisher_ = DdsFactory::create_publisher(kClient, conf_, participant_.get());
198 : :
199 [ + - ]: 24 : writer_listener_.emplace(this);
200 : :
201 : : writer_ =
202 : 24 : DdsFactory::create_datawriter(kClient, conf_, publisher_.get(), topic_req_.get(), &writer_listener_.value());
203 : :
204 : 24 : subscriber_ = DdsFactory::create_subscriber(kClient, conf_, participant_.get());
205 : :
206 [ + - ]: 24 : reader_listener_.emplace(this);
207 : :
208 : : reader_ =
209 : 24 : DdsFactory::create_datareader(kClient, conf_, subscriber_.get(), topic_resp_.get(), &reader_listener_.value());
210 : :
211 [ - + ]: 24 : if VUNLIKELY (!topic_resp_) {
212 [ # # # # ]: 0 : VLOG_E("DdsClientImpl::init(): response topic creation failed; client left uninitialised.");
213 : :
214 : 0 : return;
215 : : }
216 : :
217 : 24 : type_support_resp_ = participant_->find_type(topic_resp_->get_type_name());
218 : :
219 [ - + ]: 24 : if VUNLIKELY (!type_support_resp_) {
220 [ # # # # ]: 0 : CLOG_F("Failed to find typesupport (%s).", topic_resp_->get_type_name().c_str());
221 : : }
222 : : } else {
223 [ + - + - ]: 7 : topic_req_ = DdsFactory::create_topic(kServer | kClient, conf_, participant_.get(), is_cdr_type);
224 : :
225 : 7 : publisher_ = DdsFactory::create_publisher(kClient, conf_, participant_.get());
226 : :
227 [ + - ]: 7 : writer_listener_.emplace(this);
228 : :
229 : : writer_ =
230 : 7 : DdsFactory::create_datawriter(kClient, conf_, publisher_.get(), topic_req_.get(), &writer_listener_.value());
231 : : }
232 : :
233 : 31 : quit_flag_.store(false, std::memory_order_release);
234 : : }
235 : :
236 : 31 : void DdsClientImpl::deinit() {
237 : 31 : quit_flag_.store(true, std::memory_order_release);
238 : :
239 : 31 : detach();
240 : :
241 : 31 : reader_.reset();
242 : 31 : writer_.reset();
243 : 31 : reader_listener_.reset();
244 : 31 : writer_listener_.reset();
245 : 31 : subscriber_.reset();
246 : 31 : publisher_.reset();
247 : 31 : topic_resp_.reset();
248 : 31 : topic_req_.reset();
249 : 31 : participant_.reset();
250 : 31 : type_support_resp_.reset();
251 : 31 : }
252 : :
253 : 31 : void DdsClientImpl::interrupt() {
254 : 31 : ClientImpl::interrupt();
255 : 31 : ack_manager_.clear();
256 : 31 : }
257 : :
258 : 0 : const Conf* DdsClientImpl::get_conf() const { return &conf_; }
259 : :
260 : 4 : const AbstractNode* DdsClientImpl::get_abstract_node() const { return this; }
261 : :
262 : 7 : Status::BasePtr DdsClientImpl::get_status(Status::Type type) const {
263 [ + + ]: 7 : if (Status::is_for_writer(type)) {
264 [ + + ]: 3 : if VUNLIKELY (!writer_) {
265 : 1 : return std::make_shared<Status::Unknown>();
266 : : }
267 : :
268 : 2 : return WriterListener::get_status(writer_.get(), type);
269 : : }
270 : :
271 [ + + ]: 4 : if VUNLIKELY (!reader_) {
272 : 2 : return std::make_shared<Status::Unknown>();
273 : : }
274 : :
275 [ + - ]: 2 : if (reader_listener_) {
276 : 2 : return ReaderListener::get_status(reader_.get(), type);
277 : : }
278 : :
279 : 0 : return std::make_shared<Status::Unknown>();
280 : : }
281 : :
282 : 2 : std::any DdsClientImpl::get_native_handle() const { return publisher_; }
283 : :
284 : 105 : bool DdsClientImpl::is_connected() const {
285 [ + + ]: 105 : if (is_resp_type) {
286 [ + + ]: 261 : return write_session_count_.load(std::memory_order_acquire) > 0 &&
287 [ + + ]: 246 : read_session_count_.load(std::memory_order_acquire) > 0;
288 : : } else {
289 : 26 : return write_session_count_.load(std::memory_order_acquire) > 0;
290 : : }
291 : : }
292 : :
293 : 33 : bool DdsClientImpl::call(const Bytes& req_data, MsgCallback&& callback, std::chrono::milliseconds timeout) {
294 [ + + ]: 33 : if (!callback) {
295 [ - + ]: 6 : if (is_cdr_type) {
296 [ # # ]: 0 : return DdsFactory::write_cdr_data(writer_.get(), req_data);
297 : : } else {
298 [ + - ]: 6 : return DdsFactory::write_data(writer_.get(), req_data, 0);
299 : : }
300 : : }
301 : :
302 [ + - + - ]: 54 : uint64_t id = DdsFactory::get_guid(writer_->guid(), seq_.fetch_add(1, std::memory_order_relaxed) + 1);
303 : :
304 [ + - ]: 27 : rtps::WriteParams write_params;
305 : 27 : rtps::SampleIdentity sample_identity;
306 : :
307 [ - + ]: 27 : if (is_cdr_type) {
308 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
309 : 0 : write_params = reader_listener_->write_params_;
310 : :
311 : 0 : auto& write_identity = write_params.sample_identity();
312 [ # # ]: 0 : write_identity.writer_guid() = writer_->guid();
313 : 0 : ++write_identity.sequence_number().low;
314 : :
315 [ # # ]: 0 : if VUNLIKELY (write_identity.sequence_number().low == 0) {
316 : 0 : write_identity.sequence_number() = rtps::SequenceNumber_t(0, 1);
317 : : }
318 : :
319 : 0 : sample_identity = write_identity;
320 : 0 : reader_listener_->write_params_.sample_identity(sample_identity);
321 : 0 : }
322 : :
323 : 18 : auto cleanup_callback = [this, &id, &sample_identity]() {
324 [ + - ]: 6 : std::lock_guard param_lock(param_mtx_);
325 : :
326 [ - + ]: 6 : if (is_cdr_type) {
327 [ # # ]: 0 : cdr_callbacks_.erase(sample_identity);
328 : : } else {
329 [ + - ]: 6 : callbacks_.erase(id);
330 : : }
331 : 6 : };
332 : :
333 [ + + ]: 27 : if (timeout.count() != 0) {
334 : 21 : ack_manager_.reset_interrupted();
335 : :
336 : 21 : auto ack_request = ack_manager_.create_request();
337 : :
338 [ - + ]: 21 : if (is_cdr_type) {
339 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
340 [ # # ]: 0 : cdr_callbacks_[sample_identity] = [this, ack_request, callback = std::move(callback)](const Bytes& resp_data) {
341 [ # # ]: 0 : ack_manager_.notify(ack_request, [&callback, &resp_data]() { callback(resp_data); });
342 [ # # ]: 0 : };
343 : 0 : } else {
344 [ + - ]: 21 : std::lock_guard param_lock(param_mtx_);
345 [ + - ]: 42 : callbacks_[id] = [this, ack_request, callback = std::move(callback)](const Bytes& resp_data) {
346 [ + - ]: 30 : ack_manager_.notify(ack_request, [&callback, &resp_data]() { callback(resp_data); });
347 [ + - ]: 78 : };
348 : 21 : }
349 : :
350 : 21 : ElapsedTimer timer;
351 : 21 : timer.start();
352 : :
353 [ + - + + ]: 21 : if VUNLIKELY (!wait_for_connected(timeout)) {
354 [ + - ]: 2 : cleanup_callback();
355 : 2 : return false;
356 : : }
357 : :
358 : 19 : auto elapsed = timer.get();
359 : :
360 [ + - - + : 19 : if VUNLIKELY (timeout.count() > 0 && elapsed >= timeout.count()) {
- + ]
361 [ # # ]: 0 : cleanup_callback();
362 : 0 : return false;
363 : : }
364 : :
365 [ + - ]: 19 : bool result = ack_manager_.process(ack_request, timeout.count() - elapsed, [this, &req_data, &id, &write_params]() {
366 [ - + ]: 19 : if (is_cdr_type) {
367 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
368 [ # # ]: 0 : return DdsFactory::write_cdr_data(writer_.get(), req_data, &write_params);
369 : 0 : } else {
370 : 19 : return DdsFactory::write_data(writer_.get(), req_data, id);
371 : : }
372 : : });
373 : :
374 [ + + ]: 19 : if VUNLIKELY (!result) {
375 [ + - ]: 4 : cleanup_callback();
376 : : }
377 : :
378 : 19 : return result;
379 : 21 : }
380 : :
381 : 6 : bool result = false;
382 : :
383 [ - + ]: 6 : if (is_cdr_type) {
384 : : {
385 [ # # ]: 0 : std::lock_guard param_lock(param_mtx_);
386 : :
387 [ # # ]: 0 : cdr_callbacks_[sample_identity] = [callback = std::move(callback)](const Bytes& resp_data) {
388 : 0 : callback(resp_data);
389 [ # # ]: 0 : };
390 : :
391 [ # # ]: 0 : result = DdsFactory::write_cdr_data(writer_.get(), req_data, &write_params);
392 : 0 : }
393 : : } else {
394 : : {
395 [ + - ]: 6 : std::lock_guard param_lock(param_mtx_);
396 [ + - + - ]: 12 : callbacks_[id] = [callback = std::move(callback)](const Bytes& resp_data) { callback(resp_data); };
397 : 6 : }
398 : :
399 [ + - ]: 6 : result = DdsFactory::write_data(writer_.get(), req_data, id);
400 : : }
401 : :
402 [ - + ]: 6 : if VUNLIKELY (!result) {
403 [ # # ]: 0 : cleanup_callback();
404 : : }
405 : :
406 : 6 : return result;
407 : : }
408 : :
409 : : } // namespace vlink
|
