Repository navigation
Expand file tree
/
Copy pathddsBridge.cpp
More file actions
3059 lines (2431 loc) · 106 KB
/
Copy pathddsBridge.cpp
File metadata and controls
3059 lines (2431 loc) · 106 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
//
// FILE ddsBridge.cpp
//
// AUTHOR Ken Zangelin
//
// Copyright 2026 Seamware
// SPDX-License-Identifier: Apache-2.0
//
#include <cstdio> // snprintf, fopen, fread, fwrite
#include <cstring> // strcmp, memcpy
#include <algorithm> // std::min
#include <atomic> // std::atomic
#include <map> // std::map
#include <mutex> // std::mutex, std::recursive_mutex
#include <chrono> // std::chrono::steady_clock
#include <condition_variable> // std::condition_variable
#include <deque> // std::deque
#include <thread> // std::thread
#include <set> // std::set
#include <string> // std::string
#include <vector> // std::vector
#include <link.h> // dl_iterate_phdr, dl_phdr_info
#include <limits.h> // PATH_MAX
#include <cstdlib> // realpath
#include <fastdds/config.hpp> // FASTDDS_VERSION_STR
#include <fastcdr/config.h> // FASTCDR_VERSION_STR
#include <ddsenabler/library/config.h> // DDSENABLER_VERSION_MAJOR, DDSENABLER_VERSION_MINOR
#include <ddsenabler/dds_enabler_runner.hpp> // create_dds_enabler
#include <ddsenabler/DDSEnabler.hpp> // DDSEnabler
#include <ddsenabler/CallbackSet.hpp> // CallbackSet
#include <ddsenabler_yaml/EnablerConfiguration.hpp> // EnablerConfiguration
#include <yaml-cpp/yaml.h> // YAML::Node
#include <cpp_utils/event/FileWatcherHandler.hpp> // FileWatcherHandler
extern "C"
{
#include "corLog/corLog.h" // COR_I, COR_W, COR_E, COR_T
#include "corJson/CorJson.h" // CorJson
#include "corJson/corJsonCreate.h" // corJsonCreate
#include "corJson/corJsonParse.h" // corJsonParse
#include "corTree/corTreeLookup.h" // corTreeLookup
#include "corJson/corJsonRender.h" // corJsonFastRender
#include "corJson/corJsonRenderSize.h" // corJsonFastRenderSize
#include "corAlloc/CorAlloc.h" // CorAlloc
#include "corAlloc/corAllocBufferInit.h" // corAllocBufferInit
#include "corAlloc/corAllocBufferReset.h" // corAllocBufferReset
#include "corAlloc/corAllocStrdup.h" // corAllocStrdup
#include "corTree/corTreeBuilder.h" // corTreeObject, corTreeArray, corTreeString, corTreeBoolean, corTreeChildAdd
#include "corTree/corTreeChildPrepend.h" // corTreeChildPrepend
#include "corBase/corTimeIso.h" // corTimeIso
}
#include "ddsBridge.hpp" // Own interface
#include "version.h" // CORDDSBRIDGE_VERSION
namespace coraine {
namespace dds {
const BridgeBroker* broker = nullptr;
const char* bridgeAlias = "dds";
namespace {
std::shared_ptr<eprosima::ddsenabler::DDSEnabler> enabler;
std::unique_ptr<eprosima::utils::event::FileWatcherHandler> configWatcher; // the --bridgeConfig file (configReload)
//
// Which endpoints the broker asked for.
//
// The Enabler hands over everything its allowlist admits, which in an ordinary
// configuration is everything on the domain. A robot fleet says a great deal
// that a given broker was never configured to want, and answering
// BRIDGE_NOT_FOUND for each of those means a Channel lookup, a trace line and a
// lock, per sample, for data nobody asked for. So the set is kept here and the
// sample is dropped before it crosses the seam.
//
//
// What a Channel told us about an endpoint. The direction decides whether an
// arriving sample is wanted; the kind decides which of the transport's
// mechanisms the endpoint is - a topic is published to, a service is asked.
//
struct Carried
{
BridgeChannelKind kind;
BridgeDirection direction;
};
std::map<std::string, Carried> carried;
std::mutex carriedMutex;
// -----------------------------------------------------------------------------
//
// isCarried - does a Channel of the broker's carry this endpoint (it was handed over by channelAdd)
//
static bool isCarried(const char* endpoint)
{
std::lock_guard<std::mutex> guard(carriedMutex);
return carried.find(endpoint) != carried.end();
}
//
// unwanted - the topics the BROKER has already refused
//
// A topic nobody configured used to be dropped here, which was right while the
// broker's only answer to an unclaimed endpoint was BRIDGE_NOT_FOUND. It can
// have another one now - a catch-all entity, where every endpoint on the
// system shows up as an attribute - and that is a decision this plugin has no
// business making: it does not read the ngsild section of the file and must
// not start.
//
// So an unknown topic is handed over ONCE. If the broker says NOT_FOUND it
// goes in here and is never offered again, and a domain with a thousand
// unmapped topics costs a thousand calls in total rather than per second.
//
// ⚠ Emptied for an endpoint the moment a Channel claims it - see channelAdd.
// A stale refusal would outlive the configuration that caused it.
//
std::set<std::string> unwanted;
char versionBuffer[256];
//
// What the DDS system has told us about itself.
//
// ⭐ THE BROKER CAN ONLY WORK ON WHAT IS ALREADY KNOWN IN THE DDS SYSTEM. It is
// a participant in that system, not the author of it: types and topics are
// defined by the applications on the domain, discovered as they announce
// themselves, and the broker's job is to remember what it was told and hand it
// back when the Enabler asks.
//
// So there is no way to declare a type here and none is wanted. A topic nobody
// has announced is a topic the broker cannot publish to, and saying so is the
// correct answer.
//
std::map<std::string, std::vector<unsigned char>> typeStore;
std::map<std::string, eprosima::ddsenabler::participants::TopicInfo> topicStore;
std::mutex discoveryMutex;
//
// What a type looks like to a PERSON, for the inventory (ABI 14): the IDL text
// and the Enabler's JSON skeleton of a sample - every member, at its type's
// zero value. typeStore keeps what the Enabler needs; this keeps what a mapping
// tool shows. discoveryMutex.
//
struct TypeText
{
std::string idl;
std::string sample;
};
std::map<std::string, TypeText> typeText;
//
// When the DDS system last told us anything - a topic, a type, a service, an
// action - in steady_clock nanoseconds. Set when the Enabler starts, too: the
// inventory waits for discovery to go quiet (inventorySettle), and an empty
// network is quiet only once it has had the time to say something.
//
std::atomic<int64_t> discoveryLastNs{0};
// -----------------------------------------------------------------------------
//
// discoverySeen - the DDS system just told us something
//
static void discoverySeen()
{
discoveryLastNs = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();
}
//
// Where a discovered type is kept between runs. From dds.ngsild.typesDirectory;
// empty means types live only as long as the process, which is correct but
// means a restart is blind until every publisher has announced itself again.
//
std::string typesDirectory;
// -----------------------------------------------------------------------------
//
// typePath - where a type's bytes live on disk
//
// A type name is an IDL identifier with '::' separators; '/' never occurs in
// one, so the name is a filename as it stands.
//
std::string typePath(const char* typeName)
{
return typesDirectory + "/" + typeName + ".bin";
}
// -----------------------------------------------------------------------------
//
// typeNotification - the DDS system announced a type
//
// Kept in memory, and written to typesDirectory if there is one. The bytes are
// the Enabler's INTERNAL representation, not the IDL text: it is what
// type_query has to hand back, so it is what gets stored. The IDL text and the
// sample skeleton are kept beside it, in memory only, for the inventory.
//
void typeNotification(const char* typeName,
const char* serializedType,
const unsigned char* serializedTypeInternal,
uint32_t serializedTypeInternalSize,
const char* dataPlaceholder)
{
if ((typeName == nullptr) || (serializedTypeInternal == nullptr) || (serializedTypeInternalSize == 0))
return;
{
std::lock_guard<std::mutex> guard(discoveryMutex);
typeStore[typeName] = std::vector<unsigned char>(serializedTypeInternal,
serializedTypeInternal + serializedTypeInternalSize);
typeText[typeName] = { (serializedType != nullptr) ? serializedType : "",
(dataPlaceholder != nullptr) ? dataPlaceholder : "" };
}
discoverySeen();
COR_T(0, "dds: learned type '%s' (%u bytes)", typeName, serializedTypeInternalSize);
if (typesDirectory.empty() == true)
return;
//
// Written so a restart is not blind until every publisher happens to
// announce itself again.
//
FILE* fP = fopen(typePath(typeName).c_str(), "wb");
if (fP == nullptr)
{
COR_W("dds: cannot write type '%s' to '%s'", typeName, typesDirectory.c_str());
return;
}
fwrite(serializedTypeInternal, 1, serializedTypeInternalSize, fP);
fclose(fP);
}
// -----------------------------------------------------------------------------
//
// typeQuery - the Enabler needs a type's bytes
//
// From memory, else from typesDirectory. Ownership of the buffer passes to the
// Enabler through the unique_ptr, which is one of the several things in this
// API that cannot be expressed in C.
//
bool typeQuery(const char* typeName,
std::unique_ptr<const unsigned char[]>& serializedTypeInternal,
uint32_t& serializedTypeInternalSize)
{
if (typeName == nullptr)
return false;
{
std::lock_guard<std::mutex> guard(discoveryMutex);
auto it = typeStore.find(typeName);
if (it != typeStore.end())
{
unsigned char* copy = new unsigned char[it->second.size()];
memcpy(copy, it->second.data(), it->second.size());
serializedTypeInternal.reset(copy);
serializedTypeInternalSize = (uint32_t) it->second.size();
return true;
}
}
if (typesDirectory.empty() == true)
return false;
FILE* fP = fopen(typePath(typeName).c_str(), "rb");
if (fP == nullptr)
return false;
fseek(fP, 0, SEEK_END);
long size = ftell(fP);
fseek(fP, 0, SEEK_SET);
if (size <= 0)
{
fclose(fP);
return false;
}
unsigned char* data = new unsigned char[size];
if (fread(data, 1, (size_t) size, fP) != (size_t) size)
{
fclose(fP);
delete[] data;
return false;
}
fclose(fP);
{
std::lock_guard<std::mutex> guard(discoveryMutex);
typeStore[typeName] = std::vector<unsigned char>(data, data + size);
}
serializedTypeInternal.reset(data);
serializedTypeInternalSize = (uint32_t) size;
COR_T(0, "dds: loaded type '%s' from disk (%ld bytes)", typeName, size);
return true;
}
// -----------------------------------------------------------------------------
//
// topicNotification - the DDS system announced a topic
//
void topicNotification(const char* topicName, const eprosima::ddsenabler::participants::TopicInfo& topicInfo)
{
if (topicName == nullptr)
return;
{
std::lock_guard<std::mutex> guard(discoveryMutex);
topicStore[topicName] = topicInfo;
}
discoverySeen();
COR_T(0, "dds: learned topic '%s' of type '%s'", topicName, topicInfo.type_name.c_str());
}
// -----------------------------------------------------------------------------
//
// topicQuery - the Enabler needs a topic's type and QoS
//
// ⭐ FALSE when the topic is unknown, and that is the whole point of
// implementing this. Answering true with an unfilled TopicInfo hands the
// Enabler an empty type name, and the failure then happens further in and looks
// like something else. A topic nobody has announced is a topic this broker
// cannot serialize for, and the honest answer is the useful one.
//
bool topicQuery(const char* topicName, eprosima::ddsenabler::participants::TopicInfo& topicInfo)
{
if (topicName == nullptr)
return false;
std::lock_guard<std::mutex> guard(discoveryMutex);
auto it = topicStore.find(topicName);
if (it == topicStore.end())
{
COR_T(0, "dds: topic '%s' has not been announced on this domain", topicName);
return false;
}
topicInfo = it->second;
return true;
}
//
// Defined below, with the rest of the sample handling - a reply arrives inside
// the same envelope a sample does, so it comes off in the same place.
//
//
// EnvelopeInfo - what the envelope says ABOUT a sample: the meta object (ABI 6)
//
struct EnvelopeInfo
{
std::string participantId; // the envelope's top-level "id"
std::string dataType; // the topic member's "type"
std::string instanceHandle; // the key of the one sample under "data"
};
static bool sampleUnwrap(const char* topicName, const char* json, std::string& payload, EnvelopeInfo* infoP = nullptr);
static std::string metaBuild(const EnvelopeInfo& info, bool cutParticipant, uint64_t requestId, int64_t publishedAtNs);
// -----------------------------------------------------------------------------
//
// DDS_SERVICE_REPLY - where a reply lands, and a name that is PROVISIONAL
//
// ⚠ The next release of NGSI-LD is expected to introduce a first-class Service
// Execution concept, and it will decide how an invocation and its answer appear
// in the model. Until it does, something has to be chosen, because a client
// that invokes a service has to read the answer somewhere - and what is chosen
// here is the shape already in field use, so that deployments and tooling that
// read it keep working.
//
// ⭐ IT IS SPELLED HERE AND NOWHERE ELSE. The broker is handed a sub-attribute
// name and stores it; it does not know this string, has no branch on it and
// never will. When Service Execution lands, what changes is this line and the
// translation around it - not the broker, and not the model.
//
#define DDS_SERVICE_REPLY "reply" // Orion-LD's name for it - its clients read it
#define DDS_SERVICE_REQUEST "request" // ... and for the request it answers, beside it (ABI 7)
// -----------------------------------------------------------------------------
//
// ServiceTypes - the request and reply types of one service
//
// ⭐ A SERVICE CANNOT BE LEARNED BY LISTENING, and that is the operational
// difference from a topic. A topic's type arrives with the first publisher;
// there is nobody to learn a service's types from before the first client
// speaks, and by then it is too late - the request has to be serialized to be
// sent at all. So they are configured, and the types themselves come off disk
// (see typeQuery).
//
// The file names them per service; the convention when it does not is the one
// the tooling in this world already uses, name + _Request / _Response.
//
struct ServiceTypes
{
std::string request;
std::string reply;
};
std::map<std::string, ServiceTypes> serviceConfig; // from the configuration file
// -----------------------------------------------------------------------------
//
// Actions - what the configuration says about each, and the QoS they announce
//
// A ROS 2 action is five DDS entities, and every one of their types follows
// from the action's own: <type>_SendGoal_Request_, _SendGoal_Response_,
// _GetResult_Request_, _GetResult_Response_ and _FeedbackMessage_, plus the two
// every action shares, action_msgs' CancelGoal and GoalStatusArray. So an
// actions entry names the action type once ("type") and the rest is derived.
//
// The status topic is TRANSIENT LOCAL, as ROS 2 publishes it - a client that
// joins late still reads where each goal stands. The rest is reliable and
// volatile, as a service's topics are.
//
#define ACTION_STATUS_QOS "reliability: true\ndurability: true\nownership: false\nkeyed: false"
std::map<std::string, std::string> actionConfig; // endpoint -> action type, from the configuration file
std::mutex serviceMutex;
//
// Services and actions the configuration does not name, as discovered (ABI 8):
// their types, so that serviceQuery / actionQuery can answer for them once the
// broker carries them (endpointDiscoveredIn -> channelAdd). serviceMutex.
//
static std::map<std::string, ServiceTypes> discoveredService;
static std::set<std::string> discoveredEndpoints; // every service and action discovered, configured or not
static std::map<std::string, std::string> discoveredAction; // action -> its type (the goal request's, minus _SendGoal_Request_)
//
// Every service and action as the domain announced it, configured or not -
// names, types and QoS, for the inventory (ABI 14). serviceMutex.
//
static std::map<std::string, eprosima::ddsenabler::participants::ServiceInfo> announcedService;
static std::map<std::string, eprosima::ddsenabler::participants::ActionInfo> announcedAction;
static void upcallDiscoveredQueueAdd(const char* endpoint, BridgeChannelKind kind);
//
// Which services this process ANSWERS, and who answers them.
//
// ⛔ Empty in the broker, always. The broker is a client and has nothing to
// compute an answer with; this exists for a host that is not a broker - see
// BridgeServer.h.
//
std::map<std::string, BridgeServiceRequestFunc> served;
// -----------------------------------------------------------------------------
//
// SERVICE_QOS - what is announced about a service's two topics
//
// The Enabler asks for QoS as text in its own serialized form. Reliable and
// volatile, which is what a request/reply exchange wants: an answer that
// arrives late is still an answer, an answer that is durable is an answer to a
// question nobody is waiting for any more.
//
#define SERVICE_QOS "reliability: true\ndurability: false\nownership: false\nkeyed: false"
// -----------------------------------------------------------------------------
//
// serviceQuery - the Enabler needs a service's request and reply types
//
// ⭐ FALSE when the service was never configured, for the same reason
// topicQuery answers false for an unknown topic: an empty ServiceInfo would be
// accepted here and fail somewhere further in, looking like something else.
//
bool serviceQuery(const char* serviceName, eprosima::ddsenabler::participants::ServiceInfo& serviceInfo)
{
if (serviceName == nullptr)
return false;
std::lock_guard<std::mutex> guard(serviceMutex);
auto it = serviceConfig.find(serviceName);
if (it == serviceConfig.end())
{
it = discoveredService.find(serviceName); // not configured, but discovered (ABI 8)
if (it == discoveredService.end())
{
COR_T(0, "dds: service '%s' is neither configured nor discovered - its types are unknown", serviceName);
return false;
}
}
serviceInfo.request = eprosima::ddsenabler::participants::TopicInfo(it->second.request, SERVICE_QOS);
serviceInfo.reply = eprosima::ddsenabler::participants::TopicInfo(it->second.reply, SERVICE_QOS);
return true;
}
// -----------------------------------------------------------------------------
//
// serviceNotification - a service was discovered on the domain
//
void serviceNotification(const char* serviceName, const eprosima::ddsenabler::participants::ServiceInfo& serviceInfo)
{
if (serviceName == nullptr)
return;
//
// Kept as the domain announces it: the request's type name is what Orion-LD
// shows as the request's ddsDataType, and all of it is the inventory's.
//
bool unconfigured;
{
std::lock_guard<std::mutex> guard(serviceMutex);
announcedService[serviceName] = serviceInfo;
discoveredEndpoints.insert(serviceName);
unconfigured = (serviceConfig.find(serviceName) == serviceConfig.end());
if (unconfigured == true)
discoveredService[serviceName] = { serviceInfo.request.type_name, serviceInfo.reply.type_name };
}
discoverySeen();
COR_T(0, "dds: service '%s' discovered (request '%s', reply '%s')",
serviceName, serviceInfo.request.type_name.c_str(), serviceInfo.reply.type_name.c_str());
//
// Reported to the broker (ABI 8) when it is not configured - the broker may
// carry it - or when a Channel of the broker's carries it already: that
// Channel then shows it was found (endpointDiscovered), the one way anybody
// outside this plugin, a test included, can know that a request now has a
// server to reach. One configured here that no Channel carries is not
// reported: the broker would put it on its catch-all, which configuring it
// never asked for. Queued - this is an Enabler callback, inside its lock,
// and the broker may call channelAdd back.
//
if ((unconfigured == true) || (isCarried(serviceName) == true))
upcallDiscoveredQueueAdd(serviceName, BridgeChannelService);
}
// -----------------------------------------------------------------------------
//
// Upcalls - what the Enabler hands over, delivered to the broker by our own thread
//
// ⭐ EVERY ENABLER CALLBACK RUNS INSIDE THE ENABLER'S OWN LOCK (its Handler's
// mtx_, taken in add_data and held across the notification), and the same lock
// is taken by send_service_request. So nothing reached from a callback may wait
// for anything a thread calling the Enabler might hold - and the broker, reached
// from a callback, stores, takes its own locks, and may wait.
//
// It did deadlock, with two waited-for service requests in flight
// (serviceInvokeTracked holds trackedMutex across send_service_request, which
// wants mtx_; the reply callback held mtx_ and wanted trackedMutex): every
// request after it timed out, and the broker never answered another reply.
//
// So a callback only copies what arrived onto this queue and returns, and one
// thread of the plugin's own hands it to the broker, in arrival order, holding
// none of the Enabler's locks. The queue is bounded; a callback waits for room,
// which is the back-pressure the Enabler had anyway while the broker stored
// each sample inside its lock.
//
enum UpcallKind
{
UpcallData,
UpcallReply,
UpcallGoalFeedback,
UpcallGoalStatus,
UpcallGoalResult,
UpcallDiscovered // a service or an action nobody configured (ABI 8)
};
struct Upcall
{
UpcallKind kind;
std::string endpoint;
std::string json;
uint64_t requestId;
int64_t publishTime;
// A goal's events only
eprosima::ddsenabler::participants::UUID goalUuid;
int statusCode;
std::string statusMessage;
// UpcallDiscovered only
BridgeChannelKind channelKind;
};
static const size_t UPCALL_QUEUE_MAX = 10000;
static std::deque<Upcall> upcallQueue;
static std::mutex upcallMutex;
static std::condition_variable upcallReady;
static std::condition_variable upcallRoom;
static std::thread upcallThread;
static bool upcallRunning = false;
static void dataDeliver(const char* topicName, const char* json, int64_t publishTime);
static void replyDeliver(const char* serviceName, const char* json, uint64_t requestId, int64_t publishTime);
static void goalDeliver(const Upcall& upcall);
static void discoveredDeliver(const char* endpoint, BridgeChannelKind kind);
static void goalSweep();
// -----------------------------------------------------------------------------
//
// upcallQueueAdd - on an ENABLER thread: copy, queue, return
//
static void upcallQueueAdd(UpcallKind kind, const char* endpoint, const char* json, uint64_t requestId, int64_t publishTime)
{
std::unique_lock<std::mutex> lock(upcallMutex);
upcallRoom.wait(lock, [] { return (upcallRunning == false) || (upcallQueue.size() < UPCALL_QUEUE_MAX); });
if (upcallRunning == false)
return; // closing - nothing reaches the broker from here on
Upcall upcall;
upcall.kind = kind;
upcall.endpoint = endpoint;
upcall.json = (json != nullptr) ? json : "";
upcall.requestId = requestId;
upcall.publishTime = publishTime;
upcall.statusCode = 0;
upcallQueue.push_back(std::move(upcall));
upcallReady.notify_one();
}
// -----------------------------------------------------------------------------
//
// upcallDiscoveredQueueAdd - a service or action discovered, on an ENABLER thread: queue, return
//
static void upcallDiscoveredQueueAdd(const char* endpoint, BridgeChannelKind kind)
{
std::unique_lock<std::mutex> lock(upcallMutex);
upcallRoom.wait(lock, [] { return (upcallRunning == false) || (upcallQueue.size() < UPCALL_QUEUE_MAX); });
if (upcallRunning == false)
return;
Upcall upcall;
upcall.kind = UpcallDiscovered;
upcall.endpoint = endpoint;
upcall.requestId = 0;
upcall.publishTime = 0;
upcall.statusCode = 0;
upcall.channelKind = kind;
upcallQueue.push_back(std::move(upcall));
upcallReady.notify_one();
}
// -----------------------------------------------------------------------------
//
// discoveredDeliver - tell the broker, on the upcall thread (ABI 8)
//
// The broker may carry it: a Channel on its catch-all, handed back to this
// plugin through channelAdd() before the call returns - which is why this runs
// here, holding none of the Enabler's locks.
//
static void discoveredDeliver(const char* endpoint, BridgeChannelKind kind)
{
if ((endpoint == nullptr) || (broker == nullptr))
return;
if ((broker->abiVersion < 8) || (broker->endpointDiscoveredIn == nullptr))
{
COR_T(0, "dds: '%s' discovered - the host predates ABI 8, not reported", endpoint);
return;
}
broker->endpointDiscoveredIn(bridgeAlias, endpoint, (int) kind);
}
// -----------------------------------------------------------------------------
//
// upcallGoalQueueAdd - a goal's event, on an ENABLER thread: copy, queue, return
//
static void upcallGoalQueueAdd(UpcallKind kind,
const char* actionName,
const eprosima::ddsenabler::participants::UUID& goalUuid,
const char* json,
int statusCode,
const char* statusMessage,
int64_t publishTime)
{
std::unique_lock<std::mutex> lock(upcallMutex);
upcallRoom.wait(lock, [] { return (upcallRunning == false) || (upcallQueue.size() < UPCALL_QUEUE_MAX); });
if (upcallRunning == false)
return;
Upcall upcall;
upcall.kind = kind;
upcall.endpoint = actionName;
upcall.json = (json != nullptr) ? json : "";
upcall.requestId = 0;
upcall.publishTime = publishTime;
upcall.goalUuid = goalUuid;
upcall.statusCode = statusCode;
upcall.statusMessage = (statusMessage != nullptr) ? statusMessage : "";
upcallQueue.push_back(std::move(upcall));
upcallReady.notify_one();
}
// -----------------------------------------------------------------------------
//
// upcallLoop - the delivery thread
//
static void upcallLoop()
{
for (;;)
{
Upcall upcall;
{
std::unique_lock<std::mutex> lock(upcallMutex);
//
// Timed: a goal whose result never comes is ended by goalSweep(),
// and that has to happen whether or not anything else arrives.
//
upcallReady.wait_for(lock, std::chrono::milliseconds(200),
[] { return (upcallRunning == false) || (upcallQueue.empty() == false); });
if (upcallRunning == false)
return;
if (upcallQueue.empty() == true)
{
lock.unlock();
goalSweep();
continue;
}
upcall = std::move(upcallQueue.front());
upcallQueue.pop_front();
upcallRoom.notify_one();
}
if (upcall.kind == UpcallData)
dataDeliver(upcall.endpoint.c_str(), upcall.json.c_str(), upcall.publishTime);
else if (upcall.kind == UpcallReply)
replyDeliver(upcall.endpoint.c_str(), upcall.json.c_str(), upcall.requestId, upcall.publishTime);
else if (upcall.kind == UpcallDiscovered)
discoveredDeliver(upcall.endpoint.c_str(), upcall.channelKind);
else
goalDeliver(upcall);
}
}
// -----------------------------------------------------------------------------
//
// upcallsStart / upcallsStop -
//
// Stop discards what is still queued: it runs as the broker closes, and nothing
// may reach the broker once close() has returned.
//
static void upcallsStart()
{
std::lock_guard<std::mutex> guard(upcallMutex);
upcallRunning = true;
upcallThread = std::thread(upcallLoop);
}
static void upcallsStop()
{
{
std::lock_guard<std::mutex> guard(upcallMutex);
if (upcallRunning == false)
return;
upcallRunning = false;
upcallQueue.clear();
}
upcallReady.notify_all();
upcallRoom.notify_all();
if (upcallThread.joinable())
upcallThread.join();
}
// -----------------------------------------------------------------------------
//
// dataNotification / serviceReplyNotification - the Enabler's callbacks: queue only
//
void dataNotification(const char* topicName, const char* json, int64_t publishTime)
{
if ((topicName == nullptr) || (json == nullptr))
return;
upcallQueueAdd(UpcallData, topicName, json, 0, publishTime);
}
void serviceReplyNotification(const char* serviceName, const char* json, uint64_t requestId, int64_t publishTime)
{
if ((serviceName == nullptr) || (json == nullptr))
return;
upcallQueueAdd(UpcallReply, serviceName, json, requestId, publishTime);
}
// -----------------------------------------------------------------------------
//
// Tracked requests - the broker's token, by the Enabler's request id
//
// A request somebody waits for (ddsSync, see serviceInvokeTracked) must have
// its reply handed back with the token the broker gave it. The Enabler knows
// the reply only by ITS request id, so this is the one place the two meet.
//
// ⭐ ONE LOCK ACROSS "SEND AND REMEMBER", and the reply's delivery takes it too.
// The request id exists only once send_service_request has returned, and the
// reply may be delivered before this thread gets to write the id down; taking
// the lock first makes the reply wait for the entry instead of missing it and
// going the untracked way.
//
// ⚠ The reply WAITS on the delivery thread (see Upcalls), never on an Enabler
// thread: send_service_request takes the Enabler's lock, and an Enabler thread
// waiting for this one while holding it was a deadlock.
//
// An entry whose reply never comes is forgotten after TRACKED_KEEP: the request
// waiting for it gave up long before, and the broker drops a reply that late
// anyway.
//
struct TrackedRequest
{
uint64_t token;
std::chrono::steady_clock::time_point sentAt;
};
static std::recursive_mutex trackedMutex;
static std::map<uint64_t, TrackedRequest> trackedRequests;
static const std::chrono::minutes TRACKED_KEEP(10);
// -----------------------------------------------------------------------------
//
// Sent requests - what was asked, by request id, for the reply to carry (ABI 7)
//
// Orion-LD shows a service call as "request" beside "reply": what was sent, its
// request id, its data type and when. The Enabler hands a reply back with the
// request id and nothing of the request, so it is kept here, for EVERY request
// - tracked or not - under trackedMutex, and taken when the reply comes.
// Forgotten after TRACKED_KEEP, as a tracked one is.
//
struct SentRequest
{
std::string json;
std::string requestType;
int64_t sentNs; // wall clock, nanoseconds since the epoch
std::chrono::steady_clock::time_point sentAt;
};
static std::map<uint64_t, SentRequest> sentRequests;
// -----------------------------------------------------------------------------
//
// sentRemember - a request was sent. Caller holds trackedMutex.
//
static void sentRemember(uint64_t requestId, const char* endpoint, const char* json, std::chrono::steady_clock::time_point now)
{
for (auto it = sentRequests.begin(); it != sentRequests.end(); )
{
if (now - it->second.sentAt > TRACKED_KEEP)
it = sentRequests.erase(it);
else
++it;
}
SentRequest sent;
sent.json = json;
sent.sentAt = now;
sent.sentNs = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
{
std::lock_guard<std::mutex> guard(serviceMutex);
auto dIt = announcedService.find(endpoint);
auto cIt = serviceConfig.find(endpoint);
if ((dIt != announcedService.end()) && (dIt->second.request.type_name.empty() == false))
sent.requestType = dIt->second.request.type_name;
else if (cIt != serviceConfig.end())
sent.requestType = cIt->second.request;
}
sentRequests[requestId] = std::move(sent);
}
// -----------------------------------------------------------------------------
//
// sentTake - what was asked, for this reply; false if it is not known
//
static bool sentTake(uint64_t requestId, SentRequest& out)
{
std::lock_guard<std::recursive_mutex> guard(trackedMutex);
auto it = sentRequests.find(requestId);
if (it == sentRequests.end())
return false;
out = std::move(it->second);
sentRequests.erase(it);
return true;
}
// -----------------------------------------------------------------------------
//
// trackedTake - the token a reply must carry back, 0 if it was not tracked
//
static uint64_t trackedTake(uint64_t requestId)
{
std::lock_guard<std::recursive_mutex> guard(trackedMutex);
auto it = trackedRequests.find(requestId);
if (it == trackedRequests.end())
return 0;
uint64_t token = it->second.token;
trackedRequests.erase(it);
return token;
}