Merge branch 'fix-yarp-property' into apocanlypse

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<double>::min() : delay; }
+    { this->delay = delay <= 0.0 ? std::numeric_limits<double>::min() : delay; }
 
 protected:
     yarp::dev::ICanBus * iCanBus;

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<yarp::os::Property * const *>(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;

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();
 

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();

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();

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)

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();
 

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;

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<const yarp::os::Property * const *>(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();

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;