Skip to content

Commit a65e9ed

Browse files
PeterBowmanPeterBowman
committed
Only allow a single CAN bus per broker
1 parent 0f163f7 commit a65e9ed

File tree

6 files changed

+181
-146
lines changed

6 files changed

+181
-146
lines changed

‎libraries/YarpPlugins/CanBusBroker/CanBusBroker.hpp

+7-4
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,6 @@
33
#ifndef __CAN_BUS_BROKER_HPP__
44
#define __CAN_BUS_BROKER_HPP__
55

6-
#include <vector>
7-
86
#include <yarp/dev/IMultipleWrapper.h>
97
#include <yarp/dev/ControlBoardInterfaces.h>
108
#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
@@ -416,12 +414,17 @@ class CanBusBroker : public yarp::dev::DeviceDriver,
416414
bool getThreeAxisMagnetometerMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
417415

418416
private:
417+
static SyncPeriodicThread & getSyncThread()
418+
{
419+
static SyncPeriodicThread instance;
420+
return instance;
421+
}
422+
419423
const yarp::dev::PolyDriverDescriptor * tryCreateFakeNode(const yarp::dev::PolyDriverDescriptor * driver);
420424

421425
DeviceMapper deviceMapper;
422-
std::vector<SingleBusBroker *> brokers;
423426
yarp::dev::PolyDriverList fakeNodes;
424-
SyncPeriodicThread * syncThread {nullptr};
427+
SingleBusBroker * broker {nullptr};
425428
};
426429

427430
} // namespace roboticslab

‎libraries/YarpPlugins/CanBusBroker/DeviceDriverImpl.cpp

+34-68
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,6 @@
77
#include <yarp/os/LogStream.h>
88
#include <yarp/os/Value.h>
99

10-
#include "FutureTask.hpp"
1110
#include "ICanBusSharer.hpp"
1211
#include "LogComponent.hpp"
1312

@@ -22,49 +21,40 @@ bool CanBusBroker::open(yarp::os::Searchable & config)
2221
options.fromString(config.findGroup("common").toString(), false); // override global options
2322
options.fromString(config.toString(), false); // override common options
2423

25-
const auto * buses = options.find("buses").asList();
26-
27-
if (!buses)
28-
{
29-
yCError(CBB) << R"(Missing key "buses" or not a list)";
30-
return false;
31-
}
32-
33-
for (int i = 0; i < buses->size(); i++)
24+
if (yarp::os::Value * v; options.check("bus", v, "CAN bus") && v->isString())
3425
{
35-
auto bus = buses->get(i).asString();
26+
auto bus = v->asString();
3627

37-
if (!options.check(bus))
28+
if (yarp::os::Value * vv; options.check("nodes", vv, "CAN nodes") && vv->isList())
3829
{
39-
yCError(CBB) << "Missing CAN bus key:" << bus;
40-
return false;
41-
}
30+
const auto * nodes = vv->asList();
4231

43-
if (!options.find(bus).isList())
44-
{
45-
yCError(CBB) << "Key" << bus << "must be a list";
46-
return false;
47-
}
32+
std::vector<std::string> names;
4833

49-
const auto * nodes = options.find(bus).asList();
34+
for (int j = 0; j < nodes->size(); j++)
35+
{
36+
auto node = nodes->get(j).asString();
37+
names.push_back(node);
38+
}
5039

51-
std::vector<std::string> names;
40+
broker = new SingleBusBroker(bus, names);
5241

53-
for (int j = 0; j < nodes->size(); j++)
54-
{
55-
auto node = nodes->get(j).asString();
56-
names.push_back(node);
42+
if (!broker->configure(options))
43+
{
44+
yCError(CBB) << "Unable to configure broker of CAN bus device" << bus;
45+
return false;
46+
}
5747
}
58-
59-
auto * broker = new SingleBusBroker(bus, names);
60-
brokers.push_back(broker);
61-
62-
if (!broker->configure(options))
48+
else
6349
{
64-
yCError(CBB) << "Unable to configure broker of CAN bus device" << bus;
65-
return false;
50+
yCError(CBB) << R"(Missing key "nodes" or not a list)";
6651
}
6752
}
53+
else
54+
{
55+
yCError(CBB) << R"(Missing key "bus" or not a string)";
56+
return false;
57+
}
6858

6959
if (yarp::os::Value * v; options.check("fakeNodes", v, "fake CAN nodes"))
7060
{
@@ -89,44 +79,26 @@ bool CanBusBroker::open(yarp::os::Searchable & config)
8979
}
9080
}
9181

92-
if (options.check("syncPeriod", "SYNC message period (s)"))
82+
if (yarp::os::Value * v; options.check("syncPeriod", v, "SYNC message period (s)"))
9383
{
94-
auto syncPeriod = options.find("syncPeriod").asFloat64();
84+
auto syncPeriod = v->asFloat64();
9585

9686
if (syncPeriod <= 0.0)
9787
{
9888
yCError(CBB) << "Invalid --syncPeriod:" << syncPeriod;
9989
return false;
10090
}
10191

102-
FutureTaskFactory * taskFactory = nullptr;
92+
auto & syncThread = getSyncThread();
10393

104-
if (brokers.size() > 1)
105-
{
106-
taskFactory = new ParallelTaskFactory(brokers.size());
107-
}
108-
else if (brokers.size() == 1)
109-
{
110-
taskFactory = new SequentialTaskFactory;
111-
}
94+
syncThread.registerBroker(broker);
95+
syncThread.setPeriod(syncPeriod);
11296

113-
if (taskFactory)
97+
if (yarp::os::Value * vv; options.check("syncObserver", vv, "synchronization signal observer") && vv->isBlob())
11498
{
115-
syncThread = new SyncPeriodicThread(brokers, taskFactory); // owns `taskFactory`
116-
syncThread->setPeriod(syncPeriod);
117-
118-
if (!syncThread->openPort("/sync:o"))
119-
{
120-
yCError(CBB) << "Unable to open synchronization port";
121-
return false;
122-
}
123-
124-
if (yarp::os::Value * v; options.check("syncObserver", v, "synchronization signal observer") && v->isBlob())
125-
{
126-
yCDebug(CBB) << "Setting synchronization signal observer";
127-
auto * observer = *reinterpret_cast<TypedStateObserver<double> * const *>(v->asBlob());
128-
syncThread->setObserver(observer);
129-
}
99+
yCDebug(CBB) << "Setting synchronization signal observer";
100+
auto * observer = *reinterpret_cast<TypedStateObserver<double> * const *>(vv->asBlob());
101+
syncThread.setObserver(observer);
130102
}
131103
}
132104
else
@@ -143,18 +115,12 @@ bool CanBusBroker::close()
143115
{
144116
bool ok = detachAll();
145117

146-
if (syncThread)
147-
{
148-
delete syncThread;
149-
syncThread = nullptr;
150-
}
151-
152-
for (auto * broker : brokers)
118+
if (broker)
153119
{
154120
delete broker;
121+
broker = nullptr;
155122
}
156123

157-
brokers.clear();
158124
return ok;
159125
}
160126

‎libraries/YarpPlugins/CanBusBroker/IMultipleWrapperImpl.cpp

+53-55
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22

33
#include "CanBusBroker.hpp"
44

5-
#include <algorithm> // std::any_of, std::find_if, std::for_each
5+
#include <algorithm> // std::any_of, std::find_if
66
#include <iterator> // std::distance
77

88
#include <yarp/os/Bottle.h>
@@ -98,33 +98,23 @@ const yarp::dev::PolyDriverDescriptor * CanBusBroker::tryCreateFakeNode(const ya
9898

9999
bool CanBusBroker::attachAll(const yarp::dev::PolyDriverList & drivers)
100100
{
101-
std::vector<std::string> nodeNames; // flattened broker[i]->getNodeNames()
102-
103-
std::for_each(brokers.begin(), brokers.end(), [&nodeNames](const auto * broker)
104-
{ nodeNames.insert(nodeNames.end(),
105-
broker->getNodeNames().begin(),
106-
broker->getNodeNames().end()); });
107-
108-
std::vector<const yarp::dev::PolyDriverDescriptor *> buses(brokers.size());
101+
const auto & nodeNames = broker->getNodeNames();
102+
const yarp::dev::PolyDriverDescriptor * bus = nullptr;
109103
std::vector<const yarp::dev::PolyDriverDescriptor *> nodes(nodeNames.size());
110104

111105
for (int i = 0; i < drivers.size(); i++)
112106
{
113107
const auto * driver = drivers[i];
114108

115-
if (auto bus = std::find_if(brokers.begin(), brokers.end(), [driver](const auto * broker)
116-
{ return broker->getBusName() == driver->key; });
117-
bus != brokers.end())
109+
if (driver->key == broker->getBusName())
118110
{
119-
int index = std::distance(brokers.begin(), bus);
120-
121-
if (buses[index] != nullptr)
111+
if (bus != nullptr)
122112
{
123113
yCError(CBB) << "Bus device" << driver->key << "already attached";
124114
return false;
125115
}
126116

127-
buses[index] = driver;
117+
bus = driver;
128118
}
129119
else if (auto node = std::find_if(nodeNames.begin(), nodeNames.end(), [driver](const auto & name)
130120
{ return name == driver->key; });
@@ -147,19 +137,9 @@ bool CanBusBroker::attachAll(const yarp::dev::PolyDriverList & drivers)
147137
}
148138
}
149139

150-
if (std::any_of(buses.begin(), buses.end(), [](const auto * bus) { return bus == nullptr; }))
140+
if (bus == nullptr)
151141
{
152-
std::vector<std::string> names;
153-
154-
for (int i = 0; i < buses.size(); i++)
155-
{
156-
if (buses[i] == nullptr)
157-
{
158-
names.push_back(brokers[i]->getBusName());
159-
}
160-
}
161-
162-
yCError(CBB) << "Some bus devices are missing:" << names;
142+
yCError(CBB) << "The bus device is missing:" << broker->getBusName();
163143
return false;
164144
}
165145

@@ -188,18 +168,15 @@ bool CanBusBroker::attachAll(const yarp::dev::PolyDriverList & drivers)
188168
}
189169
}
190170

191-
yCInfo(CBB) << "Attached" << buses.size() << "bus device(s) and" << nodes.size() << "node device(s)";
171+
yCInfo(CBB) << "Attached" << bus->key << "bus device and" << nodes.size() << "node device(s):" << nodeNames;
192172

193-
for (int i = 0; i < buses.size(); i++)
173+
if (!broker->registerDevice(bus->poly))
194174
{
195-
if (!brokers[i]->registerDevice(buses[i]->poly))
196-
{
197-
yCError(CBB) << "Unable to register bus device" << buses[i]->key;
198-
return false;
199-
}
175+
yCError(CBB) << "Unable to register bus device" << bus->key;
176+
return false;
200177
}
201178

202-
yCInfo(CBB) << "Registered bus devices";
179+
yCInfo(CBB) << "Registered bus device";
203180

204181
for (int i = 0; i < nodes.size(); i++)
205182
{
@@ -219,24 +196,16 @@ bool CanBusBroker::attachAll(const yarp::dev::PolyDriverList & drivers)
219196
return false;
220197
}
221198

222-
auto it = std::find_if(brokers.begin(), brokers.end(), [node](const auto * broker)
223-
{ const auto & names = broker->getNodeNames();
224-
return std::find(names.begin(), names.end(), node->key) != names.end(); });
225-
226-
auto * broker = *it;
227199
broker->getReader()->registerHandle(iCanBusSharer);
228200
iCanBusSharer->registerSender(broker->getWriter()->getDelegate());
229201
}
230202

231203
yCInfo(CBB) << "Registered node devices";
232204

233-
for (int i = 0; i < buses.size(); i++)
205+
if (!broker->startThreads())
234206
{
235-
if (!brokers[i]->startThreads())
236-
{
237-
yCError(CBB) << "Unable to start CAN threads in" << brokers[i]->getBusName();
238-
return false;
239-
}
207+
yCError(CBB) << "Unable to start CAN threads in" << broker->getBusName();
208+
return false;
240209
}
241210

242211
yCInfo(CBB) << "Started CAN threads";
@@ -251,15 +220,31 @@ bool CanBusBroker::attachAll(const yarp::dev::PolyDriverList & drivers)
251220
}
252221
}
253222

223+
broker->markInitialized(true);
224+
254225
yCInfo(CBB) << "Initialized node devices";
255226

256-
if (syncThread && !syncThread->isRunning() && !syncThread->start())
227+
if (auto & syncThread = getSyncThread(); !syncThread.getBrokers().empty())
257228
{
258-
yCError(CBB) << "Unable to start synchronization thread";
259-
return false;
260-
}
229+
if (!syncThread.openPort("/sync:o"))
230+
{
231+
yCError(CBB) << "Unable to open synchronization port";
232+
return false;
233+
}
261234

262-
yCInfo(CBB) << "Started synchronization thread";
235+
if (!syncThread.isRunning())
236+
{
237+
if (!syncThread.start())
238+
{
239+
yCError(CBB) << "Unable to start synchronization thread";
240+
return false;
241+
}
242+
else
243+
{
244+
yCInfo(CBB) << "Started synchronization thread";
245+
}
246+
}
247+
}
263248

264249
return true;
265250
}
@@ -270,30 +255,43 @@ bool CanBusBroker::detachAll()
270255
{
271256
bool ok = true;
272257

273-
if (syncThread && syncThread->isRunning())
258+
if (broker)
274259
{
275-
syncThread->stop();
260+
broker->markInitialized(false);
261+
}
262+
263+
if (auto & syncThread = getSyncThread(); syncThread.isRunning())
264+
{
265+
syncThread.stop();
266+
syncThread.closePort();
267+
268+
yCInfo(CBB) << "Stopped synchronization thread";
276269
}
277270

278271
for (const auto & rawDevice : deviceMapper.getDevices())
279272
{
280273
auto * iCanBusSharer = rawDevice->castToType<ICanBusSharer>();
274+
yCDebug(CBB) << "Finalizing node device id" << iCanBusSharer->getId();
281275

282276
if (iCanBusSharer && !iCanBusSharer->finalize())
283277
{
284278
yCWarning(CBB) << "Node device id" << iCanBusSharer->getId() << "could not finalize CAN comms";
285279
ok = false;
286280
}
281+
282+
yCDebug(CBB) << "Unregistering node device id" << iCanBusSharer->getId();
287283
}
288284

289285
deviceMapper.clear();
290286

291-
for (auto * broker : brokers)
287+
if (broker)
292288
{
293289
ok &= broker->stopThreads();
294290
ok &= broker->clearFilters();
295291
}
296292

293+
yCDebug(CBB) << "Stopped CAN threads";
294+
297295
for (int i = 0; i < fakeNodes.size(); i++)
298296
{
299297
if (fakeNodes[i]->poly && fakeNodes[i]->poly->isValid())

0 commit comments

Comments
 (0)