Skip to content

Commit

Permalink
Implement attach logic in CanBusBroker
Browse files Browse the repository at this point in the history
  • Loading branch information
@PeterBowman
PeterBowman committed Jan 13, 2024
1 parent e1bf8c4 commit 479b218
Show file tree
Hide file tree
Showing 6 changed files with 263 additions and 231 deletions.
1 change: 1 addition & 0 deletions libraries/YarpPlugins/CanBusBroker/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ if(NOT SKIP_CanBusBroker)

yarp_add_plugin(CanBusBroker CanBusBroker.hpp
DeviceDriverImpl.cpp
IMultipleWrapperImpl.cpp
# control board interfaces
motor/IAmplifierControlImpl.cpp
motor/IAxisInfoImpl.cpp
Expand Down
13 changes: 8 additions & 5 deletions libraries/YarpPlugins/CanBusBroker/CanBusBroker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

#include <vector>

#include <yarp/dev/IMultipleWrapper.h>
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>

Expand All @@ -25,7 +26,7 @@ namespace roboticslab

/**
* @ingroup CanBusBroker
* @brief CAN-oriented control board that implements all YARP motor interfaces.
* @brief CAN-oriented control board that implements all YARP motor and MAS interfaces.
*
* This control board wrapper subdevice exposes motor commands to CAN nodes
* modelled as wrapped motor raw subdevices (i.e. devices which implement the
Expand All @@ -38,6 +39,7 @@ namespace roboticslab
* <a href="https://github.com/roboticslab-uc3m/yarp-devices/issues/241#issuecomment-569112698">instructions</a>.
*/
class CanBusBroker : public yarp::dev::DeviceDriver,
public yarp::dev::IMultipleWrapper,
// control board interfaces
public yarp::dev::IAmplifierControl,
public yarp::dev::IAxisInfo,
Expand Down Expand Up @@ -79,6 +81,11 @@ class CanBusBroker : public yarp::dev::DeviceDriver,
bool open(yarp::os::Searchable & config) override;
bool close() override;

// -------- IMultipleWrapper declarations. Implementation in IMultipleWrapperImpl.cpp --------

bool attachAll(const yarp::dev::PolyDriverList & drivers) override;
bool detachAll() override;

// ---------- CONTROL BOARD INTERFACES ----------

// --------- IAmplifierControl declarations. Implementation in IAmplifierControlImpl.cpp ---------
Expand Down Expand Up @@ -409,11 +416,7 @@ class CanBusBroker : public yarp::dev::DeviceDriver,

private:
DeviceMapper deviceMapper;

std::vector<yarp::dev::PolyDriver *> busDevices;
std::vector<yarp::dev::PolyDriver *> nodeDevices;
std::vector<SingleBusBroker *> brokers;

SyncPeriodicThread * syncThread {nullptr};
};

Expand Down
223 changes: 22 additions & 201 deletions libraries/YarpPlugins/CanBusBroker/DeviceDriverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,9 @@
#include "CanBusBroker.hpp"

#include <yarp/os/LogStream.h>
#include <yarp/os/Property.h>
#include <yarp/os/Value.h>

#include "FutureTask.hpp"
#include "ICanBusSharer.hpp"
#include "LogComponent.hpp"

Expand All @@ -15,188 +15,50 @@ using namespace roboticslab;

bool CanBusBroker::open(yarp::os::Searchable & config)
{
if (!config.check("robotConfig") || !config.find("robotConfig").isBlob())
{
yCError(CBB) << "Missing \"robotConfig\" property or not a blob";
return false;
}
const auto * buses = config.find("buses").asList();

const auto * robotConfig = *reinterpret_cast<yarp::os::Property * const *>(config.find("robotConfig").asBlob());

const auto * canBuses = config.find("buses").asList();

if (!canBuses)
if (!buses)
{
yCError(CBB) << "Missing key \"buses\" or not a list";
return false;
}

for (int i = 0; i < canBuses->size(); i++)
for (int i = 0; i < buses->size(); i++)
{
auto canBus = canBuses->get(i).asString();
auto bus = buses->get(i).asString();

if (!config.check(canBus))
if (!config.check(bus))
{
yCError(CBB) << "Missing CAN bus key:" << canBus;
yCError(CBB) << "Missing CAN bus key:" << bus;
return false;
}

const auto & nodesVal = config.find(canBus);

if (!nodesVal.isList())
if (!config.find(bus).isList())
{
yCError(CBB) << "Key" << canBus << "must be a list";
yCError(CBB) << "Key" << bus << "must be a list";
return false;
}

const auto * nodes = nodesVal.asList();
auto isFakeBus = canBus.rfind("fake-", 0) == 0; // starts with "fake-"?
std::vector<ICanBusSharer *> busSharers;
yarp::os::Bottle nodeIds;
const auto * nodes = config.find(bus).asList();

std::vector<std::string> names;

for (int j = 0; j < nodes->size(); j++)
{
auto node = nodes->get(j).asString();
auto isFakeNode = node.rfind("fake-", 0) == 0; // starts with "fake-"?

yarp::os::Property nodeOptions;
nodeOptions.setMonitor(config.getMonitor(), node.c_str());

if (!isFakeNode)
{
const auto & nodeGroup = robotConfig->findGroup(node);

if (nodeGroup.isNull())
{
yCError(CBB) << "Missing CAN node device group" << node;
return false;
}

nodeOptions.fromString(nodeGroup.toString());
nodeOptions.put("robotConfig", config.find("robotConfig"));

if (config.check("syncPeriod"))
{
nodeOptions.put("syncPeriod", config.find("syncPeriod"));
}
}
else
{
nodeOptions.put("device", "FakeJoint");
nodeOptions.put("jointName", node.substr(5));
}

auto * device = new yarp::dev::PolyDriver;
nodeDevices.push_back(device);

if (!device->open(nodeOptions))
{
yCError(CBB) << "CAN node device" << node << "configuration failure";
return false;
}

if (!deviceMapper.registerDevice(device))
{
yCError(CBB) << "Unable to register CAN node device" << node;
return false;
}

if (!isFakeBus && !isFakeNode)
{
ICanBusSharer * iCanBusSharer;

if (!device->view(iCanBusSharer))
{
yCError(CBB) << "Unable to view ICanBusSharer in" << node;
return false;
}

busSharers.push_back(iCanBusSharer);
nodeIds.addInt32(iCanBusSharer->getId());

for (auto id : iCanBusSharer->getAdditionalIds())
{
nodeIds.addInt32(id);
}
}
names.push_back(node);
}

if (!isFakeBus)
{
const auto & canBusGroup = robotConfig->findGroup(canBus);

if (canBusGroup.isNull())
{
yCError(CBB) << "Missing CAN bus device group" << canBus;
return false;
}

yarp::os::Property canBusOptions;
canBusOptions.setMonitor(config.getMonitor(), canBus.c_str());
canBusOptions.fromString(canBusGroup.toString());
canBusOptions.put("robotConfig", config.find("robotConfig"));
canBusOptions.put("blockingMode", false); // enforce non-blocking mode
canBusOptions.put("allowPermissive", false); // always check usage requirements

auto enableAcceptanceFilters = canBusOptions.check("enableAcceptanceFilters", yarp::os::Value(false),
"enable CAN acceptance filters").asBool();

if (enableAcceptanceFilters && nodeIds.size() != 0)
{
canBusOptions.put("filteredIds", yarp::os::Value::makeList(nodeIds.toString().c_str()));
}

auto * canBusDevice = new yarp::dev::PolyDriver;
busDevices.push_back(canBusDevice);

if (!canBusDevice->open(canBusOptions))
{
yCError(CBB) << "canBusDevice instantiation failed:" << canBus;
return false;
}

auto * broker = new SingleBusBroker(canBus);
brokers.push_back(broker);

if (!broker->configure(canBusOptions))
{
yCError(CBB) << "Unable to configure broker of CAN bus device" << canBus;
return false;
}

if (!broker->registerDevice(canBusDevice))
{
yCError(CBB) << "Unable to register CAN bus device" << canBus;
return false;
}

for (auto * iCanBusSharer : busSharers)
{
broker->getReader()->registerHandle(iCanBusSharer);
iCanBusSharer->registerSender(broker->getWriter()->getDelegate());
}
}
}
auto * broker = new SingleBusBroker(bus, names);
brokers.push_back(broker);

for (auto * broker : brokers)
{
if (!broker->startThreads())
if (!broker->configure(config))
{
yCError(CBB) << "Unable to start CAN threads in" << broker->getName();
yCError(CBB) << "Unable to configure broker of CAN bus device" << bus;
return false;
}
}

for (const auto & rawDevice : deviceMapper.getDevices())
{
auto * iCanBusSharer = rawDevice->castToType<ICanBusSharer>();

if (iCanBusSharer && !iCanBusSharer->initialize())
{
yCError(CBB) << "Node device id" << iCanBusSharer->getId() << "could not initialize CAN comms";
}
}

if (config.check("syncPeriod", "SYNC message period (s)"))
{
auto syncPeriod = config.find("syncPeriod").asFloat64();
Expand All @@ -209,11 +71,11 @@ bool CanBusBroker::open(yarp::os::Searchable & config)

FutureTaskFactory * taskFactory = nullptr;

if (busDevices.size() > 1)
if (brokers.size() > 1)
{
taskFactory = new ParallelTaskFactory(busDevices.size());
taskFactory = new ParallelTaskFactory(brokers.size());
}
else if (busDevices.size() == 1)
else if (brokers.size() == 1)
{
taskFactory = new SequentialTaskFactory;
}
Expand Down Expand Up @@ -243,62 +105,21 @@ bool CanBusBroker::open(yarp::os::Searchable & config)
yCWarning(CBB) << "Synchronization thread not enabled, use --syncPeriod";
}

return !syncThread || syncThread->start();
return true;
}

// -----------------------------------------------------------------------------

bool CanBusBroker::close()
{
bool ok = true;

if (syncThread && syncThread->isRunning())
{
syncThread->stop();
}

delete syncThread;
syncThread = nullptr;

for (const auto & rawDevice : deviceMapper.getDevices())
{
auto * iCanBusSharer = rawDevice->castToType<ICanBusSharer>();

if (iCanBusSharer && !iCanBusSharer->finalize())
{
yCWarning(CBB) << "Node device id" << iCanBusSharer->getId() << "could not finalize CAN comms";
ok = false;
}
}

deviceMapper.clear();
bool ok = detachAll();

for (auto * broker : brokers)
{
ok &= broker->stopThreads();
ok &= broker->clearFilters();
delete broker;
}

brokers.clear();

for (auto * device : nodeDevices)
{
// CAN read threads must not live beyond this point.
ok &= device->close();
delete device;
}

nodeDevices.clear();

for (auto * device : busDevices)
{
ok &= device->close();
delete device;
}

busDevices.clear();

return ok;
}

Expand Down
Loading

0 comments on commit 479b218

Please sign in to comment.