From 4fdfc6bff26e1ab4746918868716660e2b125bf6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Fri, 29 Nov 2019 19:04:28 +0100 Subject: [PATCH 1/3] Propagate robot config options via separate property --- .../YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp | 8 ++++---- libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp | 4 ++-- programs/launchCanBus/LaunchCanBus.cpp | 8 ++++---- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp b/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp index c033d289f..9e240ed57 100644 --- a/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp @@ -46,8 +46,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()); @@ -114,8 +114,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/TechnosoftIpos/DeviceDriverImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp index 17d28e602..9c37fb94a 100644 --- a/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp @@ -61,8 +61,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)) diff --git a/programs/launchCanBus/LaunchCanBus.cpp b/programs/launchCanBus/LaunchCanBus.cpp index 094b5c0a8..6f275a27e 100644 --- a/programs/launchCanBus/LaunchCanBus.cpp +++ b/programs/launchCanBus/LaunchCanBus.cpp @@ -74,8 +74,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", robotConfig.toString()); canDeviceOptions.put("home", homing); yarp::dev::PolyDriver * canDevice = new yarp::dev::PolyDriver; @@ -138,8 +138,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", robotConfig.toString()); calibratorDeviceOptions.put("joints", wrapperDeviceOptions.find("joints")); yarp::dev::PolyDriver * calibratorDevice = new yarp::dev::PolyDriver; From d73008f1511f1f5887941029779dceb760e00385 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Wed, 11 Dec 2019 02:26:57 +0100 Subject: [PATCH 2/3] Propagate robot config dictionary as blob --- .../CanBusControlboard/CanRxTxThreads.hpp | 2 +- .../CanBusControlboard/DeviceDriverImpl.cpp | 12 ++++++++++-- .../CanBusHico/DeviceDriverImpl.cpp | 2 ++ .../CanBusPeak/DeviceDriverImpl.cpp | 2 ++ .../CuiAbsolute/DeviceDriverImpl.cpp | 2 +- .../DextraCanControlboard/DeviceDriverImpl.cpp | 2 ++ .../JointCalibrator/DeviceDriverImpl.cpp | 2 +- .../LacqueyFetch/DeviceDriverImpl.cpp | 2 ++ .../TechnosoftIpos/DeviceDriverImpl.cpp | 18 +++++++++++++----- programs/launchCanBus/LaunchCanBus.cpp | 6 ++++-- 10 files changed, 38 insertions(+), 12 deletions(-) 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 9e240ed57..d105b061b 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()) { @@ -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()) { 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 9c37fb94a..c46a48c30 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()) { diff --git a/programs/launchCanBus/LaunchCanBus.cpp b/programs/launchCanBus/LaunchCanBus.cpp index 6f275a27e..9bb25c7e0 100644 --- a/programs/launchCanBus/LaunchCanBus.cpp +++ b/programs/launchCanBus/LaunchCanBus.cpp @@ -42,6 +42,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(); @@ -75,7 +77,7 @@ bool LaunchCanBus::configure(yarp::os::ResourceFinder &rf) yarp::os::Property canDeviceOptions; canDeviceOptions.fromString(canDeviceGroup.toString()); - canDeviceOptions.put("robotConfig", robotConfig.toString()); + canDeviceOptions.put("robotConfig", yarp::os::Value::makeBlob(&robotConfig, sizeof(robotConfig))); canDeviceOptions.put("home", homing); yarp::dev::PolyDriver * canDevice = new yarp::dev::PolyDriver; @@ -139,7 +141,7 @@ bool LaunchCanBus::configure(yarp::os::ResourceFinder &rf) yarp::os::Property calibratorDeviceOptions; calibratorDeviceOptions.fromString(calibratorDeviceGroup.toString()); - calibratorDeviceOptions.put("robotConfig", robotConfig.toString()); + calibratorDeviceOptions.put("robotConfig", yarp::os::Value::makeBlob(&robotConfig, sizeof(robotConfig))); calibratorDeviceOptions.put("joints", wrapperDeviceOptions.find("joints")); yarp::dev::PolyDriver * calibratorDevice = new yarp::dev::PolyDriver; From a4bac7647892feed4cdff99613c8106d753a292f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Wed, 11 Dec 2019 13:33:45 +0100 Subject: [PATCH 3/3] Copy and pass pointer instead of full object --- .../YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp | 2 +- libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp | 4 +++- programs/launchCanBus/LaunchCanBus.cpp | 5 +++-- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp b/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp index d105b061b..8e98edd14 100644 --- a/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp @@ -23,7 +23,7 @@ bool CanBusControlboard::open(yarp::os::Searchable & config) return false; } - const auto * robotConfig = reinterpret_cast(config.find("robotConfig").asBlob()); + const auto * robotConfig = *reinterpret_cast(config.find("robotConfig").asBlob()); int parallelCanThreadLimit = config.check("parallelCanThreadLimit", yarp::os::Value(0), "parallel CAN TX thread limit").asInt32(); diff --git a/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp index c46a48c30..4b9ba8909 100644 --- a/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp @@ -22,7 +22,7 @@ bool TechnosoftIpos::open(yarp::os::Searchable & config) return false; } - const auto * robotConfig = reinterpret_cast(config.find("robotConfig").asBlob()); + const auto * robotConfig = *reinterpret_cast(config.find("robotConfig").asBlob()); int canId = config.check("canId", yarp::os::Value(0), "CAN node ID").asInt32(); @@ -92,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 9bb25c7e0..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)) @@ -77,7 +78,7 @@ bool LaunchCanBus::configure(yarp::os::ResourceFinder &rf) yarp::os::Property canDeviceOptions; canDeviceOptions.fromString(canDeviceGroup.toString()); - canDeviceOptions.put("robotConfig", yarp::os::Value::makeBlob(&robotConfig, sizeof(robotConfig))); + canDeviceOptions.put("robotConfig", yarp::os::Value::makeBlob(&robotConfigPtr, sizeof(robotConfigPtr))); canDeviceOptions.put("home", homing); yarp::dev::PolyDriver * canDevice = new yarp::dev::PolyDriver; @@ -141,7 +142,7 @@ bool LaunchCanBus::configure(yarp::os::ResourceFinder &rf) yarp::os::Property calibratorDeviceOptions; calibratorDeviceOptions.fromString(calibratorDeviceGroup.toString()); - calibratorDeviceOptions.put("robotConfig", yarp::os::Value::makeBlob(&robotConfig, sizeof(robotConfig))); + calibratorDeviceOptions.put("robotConfig", yarp::os::Value::makeBlob(&robotConfigPtr, sizeof(robotConfigPtr))); calibratorDeviceOptions.put("joints", wrapperDeviceOptions.find("joints")); yarp::dev::PolyDriver * calibratorDevice = new yarp::dev::PolyDriver;