diff --git a/libraries/YarpPlugins/CanBusControlboard/CanRxTxThreads.hpp b/libraries/YarpPlugins/CanBusControlboard/CanRxTxThreads.hpp index ce634f6d1..f6f19dd50 100644 --- a/libraries/YarpPlugins/CanBusControlboard/CanRxTxThreads.hpp +++ b/libraries/YarpPlugins/CanBusControlboard/CanRxTxThreads.hpp @@ -48,7 +48,7 @@ class CanReaderWriterThread : public yarp::os::Thread { this->iCanBus = iCanBus; this->iCanBufferFactory = iCanBufferFactory; this->bufferSize = bufferSize; } void setDelay(double delay) - { this->delay = delay <= 0 ? std::numeric_limits::min() : delay; } + { this->delay = delay <= 0.0 ? std::numeric_limits::min() : delay; } protected: yarp::dev::ICanBus * iCanBus; diff --git a/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp b/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp index c033d289f..8e98edd14 100644 --- a/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp @@ -17,6 +17,14 @@ bool CanBusControlboard::open(yarp::os::Searchable & config) { CD_DEBUG("%s\n", config.toString().c_str()); + if (!config.check("robotConfig") || !config.find("robotConfig").isBlob()) + { + CD_ERROR("Missing \"robotConfig\" property or not a blob.\n"); + return false; + } + + const auto * robotConfig = *reinterpret_cast(config.find("robotConfig").asBlob()); + int parallelCanThreadLimit = config.check("parallelCanThreadLimit", yarp::os::Value(0), "parallel CAN TX thread limit").asInt32(); if (parallelCanThreadLimit > 0) @@ -37,7 +45,7 @@ bool CanBusControlboard::open(yarp::os::Searchable & config) for (int i = 0; i < canBuses->size(); i++) { std::string canBus = canBuses->get(i).asString(); - yarp::os::Bottle & canBusGroup = config.findGroup(canBus); + yarp::os::Bottle & canBusGroup = robotConfig->findGroup(canBus); if (canBusGroup.isNull()) { @@ -46,8 +54,8 @@ bool CanBusControlboard::open(yarp::os::Searchable & config) } yarp::os::Property canBusOptions; - canBusOptions.fromString(canBusGroup.toString(), false); - canBusOptions.fromString(config.toString(), false); + canBusOptions.fromString(canBusGroup.toString()); + canBusOptions.put("robotConfig", config.find("robotConfig")); canBusOptions.put("canBlockingMode", false); // enforce non-blocking mode canBusOptions.put("canAllowPermissive", false); // always check usage requirements canBusOptions.setMonitor(config.getMonitor(), canBus.c_str()); @@ -105,7 +113,7 @@ bool CanBusControlboard::open(yarp::os::Searchable & config) for (int i = 0; i < nodes->size(); i++) { std::string node = nodes->get(i).asString(); - yarp::os::Bottle & nodeGroup = config.findGroup(node); + yarp::os::Bottle & nodeGroup = robotConfig->findGroup(node); if (nodeGroup.isNull()) { @@ -114,8 +122,8 @@ bool CanBusControlboard::open(yarp::os::Searchable & config) } yarp::os::Property nodeOptions; - nodeOptions.fromString(nodeGroup.toString(), false); - nodeOptions.fromString(config.toString(), false); + nodeOptions.fromString(nodeGroup.toString()); + nodeOptions.put("robotConfig", config.find("robotConfig")); nodeOptions.setMonitor(config.getMonitor(), node.c_str()); yarp::dev::PolyDriver * device = new yarp::dev::PolyDriver; diff --git a/libraries/YarpPlugins/CanBusHico/DeviceDriverImpl.cpp b/libraries/YarpPlugins/CanBusHico/DeviceDriverImpl.cpp index abe07159e..123cd30e7 100644 --- a/libraries/YarpPlugins/CanBusHico/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/CanBusHico/DeviceDriverImpl.cpp @@ -19,6 +19,8 @@ bool roboticslab::CanBusHico::open(yarp::os::Searchable& config) { + CD_DEBUG("%s\n", config.toString().c_str()); + std::string devicePath = config.check("port", yarp::os::Value(DEFAULT_PORT), "CAN device path").asString(); int bitrate = config.check("bitrate", yarp::os::Value(DEFAULT_BITRATE), "CAN bitrate (bps)").asInt32(); diff --git a/libraries/YarpPlugins/CanBusPeak/DeviceDriverImpl.cpp b/libraries/YarpPlugins/CanBusPeak/DeviceDriverImpl.cpp index c97d6b35b..ae481aa5e 100644 --- a/libraries/YarpPlugins/CanBusPeak/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/CanBusPeak/DeviceDriverImpl.cpp @@ -12,6 +12,8 @@ bool roboticslab::CanBusPeak::open(yarp::os::Searchable& config) { + CD_DEBUG("%s\n", config.toString().c_str()); + std::string devicePath = config.check("port", yarp::os::Value(DEFAULT_PORT), "CAN device path").asString(); int bitrate = config.check("bitrate", yarp::os::Value(DEFAULT_BITRATE), "CAN bitrate (bps)").asInt32(); diff --git a/libraries/YarpPlugins/CuiAbsolute/DeviceDriverImpl.cpp b/libraries/YarpPlugins/CuiAbsolute/DeviceDriverImpl.cpp index e539b2aa8..d46edba6b 100644 --- a/libraries/YarpPlugins/CuiAbsolute/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/CuiAbsolute/DeviceDriverImpl.cpp @@ -14,7 +14,7 @@ using namespace roboticslab; bool CuiAbsolute::open(yarp::os::Searchable& config) { - CD_DEBUG("%s.\n", config.toString().c_str()); + CD_DEBUG("%s\n", config.toString().c_str()); canId = config.check("canId", yarp::os::Value(0), "CAN bus ID").asInt8(); reverse = config.check("reverse", yarp::os::Value(false), "reverse").asBool(); diff --git a/libraries/YarpPlugins/DextraCanControlboard/DeviceDriverImpl.cpp b/libraries/YarpPlugins/DextraCanControlboard/DeviceDriverImpl.cpp index 119b68a03..77e48f9cb 100644 --- a/libraries/YarpPlugins/DextraCanControlboard/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/DextraCanControlboard/DeviceDriverImpl.cpp @@ -12,6 +12,8 @@ using namespace roboticslab; bool DextraCanControlboard::open(yarp::os::Searchable & config) { + CD_DEBUG("%s\n", config.toString().c_str()); + canId = config.check("canId", yarp::os::Value(0), "can bus ID").asInt32(); if (canId == 0) diff --git a/libraries/YarpPlugins/JointCalibrator/DeviceDriverImpl.cpp b/libraries/YarpPlugins/JointCalibrator/DeviceDriverImpl.cpp index 652ee14bc..11a657ef5 100644 --- a/libraries/YarpPlugins/JointCalibrator/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/JointCalibrator/DeviceDriverImpl.cpp @@ -36,7 +36,7 @@ namespace bool JointCalibrator::open(yarp::os::Searchable & config) { - CD_DEBUG("$s\n", config.toString().c_str()); + CD_DEBUG("%s\n", config.toString().c_str()); axes = config.check("joints", yarp::os::Value(0), "number of controlled axes").asInt32(); diff --git a/libraries/YarpPlugins/LacqueyFetch/DeviceDriverImpl.cpp b/libraries/YarpPlugins/LacqueyFetch/DeviceDriverImpl.cpp index de930ffa6..431a88e66 100644 --- a/libraries/YarpPlugins/LacqueyFetch/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/LacqueyFetch/DeviceDriverImpl.cpp @@ -10,6 +10,8 @@ using namespace roboticslab; bool LacqueyFetch::open(yarp::os::Searchable& config) { + CD_DEBUG("%s\n", config.toString().c_str()); + canId = config.check("canId", yarp::os::Value(0), "can bus ID").asInt8(); CD_SUCCESS("Created LacqueyFetch with canId %d.\n", canId); return true; diff --git a/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp index 17d28e602..4b9ba8909 100644 --- a/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp @@ -16,12 +16,20 @@ bool TechnosoftIpos::open(yarp::os::Searchable & config) { CD_DEBUG("%s\n", config.toString().c_str()); + if (!config.check("robotConfig") || !config.find("robotConfig").isBlob()) + { + CD_ERROR("Missing \"robotConfig\" property or not a blob.\n"); + return false; + } + + const auto * robotConfig = *reinterpret_cast(config.find("robotConfig").asBlob()); + int canId = config.check("canId", yarp::os::Value(0), "CAN node ID").asInt32(); - yarp::os::Bottle & driverGroup = config.findGroup("driver"); - yarp::os::Bottle & motorGroup = config.findGroup("motor"); - yarp::os::Bottle & gearboxGroup = config.findGroup("gearbox"); - yarp::os::Bottle & encoderGroup = config.findGroup("encoder"); + yarp::os::Bottle & driverGroup = robotConfig->findGroup(config.find("driver").asString()); + yarp::os::Bottle & motorGroup = robotConfig->findGroup(config.find("motor").asString()); + yarp::os::Bottle & gearboxGroup = robotConfig->findGroup(config.find("gearbox").asString()); + yarp::os::Bottle & encoderGroup = robotConfig->findGroup(config.find("encoder").asString()); // mutable variables vars.tr = gearboxGroup.check("tr", yarp::os::Value(0.0), "reduction").asFloat64(); @@ -52,7 +60,7 @@ bool TechnosoftIpos::open(yarp::os::Searchable & config) if (config.check("externalEncoder", "external encoder")) { std::string externalEncoder = config.find("externalEncoder").asString(); - yarp::os::Bottle & externalEncoderGroup = config.findGroup(externalEncoder); + yarp::os::Bottle & externalEncoderGroup = robotConfig->findGroup(externalEncoder); if (externalEncoderGroup.isNull()) { @@ -61,8 +69,8 @@ bool TechnosoftIpos::open(yarp::os::Searchable & config) } yarp::os::Property externalEncoderOptions; - externalEncoderOptions.fromString(externalEncoderGroup.toString(), false); - externalEncoderOptions.fromString(config.toString(), false); + externalEncoderOptions.fromString(externalEncoderGroup.toString()); + externalEncoderOptions.put("robotConfig", config.find("robotConfig")); externalEncoderOptions.setMonitor(config.getMonitor(), externalEncoder.c_str()); if (!externalEncoderDevice.open(externalEncoderOptions)) @@ -84,12 +92,14 @@ bool TechnosoftIpos::open(yarp::os::Searchable & config) } } + /* FIXME linInterpBuffer = LinearInterpolationBuffer::createBuffer(config); if (!linInterpBuffer) { return false; } + */ double canSdoTimeoutMs = config.check("canSdoTimeoutMs", yarp::os::Value(0.0), "CAN SDO timeout (ms)").asFloat64(); double canDriveStateTimeout = config.check("canDriveStateTimeout", yarp::os::Value(0.0), "CAN drive state timeout (s)").asFloat64(); diff --git a/programs/launchCanBus/LaunchCanBus.cpp b/programs/launchCanBus/LaunchCanBus.cpp index 094b5c0a8..836763389 100644 --- a/programs/launchCanBus/LaunchCanBus.cpp +++ b/programs/launchCanBus/LaunchCanBus.cpp @@ -34,6 +34,7 @@ bool LaunchCanBus::configure(yarp::os::ResourceFinder &rf) } yarp::os::Property robotConfig; + const auto * robotConfigPtr = &robotConfig; std::string configPath = rf.findFileByName("config.ini"); if (configPath.empty() || !robotConfig.fromConfigFile(configPath)) @@ -42,6 +43,8 @@ bool LaunchCanBus::configure(yarp::os::ResourceFinder &rf) return false; } + CD_DEBUG("%s\n", robotConfig.toString().c_str()); + yarp::conf::vocab32_t mode = rf.check("mode", yarp::os::Value(VOCAB_CM_POSITION), "initial mode of operation").asVocab(); bool homing = rf.check("home", yarp::os::Value(false), "perform initial homing procedure").asBool(); @@ -74,8 +77,8 @@ bool LaunchCanBus::configure(yarp::os::ResourceFinder &rf) } yarp::os::Property canDeviceOptions; - canDeviceOptions.fromString(canDeviceGroup.toString(), false); - canDeviceOptions.fromString(robotConfig.toString(), false); + canDeviceOptions.fromString(canDeviceGroup.toString()); + canDeviceOptions.put("robotConfig", yarp::os::Value::makeBlob(&robotConfigPtr, sizeof(robotConfigPtr))); canDeviceOptions.put("home", homing); yarp::dev::PolyDriver * canDevice = new yarp::dev::PolyDriver; @@ -138,8 +141,8 @@ bool LaunchCanBus::configure(yarp::os::ResourceFinder &rf) } yarp::os::Property calibratorDeviceOptions; - calibratorDeviceOptions.fromString(calibratorDeviceGroup.toString(), false); - calibratorDeviceOptions.fromString(robotConfig.toString(), false); + calibratorDeviceOptions.fromString(calibratorDeviceGroup.toString()); + calibratorDeviceOptions.put("robotConfig", yarp::os::Value::makeBlob(&robotConfigPtr, sizeof(robotConfigPtr))); calibratorDeviceOptions.put("joints", wrapperDeviceOptions.find("joints")); yarp::dev::PolyDriver * calibratorDevice = new yarp::dev::PolyDriver;