Merge branch 'new-inis' into apocanlypse

libraries/YarpPlugins/CanBusControlboard/CanBusControlboard.hpp

@@ -10,18 +10,13 @@
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/dev/PolyDriverList.h>
 
-#include <ColorDebug.h>
-
 #include "ICanBusSharer.hpp"
 #include "PositionDirectThread.hpp"
 #include "DeviceMapper.hpp"
 #include "CanRxTxThreads.hpp"
 
 #define CHECK_JOINT(j) do { int n = deviceMapper.getControlledAxes(); if ((j) < 0 || (j) > n - 1) return false; } while (0)
 
-#define DEFAULT_MODE "position"
-#define DEFAULT_CUI_TIMEOUT 1.0
-#define DEFAULT_CAN_BUS "CanBusHico"
 #define DEFAULT_LIN_INTERP_PERIOD_MS 50
 #define DEFAULT_LIN_INTERP_BUFFER_SIZE 1
 #define DEFAULT_LIN_INTERP_MODE "pt"
@@ -30,8 +25,8 @@
 #define DEFAULT_CAN_TX_BUFFER_SIZE 500
 #define DEFAULT_CAN_RX_PERIOD_MS -1
 #define DEFAULT_CAN_TX_PERIOD_MS 1.0
-#define DEFAULT_CAN_SDO_TIMEOUT_MS 25.0
-#define DEFAULT_CAN_DRIVE_STATE_TIMEOUT 2.5
+#define DEFAULT_CAN_SDO_TIMEOUT_MS 25.0 // FIXME unused
+#define DEFAULT_CAN_DRIVE_STATE_TIMEOUT 2.5 // FIXME unused
 
 namespace roboticslab
 {
@@ -322,7 +317,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver,
  yarp::dev::PolyDriver canBusDevice;
  yarp::dev::ICanBus * iCanBus;
 
- yarp::dev::PolyDriverList nodes;
+ yarp::dev::PolyDriverList nodeDevices;
 
  DeviceMapper deviceMapper;
  std::vector<ICanBusSharer *> iCanBusSharers;

libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp

@@ -2,56 +2,47 @@
 
 #include "CanBusControlboard.hpp"
 
-#include <map>
-#include <string>
+#include <yarp/os/Property.h>
+
+#include <ColorDebug.h>
 
 using namespace roboticslab;
 
 // -----------------------------------------------------------------------------
 
 bool CanBusControlboard::open(yarp::os::Searchable & config)
 {
- int cuiTimeout = config.check("waitEncoder", yarp::os::Value(DEFAULT_CUI_TIMEOUT), "CUI timeout (seconds)").asInt32();
+ CD_DEBUG("%s\n", config.toString().c_str());
 
- std::string canBusType = config.check("canBusType", yarp::os::Value(DEFAULT_CAN_BUS), "CAN bus device name").asString();
- int canRxBufferSize = config.check("canBusRxBufferSize", yarp::os::Value(DEFAULT_CAN_RX_BUFFER_SIZE), "CAN bus RX buffer size").asInt();
- int canTxBufferSize = config.check("canBusTxBufferSize", yarp::os::Value(DEFAULT_CAN_TX_BUFFER_SIZE), "CAN bus TX buffer size").asInt();
- double canRxPeriodMs = config.check("canRxPeriodMs", yarp::os::Value(DEFAULT_CAN_RX_PERIOD_MS), "CAN bus RX period (milliseconds)").asFloat64();
- double canTxPeriodMs = config.check("canTxPeriodMs", yarp::os::Value(DEFAULT_CAN_TX_PERIOD_MS), "CAN bus TX period (milliseconds)").asFloat64();
- double canSdoTimeoutMs = config.check("canSdoTimeoutMs", yarp::os::Value(DEFAULT_CAN_SDO_TIMEOUT_MS), "CAN bus SDO timeout (milliseconds)").asFloat64();
- double canDriveStateTimeout = config.check("canDriveStateTimeout", yarp::os::Value(DEFAULT_CAN_DRIVE_STATE_TIMEOUT), "CAN drive state timeout (seconds)").asFloat64();
+ if (!config.check("bus", "CAN bus") || !config.check("nodes", "CAN nodes"))
+ {
+ CD_ERROR("Some mandatory keys are missing: \"bus\", \"nodes\".\n");
+ return false;
+ }
 
- linInterpPeriodMs = config.check("linInterpPeriodMs", yarp::os::Value(DEFAULT_LIN_INTERP_PERIOD_MS), "linear interpolation mode period (milliseconds)").asInt32();
- linInterpBufferSize = config.check("linInterpBufferSize", yarp::os::Value(DEFAULT_LIN_INTERP_BUFFER_SIZE), "linear interpolation mode buffer size").asInt32();
- linInterpMode = config.check("linInterpMode", yarp::os::Value(DEFAULT_LIN_INTERP_MODE), "linear interpolation mode (pt/pvt)").asString();
+ std::string canBus = config.find("bus").asString();
+ yarp::os::Bottle * nodes = config.find("nodes").asList();
 
- yarp::os::Bottle ids = config.findGroup("ids", "CAN bus IDs").tail(); //-- e.g. 15
- yarp::os::Bottle trs = config.findGroup("trs", "reductions").tail(); //-- e.g. 160
- yarp::os::Bottle ks = config.findGroup("ks", "motor constants").tail(); //-- e.g. 0.0706
+ if (canBus.empty() || nodes == nullptr)
+ {
+ CD_ERROR("Illegal key(s): empty \"bus\" or nullptr \"nodes\".\n");
+ return false;
+ }
 
- yarp::os::Bottle maxs = config.findGroup("maxs", "maximum joint limits (meters or degrees)").tail(); //-- e.g. 360
- yarp::os::Bottle mins = config.findGroup("mins", "minimum joint limits (meters or degrees)").tail(); //-- e.g. -360
- yarp::os::Bottle maxVels = config.findGroup("maxVels", "maximum joint velocities (meters/second or degrees/second)").tail(); //-- e.g. 1000
- yarp::os::Bottle refAccelerations = config.findGroup("refAccelerations", "ref accelerations (meters/second^2 or degrees/second^2)").tail(); //-- e.g. 0.575437
- yarp::os::Bottle refSpeeds = config.findGroup("refSpeeds", "ref speeds (meters/second or degrees/second)").tail(); //-- e.g. 737.2798
- yarp::os::Bottle encoderPulsess = config.findGroup("encoderPulsess", "encoder pulses (multiple nodes)").tail(); //-- e.g. 4096 (4 * 1024)
- yarp::os::Bottle pulsesPerSamples = config.findGroup("pulsesPerSamples", "encoder pulses per sample (multiple nodes)").tail(); //-- e.g. 1000
+ yarp::os::Bottle & canBusGroup = config.findGroup(canBus);
 
- yarp::os::Bottle types = config.findGroup("types", "device name of each node").tail(); //-- e.g. 15
+ if (canBusGroup.isNull())
+ {
+ CD_ERROR("Missing CAN bus device group %s.\n", canBus.c_str());
+ return false;
+ }
 
  yarp::os::Property canBusOptions;
- canBusOptions.fromString(config.toString()); // canDevice, canBitrate
- canBusOptions.put("device", canBusType);
+ canBusOptions.fromString(canBusGroup.toString(), false);
+ canBusOptions.fromString(config.toString(), false);
  canBusOptions.put("canBlockingMode", false); // enforce non-blocking mode
  canBusOptions.put("canAllowPermissive", false); // always check usage requirements
- canBusOptions.setMonitor(config.getMonitor(), canBusType.c_str());
-
- int parallelCanThreadLimit = config.check("parallelCanThreadLimit", yarp::os::Value(0), "parallel CAN TX thread limit").asInt32();
-
- if (parallelCanThreadLimit > 0)
- {
- deviceMapper.enableParallelization(parallelCanThreadLimit);
- }
+ canBusOptions.setMonitor(config.getMonitor(), canBus.c_str());
 
  if (!canBusDevice.open(canBusOptions))
  {
@@ -61,58 +52,69 @@ bool CanBusControlboard::open(yarp::os::Searchable & config)
 
  if (!canBusDevice.view(iCanBus))
  {
- CD_ERROR("Cannot view ICanBus interface in device: %s.\n", canBusType.c_str());
+ CD_ERROR("Cannot view ICanBus interface in device: %s.\n", canBus.c_str());
  return false;
  }
 
  yarp::dev::ICanBufferFactory * iCanBufferFactory;
 
  if (!canBusDevice.view(iCanBufferFactory))
  {
- CD_ERROR("Cannot view ICanBufferFactory interface in device: %s.\n", canBusType.c_str());
+ CD_ERROR("Cannot view ICanBufferFactory interface in device: %s.\n", canBus.c_str());
  return false;
  }
 
- iCanBusSharers.resize(ids.size());
+ iCanBusSharers.resize(nodes->size());
+
+ int canRxBufferSize = config.check("canBusRxBufferSize", yarp::os::Value(DEFAULT_CAN_RX_BUFFER_SIZE), "CAN bus RX buffer size").asInt();
+ int canTxBufferSize = config.check("canBusTxBufferSize", yarp::os::Value(DEFAULT_CAN_TX_BUFFER_SIZE), "CAN bus TX buffer size").asInt();
+ double canRxPeriodMs = config.check("canRxPeriodMs", yarp::os::Value(DEFAULT_CAN_RX_PERIOD_MS), "CAN bus RX period (milliseconds)").asFloat64();
+ double canTxPeriodMs = config.check("canTxPeriodMs", yarp::os::Value(DEFAULT_CAN_TX_PERIOD_MS), "CAN bus TX period (milliseconds)").asFloat64();
+
+ canReaderThread = new CanReaderThread(canBus, iCanBusSharers);
+ canReaderThread->setCanHandles(iCanBus, iCanBufferFactory, canRxBufferSize);
+ canReaderThread->setPeriod(canRxPeriodMs);
+
+ canWriterThread = new CanWriterThread(canBus);
+ canWriterThread->setCanHandles(iCanBus, iCanBufferFactory, canTxBufferSize);
+ canWriterThread->setPeriod(canTxPeriodMs);
+
+ linInterpPeriodMs = config.check("linInterpPeriodMs", yarp::os::Value(DEFAULT_LIN_INTERP_PERIOD_MS), "linear interpolation mode period (milliseconds)").asInt32();
+ linInterpBufferSize = config.check("linInterpBufferSize", yarp::os::Value(DEFAULT_LIN_INTERP_BUFFER_SIZE), "linear interpolation mode buffer size").asInt32();
+ linInterpMode = config.check("linInterpMode", yarp::os::Value(DEFAULT_LIN_INTERP_MODE), "linear interpolation mode (pt/pvt)").asString();
+
+ int parallelCanThreadLimit = config.check("parallelCanThreadLimit", yarp::os::Value(0), "parallel CAN TX thread limit").asInt32();
+
+ if (parallelCanThreadLimit > 0)
+ {
+ deviceMapper.enableParallelization(parallelCanThreadLimit);
+ }
 
- for (int i = 0; i < ids.size(); i++)
+ for (int i = 0; i < nodes->size(); i++)
  {
- if (types.get(i).asString().empty())
+ std::string node = nodes->get(i).asString();
+ yarp::os::Bottle & nodeGroup = config.findGroup(node);
+
+ if (nodeGroup.isNull())
  {
- CD_WARNING("Argument \"types\" empty at %d.\n", i);
+ CD_ERROR("Missing CAN node device group %s.\n", node.c_str());
+ return false;
  }
 
- yarp::os::Property options;
- options.put("device", types.get(i));
- options.put("canId", ids.get(i));
- options.put("tr", trs.get(i));
- options.put("min", mins.get(i));
- options.put("max", maxs.get(i));
- options.put("maxVel", maxVels.get(i));
- options.put("k", ks.get(i));
- options.put("refAcceleration", refAccelerations.get(i));
- options.put("refSpeed", refSpeeds.get(i));
- options.put("encoderPulses", encoderPulsess.get(i));
- options.put("pulsesPerSample", pulsesPerSamples.get(i));
- options.put("linInterpPeriodMs", linInterpPeriodMs);
- options.put("linInterpBufferSize", linInterpBufferSize);
- options.put("linInterpMode", linInterpMode);
- options.put("canSdoTimeoutMs", canSdoTimeoutMs);
- options.put("canDriveStateTimeout", canDriveStateTimeout);
- options.put("cuiTimeout", cuiTimeout);
- std::string context = types.get(i).asString() + "_" + std::to_string(ids.get(i).asInt32());
- options.setMonitor(config.getMonitor(),context.c_str());
-
- yarp::dev::PolyDriver * device = new yarp::dev::PolyDriver(options);
-
- if (!device->isValid())
+ yarp::os::Property nodeOptions;
+ nodeOptions.fromString(nodeGroup.toString(), false);
+ nodeOptions.fromString(config.toString(), false);
+ nodeOptions.setMonitor(config.getMonitor(), node.c_str());
+
+ yarp::dev::PolyDriver * device = new yarp::dev::PolyDriver;
+ nodeDevices.push(device, node.c_str());
+
+ if (!device->open(nodeOptions))
  {
- CD_ERROR("CAN node [%d] '%s' instantiation failure.\n", i, types.get(i).asString().c_str());
+ CD_ERROR("CAN node device %s configuration failure.\n", node.c_str());
  return false;
  }
 
- nodes.push(device, ""); // TODO: device key
-
  if (!deviceMapper.registerDevice(device))
  {
  CD_ERROR("Unable to register device.\n");
@@ -127,24 +129,24 @@ bool CanBusControlboard::open(yarp::os::Searchable & config)
 
  iCanBusSharers[i]->registerSender(canWriterThread->getDelegate());
 
- if (!iCanBus->canIdAdd(ids.get(i).asInt32()))
+ if (!iCanBus->canIdAdd(iCanBusSharers[i]->getId()))
  {
- CD_ERROR("Cannot register acceptance filter for node ID: %d.\n", ids.get(i).asInt32());
+ CD_ERROR("Cannot register acceptance filter for node ID: %d.\n", iCanBusSharers[i]->getId());
  return false;
  }
  }
 
- const std::string canDevice = canBusOptions.find("canDevice").asString();
-
- canReaderThread = new CanReaderThread(canDevice, iCanBusSharers);
- canReaderThread->setCanHandles(iCanBus, iCanBufferFactory, canRxBufferSize);
- canReaderThread->setPeriod(canRxPeriodMs);
- canReaderThread->start();
+ if (!canReaderThread->start())
+ {
+ CD_ERROR("Unable to start reader thread.\n");
+ return false;
+ }
 
- canWriterThread = new CanWriterThread(canDevice);
- canWriterThread->setCanHandles(iCanBus, iCanBufferFactory, canTxBufferSize);
- canWriterThread->setPeriod(canTxPeriodMs);
- canWriterThread->start();
+ if (!canWriterThread->start())
+ {
+ CD_ERROR("Unable to start writer thread.\n");
+ return false;
+ }
 
  for (auto p : iCanBusSharers)
  {
@@ -178,10 +180,10 @@ bool CanBusControlboard::close()
  ok &= p->finalize();
  }
 
- for (int i = 0; i < nodes.size(); i++)
+ for (int i = 0; i < nodeDevices.size(); i++)
  {
- ok &= nodes[i]->poly->close();
- delete nodes[i]->poly;
+ ok &= nodeDevices[i]->poly->close();
+ delete nodeDevices[i]->poly;
  }
 
  if (canWriterThread && canWriterThread->isRunning())

libraries/YarpPlugins/CanBusControlboard/IControlModeImpl.cpp

@@ -60,7 +60,7 @@ bool CanBusControlboard::setControlModes(int * modes)
  return false;
  }
 
- for (unsigned int i = 0; i < nodes.size(); i++)
+ for (unsigned int i = 0; i < nodeDevices.size(); i++)
  {
  posdThread->updateControlModeRegister(i, modes[i] == VOCAB_CM_POSITION_DIRECT);
  }

libraries/YarpPlugins/CanBusHico/CanBusHico.hpp

@@ -17,18 +17,18 @@
 #include "hico_api.h"
 #include "HicoCanMessage.hpp"
 
-#define DEFAULT_CAN_DEVICE "/dev/can0"
-#define DEFAULT_CAN_BITRATE 1000000
+#define DEFAULT_PORT "/dev/can0"
+#define DEFAULT_BITRATE 1000000
 
-#define DEFAULT_CAN_RX_TIMEOUT_MS 1
-#define DEFAULT_CAN_TX_TIMEOUT_MS 0 // '0' means no timeout
+#define DEFAULT_RX_TIMEOUT_MS 1
+#define DEFAULT_TX_TIMEOUT_MS 0 // '0' means no timeout
 
-#define DEFAULT_CAN_BLOCKING_MODE true
-#define DEFAULT_CAN_ALLOW_PERMISSIVE false
+#define DEFAULT_BLOCKING_MODE true
+#define DEFAULT_ALLOW_PERMISSIVE false
 
 #define DELAY 0.001 // [s]
 
-#define DEFAULT_CAN_FILTER_CONFIGURATION "disabled"
+#define DEFAULT_FILTER_CONFIGURATION "disabled"
 
 namespace roboticslab
 {
@@ -51,10 +51,10 @@ class CanBusHico : public yarp::dev::DeviceDriver,
 public:
 
  CanBusHico() : fileDescriptor(0),
- rxTimeoutMs(DEFAULT_CAN_RX_TIMEOUT_MS),
- txTimeoutMs(DEFAULT_CAN_TX_TIMEOUT_MS),
- blockingMode(DEFAULT_CAN_BLOCKING_MODE),
- allowPermissive(DEFAULT_CAN_ALLOW_PERMISSIVE),
+ rxTimeoutMs(DEFAULT_RX_TIMEOUT_MS),
+ txTimeoutMs(DEFAULT_TX_TIMEOUT_MS),
+ blockingMode(DEFAULT_BLOCKING_MODE),
+ allowPermissive(DEFAULT_ALLOW_PERMISSIVE),
  filterManager(NULL),
  filterConfig(FilterManager::DISABLED)
  {}

libraries/YarpPlugins/CanBusHico/DeviceDriverImpl.cpp

@@ -19,18 +19,18 @@
 
 bool roboticslab::CanBusHico::open(yarp::os::Searchable& config)
 {
- std::string devicePath = config.check("canDevice", yarp::os::Value(DEFAULT_CAN_DEVICE), "CAN device path").asString();
- int bitrate = config.check("canBitrate", yarp::os::Value(DEFAULT_CAN_BITRATE), "CAN bitrate (bps)").asInt32();
+ 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();
 
- blockingMode = config.check("canBlockingMode", yarp::os::Value(DEFAULT_CAN_BLOCKING_MODE), "CAN blocking mode enabled").asBool();
- allowPermissive = config.check("canAllowPermissive", yarp::os::Value(DEFAULT_CAN_ALLOW_PERMISSIVE), "CAN read/write permissive mode").asBool();
+ blockingMode = config.check("blockingMode", yarp::os::Value(DEFAULT_BLOCKING_MODE), "CAN blocking mode enabled").asBool();
+ allowPermissive = config.check("allowPermissive", yarp::os::Value(DEFAULT_ALLOW_PERMISSIVE), "CAN read/write permissive mode").asBool();
 
  if (blockingMode)
  {
  CD_INFO("Blocking mode enabled for CAN device: %s.\n", devicePath.c_str());
 
- rxTimeoutMs = config.check("canRxTimeoutMs", yarp::os::Value(DEFAULT_CAN_RX_TIMEOUT_MS), "CAN RX timeout (milliseconds)").asInt32();
- txTimeoutMs = config.check("canTxTimeoutMs", yarp::os::Value(DEFAULT_CAN_TX_TIMEOUT_MS), "CAN TX timeout (milliseconds)").asInt32();
+ rxTimeoutMs = config.check("rxTimeoutMs", yarp::os::Value(DEFAULT_RX_TIMEOUT_MS), "CAN RX timeout (milliseconds)").asInt32();
+ txTimeoutMs = config.check("txTimeoutMs", yarp::os::Value(DEFAULT_TX_TIMEOUT_MS), "CAN TX timeout (milliseconds)").asInt32();
 
  if (rxTimeoutMs <= 0)
  {
@@ -49,7 +49,7 @@ bool roboticslab::CanBusHico::open(yarp::os::Searchable& config)
 
  CD_INFO("Permissive mode flag for read/write operations on CAN device %s: %d.\n", devicePath.c_str(), allowPermissive);
 
- std::string filterConfigStr = config.check("canFilterConfiguration", yarp::os::Value(DEFAULT_CAN_FILTER_CONFIGURATION),
+ std::string filterConfigStr = config.check("filterConfiguration", yarp::os::Value(DEFAULT_FILTER_CONFIGURATION),
  "CAN filter configuration (disabled|noRange|maskAndRange)").asString();
 
  CD_INFO("CAN filter configuration for CAN device %s: %s.\n", devicePath.c_str(), filterConfigStr.c_str());

libraries/YarpPlugins/CanBusPeak/CanBusPeak.hpp

@@ -15,14 +15,14 @@
 
 #include "PeakCanMessage.hpp"
 
-#define DEFAULT_CAN_DEVICE "/dev/pcan0"
-#define DEFAULT_CAN_BITRATE 1000000
+#define DEFAULT_PORT "/dev/pcan0"
+#define DEFAULT_BITRATE 1000000
 
-#define DEFAULT_CAN_RX_TIMEOUT_MS 1
-#define DEFAULT_CAN_TX_TIMEOUT_MS 0 // '0' means no timeout
+#define DEFAULT_RX_TIMEOUT_MS 1
+#define DEFAULT_TX_TIMEOUT_MS 0 // '0' means no timeout
 
-#define DEFAULT_CAN_BLOCKING_MODE true
-#define DEFAULT_CAN_ALLOW_PERMISSIVE false
+#define DEFAULT_BLOCKING_MODE true
+#define DEFAULT_ALLOW_PERMISSIVE false
 
 namespace roboticslab
 {
@@ -76,10 +76,10 @@ class CanBusPeak : public yarp::dev::DeviceDriver,
 public:
 
  CanBusPeak() : fileDescriptor(0),
- rxTimeoutMs(DEFAULT_CAN_RX_TIMEOUT_MS),
- txTimeoutMs(DEFAULT_CAN_TX_TIMEOUT_MS),
- blockingMode(DEFAULT_CAN_BLOCKING_MODE),
- allowPermissive(DEFAULT_CAN_ALLOW_PERMISSIVE)
+ rxTimeoutMs(DEFAULT_RX_TIMEOUT_MS),
+ txTimeoutMs(DEFAULT_TX_TIMEOUT_MS),
+ blockingMode(DEFAULT_BLOCKING_MODE),
+ allowPermissive(DEFAULT_ALLOW_PERMISSIVE)
  {}
 
  // --------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp ---------