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_getter_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 : 23 : DdsGetterImpl::ReaderListener::ReaderListener(NodeImpl* impl) : DdsReaderListener(impl) {}
36 : :
37 : 28 : void DdsGetterImpl::ReaderListener::on_data_available(dds::DataReader* reader) {
38 : 28 : auto* instance = static_cast<DdsGetterImpl*>(get_impl());
39 : 28 : auto* message_loop = instance->get_message_loop();
40 : :
41 [ + + ]: 28 : if VUNLIKELY (instance->has_suspend.load(std::memory_order_acquire)) {
42 [ - + ]: 1 : if (instance->is_cdr_type) {
43 : 0 : DdsFactory::ReadCdrMessage msg;
44 : :
45 [ # # ]: 0 : msg.sample = instance->type_support_.create_data();
46 : :
47 [ # # # # ]: 0 : while (DdsFactory::take_cdr_data(reader, msg)) {
48 [ # # ]: 0 : if VUNLIKELY (instance->quit_flag_.load(std::memory_order_acquire)) {
49 : 0 : break;
50 : : }
51 : : }
52 : :
53 [ # # ]: 0 : instance->type_support_.delete_data(msg.sample);
54 : 0 : } else {
55 [ + - ]: 1 : DdsFactory::ReadMessage msg;
56 : :
57 [ + - + + ]: 2 : while (DdsFactory::take_data(reader, msg)) {
58 [ - + ]: 1 : if VUNLIKELY (instance->quit_flag_.load(std::memory_order_acquire)) {
59 : 0 : break;
60 : : }
61 : : }
62 : 1 : }
63 : :
64 : 1 : return;
65 : : }
66 : :
67 [ - + ]: 27 : if VUNLIKELY (!instance->callback_) {
68 : 0 : return;
69 : : }
70 : :
71 [ + + ]: 27 : if (message_loop) {
72 [ + - + - ]: 1 : message_loop->post_task([instance, reader]() {
73 [ - + ]: 1 : if VUNLIKELY (!instance->get_message_loop()) {
74 : 0 : return;
75 : : }
76 : :
77 : 1 : instance->process_message(reader);
78 : : });
79 : : } else {
80 : 26 : instance->process_message(reader);
81 : : }
82 : : }
83 : :
84 : : // DdsGetterImpl
85 [ + - + - ]: 27 : DdsGetterImpl::DdsGetterImpl(const DdsConf& conf) : conf_(conf) {}
86 : :
87 : 27 : void DdsGetterImpl::process_message(dds::DataReader* reader) {
88 [ - + ]: 27 : if (is_cdr_type) {
89 : 0 : DdsFactory::ReadCdrMessage msg;
90 : :
91 [ # # ]: 0 : msg.sample = type_support_.create_data();
92 : :
93 [ # # # # ]: 0 : while (DdsFactory::take_cdr_data(reader, msg)) {
94 [ # # ]: 0 : if VUNLIKELY (quit_flag_.load(std::memory_order_acquire)) {
95 : 0 : break;
96 : : }
97 : :
98 [ # # ]: 0 : if VUNLIKELY (!msg.info.valid_data) {
99 : 0 : continue;
100 : : }
101 : :
102 [ # # ]: 0 : if VUNLIKELY (is_latency_and_lost_enabled_.load(std::memory_order_acquire)) {
103 : 0 : last_latency_.store(ElapsedTimer::get_sys_timestamp(ElapsedTimer::kNano, false) - msg.timestamp,
104 : : std::memory_order_relaxed);
105 : :
106 : 0 : uint64_t part1 = 0;
107 : 0 : uint64_t part2 = 0;
108 : :
109 : 0 : std::memcpy(&part1, msg.info.publication_handle.value, sizeof(uint64_t));
110 : 0 : std::memcpy(&part2, msg.info.publication_handle.value + 8, sizeof(uint64_t));
111 : :
112 : 0 : calc_sample_.update(msg.id, part1 ^ part2);
113 : : }
114 : :
115 [ # # ]: 0 : callback_(msg.bytes);
116 : : }
117 : :
118 [ # # ]: 0 : type_support_.delete_data(msg.sample);
119 : 0 : } else {
120 [ + - ]: 27 : DdsFactory::ReadMessage msg;
121 : :
122 [ + - + + ]: 54 : while (DdsFactory::take_data(reader, msg)) {
123 [ - + ]: 27 : if VUNLIKELY (quit_flag_.load(std::memory_order_acquire)) {
124 : 0 : break;
125 : : }
126 : :
127 [ - + ]: 27 : if VUNLIKELY (!msg.info.valid_data) {
128 : 0 : continue;
129 : : }
130 : :
131 [ + + ]: 27 : if VUNLIKELY (is_latency_and_lost_enabled_.load(std::memory_order_acquire)) {
132 : 5 : last_latency_.store(ElapsedTimer::get_sys_timestamp(ElapsedTimer::kNano, false) - msg.timestamp,
133 : : std::memory_order_relaxed);
134 : :
135 : 5 : uint64_t part1 = 0;
136 : 5 : uint64_t part2 = 0;
137 : :
138 : 5 : std::memcpy(&part1, msg.info.publication_handle.value, sizeof(uint64_t));
139 : 5 : std::memcpy(&part2, msg.info.publication_handle.value + 8, sizeof(uint64_t));
140 : :
141 : 5 : calc_sample_.update(msg.id, part1 ^ part2);
142 : : }
143 : :
144 [ + - ]: 27 : callback_(msg.bytes);
145 : : }
146 : 27 : }
147 : 27 : }
148 : :
149 : 23 : void DdsGetterImpl::init() {
150 [ - + - - : 23 : if VUNLIKELY (is_cdr_type && is_security_type) {
- + ]
151 [ # # # # ]: 0 : VLOG_F("Cdr type does not support security.");
152 : : }
153 : :
154 [ + - ]: 23 : participant_ = DdsFactory::create_participant(kPublisher | kSubscriber, conf_, get_all_properties());
155 : :
156 [ + - + - ]: 23 : topic_ = DdsFactory::create_topic(kPublisher | kSubscriber, conf_, participant_.get(), is_cdr_type);
157 : :
158 : 23 : subscriber_ = DdsFactory::create_subscriber(kSubscriber, conf_, participant_.get());
159 : :
160 [ + - - + : 23 : if VUNLIKELY (!participant_ || !topic_) {
- + ]
161 [ # # # # ]: 0 : VLOG_E("DdsGetterImpl::init(): participant/topic creation failed; getter left uninitialised.");
162 : :
163 : 0 : return;
164 : : }
165 : :
166 : 23 : type_support_ = participant_->find_type(topic_->get_type_name());
167 : :
168 [ - + ]: 23 : if VUNLIKELY (!type_support_) {
169 [ # # # # ]: 0 : CLOG_F("Failed to find typesupport (%s).", topic_->get_type_name().c_str());
170 : : }
171 : :
172 : 23 : quit_flag_.store(false, std::memory_order_release);
173 : : }
174 : :
175 : 23 : void DdsGetterImpl::deinit() {
176 : 23 : quit_flag_.store(true, std::memory_order_release);
177 : :
178 : 23 : detach();
179 : :
180 : 23 : reader_.reset();
181 : 23 : listener_.reset();
182 : 23 : subscriber_.reset();
183 : 23 : topic_.reset();
184 : 23 : participant_.reset();
185 : 23 : type_support_.reset();
186 : 23 : }
187 : :
188 : 2 : bool DdsGetterImpl::suspend() {
189 : 2 : has_suspend.store(true, std::memory_order_release);
190 : :
191 : 2 : return true;
192 : : }
193 : :
194 : 2 : bool DdsGetterImpl::resume() {
195 : 2 : has_suspend.store(false, std::memory_order_release);
196 : :
197 : 2 : return true;
198 : : }
199 : :
200 : 3 : bool DdsGetterImpl::is_suspend() const { return has_suspend.load(std::memory_order_acquire); }
201 : :
202 : 0 : const Conf* DdsGetterImpl::get_conf() const { return &conf_; }
203 : :
204 : 2 : const AbstractNode* DdsGetterImpl::get_abstract_node() const { return this; }
205 : :
206 : 4 : Status::BasePtr DdsGetterImpl::get_status(Status::Type type) const {
207 [ + + ]: 4 : if VUNLIKELY (!reader_) {
208 : 1 : return std::make_shared<Status::Unknown>();
209 : : }
210 : :
211 : 3 : return ReaderListener::get_status(reader_.get(), type);
212 : : }
213 : :
214 : 1 : std::any DdsGetterImpl::get_native_handle() const { return subscriber_; }
215 : :
216 : 23 : bool DdsGetterImpl::listen(MsgCallback&& callback) {
217 [ - + ]: 23 : if VUNLIKELY (callback_) {
218 : 0 : return false;
219 : : }
220 : :
221 : 23 : callback_ = std::move(callback);
222 : :
223 [ + - ]: 23 : listener_.emplace(this);
224 : :
225 : 23 : reader_ = DdsFactory::create_datareader(kGetter, conf_, subscriber_.get(), topic_.get(), &listener_.value());
226 : :
227 : 23 : return true;
228 : : }
229 : :
230 : 4 : void DdsGetterImpl::set_latency_and_lost_enabled(bool enable) {
231 : 4 : is_latency_and_lost_enabled_.store(enable, std::memory_order_release);
232 : 4 : }
233 : :
234 : 5 : bool DdsGetterImpl::is_latency_and_lost_enabled() const {
235 : 5 : return is_latency_and_lost_enabled_.load(std::memory_order_acquire);
236 : : }
237 : :
238 : 3 : int64_t DdsGetterImpl::get_latency() const {
239 [ + + ]: 3 : if (!is_latency_and_lost_enabled_.load(std::memory_order_acquire)) {
240 : 1 : return 0;
241 : : }
242 : :
243 : 4 : return last_latency_.load(std::memory_order_relaxed);
244 : : }
245 : :
246 : 2 : SampleLostInfo DdsGetterImpl::get_lost() const {
247 [ + + ]: 2 : if (!is_latency_and_lost_enabled_.load(std::memory_order_acquire)) {
248 : 1 : return SampleLostInfo();
249 : : }
250 : :
251 : 1 : return SampleLostInfo{calc_sample_.get_total(), calc_sample_.get_lost()};
252 : : }
253 : :
254 : : } // namespace vlink
|
