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_subscriber_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 : 60 : DdsSubscriberImpl::ReaderListener::ReaderListener(NodeImpl* impl) : DdsReaderListener(impl) {}
36 : :
37 : 107 : void DdsSubscriberImpl::ReaderListener::on_data_available(dds::DataReader* reader) {
38 : 107 : auto* instance = static_cast<DdsSubscriberImpl*>(get_impl());
39 : 107 : auto* message_loop = instance->get_message_loop();
40 : :
41 [ + + ]: 107 : if VUNLIKELY (instance->has_suspend.load(std::memory_order_relaxed)) {
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 [ - + ]: 106 : if VUNLIKELY (!instance->callback_) {
69 : 0 : return;
70 : : }
71 : :
72 [ + + ]: 106 : 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 : 105 : instance->process_message(reader);
82 : : }
83 : : }
84 : :
85 : : // DdsSubscriberImpl
86 [ + - + - ]: 73 : DdsSubscriberImpl::DdsSubscriberImpl(const DdsConf& conf) : conf_(conf) {}
87 : :
88 : 106 : void DdsSubscriberImpl::process_message(dds::DataReader* reader) {
89 [ - + ]: 106 : 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 : 106 : DdsFactory::ReadMessage msg;
123 : :
124 [ + - + + ]: 212 : while (DdsFactory::take_data(reader, msg)) {
125 [ - + ]: 106 : if VUNLIKELY (quit_flag_.load(std::memory_order_acquire)) {
126 [ # # ]: 0 : DdsFactory::return_data_loan(reader, msg);
127 : 0 : break;
128 : : }
129 : :
130 [ + - ]: 106 : const auto& info = msg.infos[0];
131 : :
132 [ - + ]: 106 : if VUNLIKELY (!info.valid_data) {
133 [ # # ]: 0 : DdsFactory::return_data_loan(reader, msg);
134 : 0 : continue;
135 : : }
136 : :
137 [ + + ]: 106 : if VUNLIKELY (is_latency_and_lost_enabled_.load(std::memory_order_acquire)) {
138 : 10 : last_latency_.store(ElapsedTimer::get_sys_timestamp(ElapsedTimer::kNano, false) - msg.timestamp,
139 : : std::memory_order_relaxed);
140 : :
141 : 10 : uint64_t part1 = 0;
142 : 10 : uint64_t part2 = 0;
143 : :
144 : 10 : std::memcpy(&part1, info.publication_handle.value, sizeof(uint64_t));
145 : 10 : std::memcpy(&part2, info.publication_handle.value + 8, sizeof(uint64_t));
146 : :
147 : 10 : calc_sample_.update(msg.id, part1 ^ part2);
148 : : }
149 : :
150 [ + - + - : 106 : callback_(msg.samples[0].data());
+ - ]
151 [ + - ]: 106 : DdsFactory::return_data_loan(reader, msg);
152 : : }
153 : 106 : }
154 : 106 : }
155 : :
156 : 69 : void DdsSubscriberImpl::init() {
157 [ + + - + : 69 : if VUNLIKELY (is_cdr_type && is_security_type) {
- + ]
158 [ # # # # ]: 0 : VLOG_F("Cdr type does not support security.");
159 : : }
160 : :
161 [ + - ]: 69 : participant_ = DdsFactory::create_participant(kPublisher | kSubscriber, conf_, get_all_properties());
162 : :
163 [ + - ]: 69 : topic_ = DdsFactory::create_topic(kPublisher | kSubscriber, conf_, participant_.get(), is_cdr_type, {}, ser_type);
164 : :
165 : 69 : subscriber_ = DdsFactory::create_subscriber(kSubscriber, conf_, participant_.get());
166 : :
167 [ + - - + : 69 : if VUNLIKELY (!participant_ || !topic_) {
- + ]
168 [ # # # # ]: 0 : VLOG_E("DdsSubscriberImpl::init(): participant/topic creation failed; subscriber left uninitialised.");
169 : :
170 : 0 : return;
171 : : }
172 : :
173 [ + + ]: 69 : if (is_cdr_type) {
174 : 1 : ser_type = topic_->get_type_name();
175 : : }
176 : :
177 : 69 : quit_flag_.store(false, std::memory_order_release);
178 : : }
179 : :
180 : 69 : void DdsSubscriberImpl::deinit() {
181 : 69 : quit_flag_.store(true, std::memory_order_release);
182 : :
183 : 69 : detach();
184 : :
185 : 69 : reader_.reset();
186 : 69 : listener_.reset();
187 : 69 : subscriber_.reset();
188 : 69 : topic_.reset();
189 : 69 : participant_.reset();
190 : 69 : callback_ = {};
191 : 69 : is_listened = false;
192 : 69 : }
193 : :
194 : 2 : bool DdsSubscriberImpl::suspend() {
195 : 2 : has_suspend.store(true, std::memory_order_relaxed);
196 : :
197 : 2 : return true;
198 : : }
199 : :
200 : 2 : bool DdsSubscriberImpl::resume() {
201 : 2 : has_suspend.store(false, std::memory_order_relaxed);
202 : :
203 : 2 : return true;
204 : : }
205 : :
206 : 3 : bool DdsSubscriberImpl::is_suspend() const { return has_suspend.load(std::memory_order_relaxed); }
207 : :
208 : 0 : const Conf* DdsSubscriberImpl::get_conf() const { return &conf_; }
209 : :
210 : 16 : const AbstractNode* DdsSubscriberImpl::get_abstract_node() const { return this; }
211 : :
212 : 9 : Status::BasePtr DdsSubscriberImpl::get_status(Status::Type type) const {
213 [ + + ]: 9 : if VUNLIKELY (!reader_) {
214 : 2 : return std::make_shared<Status::Unknown>();
215 : : }
216 : :
217 : 7 : return ReaderListener::get_status(reader_.get(), type);
218 : : }
219 : :
220 : 11 : std::any DdsSubscriberImpl::get_native_handle() const { return subscriber_; }
221 : :
222 : 60 : bool DdsSubscriberImpl::listen(MsgCallback&& callback) {
223 [ - + ]: 60 : if VUNLIKELY (callback_) {
224 : 0 : return false;
225 : : }
226 : :
227 : 60 : callback_ = std::move(callback);
228 : :
229 [ + - ]: 60 : listener_.emplace(this);
230 : :
231 : 120 : reader_ = DdsFactory::create_datareader(kSubscriber, conf_, subscriber_.get(), topic_.get(), &listener_.value(),
232 : 120 : is_cdr_type);
233 : :
234 : 60 : return true;
235 : : }
236 : :
237 : 4 : void DdsSubscriberImpl::set_latency_and_lost_enabled(bool enable) {
238 : 4 : is_latency_and_lost_enabled_.store(enable, std::memory_order_release);
239 : 4 : }
240 : :
241 : 5 : bool DdsSubscriberImpl::is_latency_and_lost_enabled() const {
242 : 5 : return is_latency_and_lost_enabled_.load(std::memory_order_acquire);
243 : : }
244 : :
245 : 3 : int64_t DdsSubscriberImpl::get_latency() const {
246 [ + + ]: 3 : if (!is_latency_and_lost_enabled_.load(std::memory_order_acquire)) {
247 : 1 : return 0;
248 : : }
249 : :
250 : 4 : return last_latency_.load(std::memory_order_relaxed);
251 : : }
252 : :
253 : 3 : SampleLostInfo DdsSubscriberImpl::get_lost() const {
254 [ + + ]: 3 : if (!is_latency_and_lost_enabled_.load(std::memory_order_acquire)) {
255 : 1 : return SampleLostInfo();
256 : : }
257 : :
258 : 2 : return SampleLostInfo{calc_sample_.get_total(), calc_sample_.get_lost()};
259 : : }
260 : :
261 : : } // namespace vlink
|
