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 : 63 : DdsSubscriberImpl::ReaderListener::ReaderListener(NodeImpl* impl) : DdsReaderListener(impl) {}
36 : :
37 : 109 : void DdsSubscriberImpl::ReaderListener::on_data_available(dds::DataReader* reader) {
38 : 109 : auto* instance = static_cast<DdsSubscriberImpl*>(get_impl());
39 : 109 : auto* message_loop = instance->get_message_loop();
40 : :
41 [ + + ]: 109 : 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 [ - + ]: 108 : if VUNLIKELY (!instance->callback_) {
69 : 0 : return;
70 : : }
71 : :
72 [ + + ]: 108 : 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 : 107 : instance->process_message(reader);
82 : : }
83 : : }
84 : :
85 : : // DdsSubscriberImpl
86 [ + - + - ]: 76 : DdsSubscriberImpl::DdsSubscriberImpl(const DdsConf& conf) : conf_(conf) {}
87 : :
88 : 108 : void DdsSubscriberImpl::process_message(dds::DataReader* reader) {
89 [ - + ]: 108 : 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 : 108 : DdsFactory::ReadMessage msg;
123 : :
124 [ + - + + ]: 216 : while (DdsFactory::take_data(reader, msg)) {
125 [ - + ]: 108 : if VUNLIKELY (quit_flag_.load(std::memory_order_acquire)) {
126 [ # # ]: 0 : DdsFactory::return_data_loan(reader, msg);
127 : 0 : break;
128 : : }
129 : :
130 [ + - ]: 108 : const auto& info = msg.infos[0];
131 : :
132 [ - + ]: 108 : if VUNLIKELY (!info.valid_data) {
133 [ # # ]: 0 : DdsFactory::return_data_loan(reader, msg);
134 : 0 : continue;
135 : : }
136 : :
137 [ + + ]: 108 : 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 [ + - + - : 108 : callback_(msg.samples[0].data());
+ - ]
151 [ + - ]: 108 : DdsFactory::return_data_loan(reader, msg);
152 : : }
153 : 108 : }
154 : 108 : }
155 : :
156 : 72 : void DdsSubscriberImpl::init() {
157 [ + + - + : 72 : if VUNLIKELY (is_cdr_type && is_security_type) {
- + ]
158 [ # # # # : 0 : VLOG_F("Cdr type does not support security.");
# # ]
159 : : }
160 : :
161 [ + - ]: 72 : participant_ = DdsFactory::create_participant(kPublisher | kSubscriber, conf_, get_all_properties());
162 : :
163 [ + - ]: 72 : topic_ = DdsFactory::create_topic(kPublisher | kSubscriber, conf_, participant_.get(), is_cdr_type, {}, ser_type);
164 : :
165 : 72 : subscriber_ = DdsFactory::create_subscriber(kSubscriber, conf_, participant_.get());
166 : :
167 [ + - - + : 72 : if VUNLIKELY (!participant_ || !topic_) {
- + ]
168 [ # # # # : 0 : VLOG_E("DdsSubscriberImpl::init(): participant/topic creation failed; subscriber left uninitialised.");
# # ]
169 : :
170 : 0 : return;
171 : : }
172 : :
173 [ + + ]: 72 : if (is_cdr_type) {
174 : 1 : ser_type = topic_->get_type_name();
175 : : }
176 : :
177 : 72 : quit_flag_.store(false, std::memory_order_release);
178 : : }
179 : :
180 : 72 : void DdsSubscriberImpl::deinit() {
181 : 72 : quit_flag_.store(true, std::memory_order_release);
182 : :
183 : 72 : detach();
184 : :
185 : 72 : reader_.reset();
186 : 72 : listener_.reset();
187 : 72 : subscriber_.reset();
188 : 72 : topic_.reset();
189 : 72 : participant_.reset();
190 : 72 : callback_ = {};
191 : 72 : is_listened = false;
192 : 72 : }
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 : 63 : bool DdsSubscriberImpl::listen(MsgCallback&& callback) {
223 [ - + ]: 63 : if VUNLIKELY (callback_) {
224 : 0 : return false;
225 : : }
226 : :
227 : 63 : callback_ = std::move(callback);
228 : :
229 [ + - ]: 63 : listener_.emplace(this);
230 : :
231 : 126 : reader_ = DdsFactory::create_datareader(init_impl_type, conf_, subscriber_.get(), topic_.get(), &listener_.value(),
232 : 126 : is_cdr_type);
233 : :
234 [ + + ]: 63 : if VUNLIKELY (!reader_) {
235 : 4 : listener_.reset();
236 : 4 : callback_ = {};
237 : 4 : return false;
238 : : }
239 : :
240 : 59 : return true;
241 : : }
242 : :
243 : 4 : void DdsSubscriberImpl::set_latency_and_lost_enabled(bool enable) {
244 : 4 : is_latency_and_lost_enabled_.store(enable, std::memory_order_release);
245 : 4 : }
246 : :
247 : 5 : bool DdsSubscriberImpl::is_latency_and_lost_enabled() const {
248 : 5 : return is_latency_and_lost_enabled_.load(std::memory_order_acquire);
249 : : }
250 : :
251 : 3 : int64_t DdsSubscriberImpl::get_latency() const {
252 [ + + ]: 3 : if (!is_latency_and_lost_enabled_.load(std::memory_order_acquire)) {
253 : 1 : return 0;
254 : : }
255 : :
256 : 4 : return last_latency_.load(std::memory_order_relaxed);
257 : : }
258 : :
259 : 3 : SampleLostInfo DdsSubscriberImpl::get_lost() const {
260 [ + + ]: 3 : if (!is_latency_and_lost_enabled_.load(std::memory_order_acquire)) {
261 : 1 : return SampleLostInfo();
262 : : }
263 : :
264 : 2 : return SampleLostInfo{calc_sample_.get_total(), calc_sample_.get_lost()};
265 : : }
266 : :
267 : : } // namespace vlink
|
