Create canBusLauncher app

#20

libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp

@@ -12,6 +12,8 @@ bool roboticslab::CanBusControlboard::open(yarp::os::Searchable& config)
 {
     std::string mode = config.check("mode",yarp::os::Value("position"),"control mode on startup (position/velocity)").asString();
     int timeCuiWait  = config.check("waitEncoder", yarp::os::Value(DEFAULT_TIME_TO_WAIT_CUI), "CUI timeout (seconds)").asInt32();
+    bool homing = config.check("home", yarp::os::Value(false), "perform homing maneuver on start").asBool();
+
     std::string canBusType = config.check("canBusType", yarp::os::Value(DEFAULT_CAN_BUS), "CAN bus device name").asString();
     canRxBufferSize = config.check("canBusRxBufferSize", yarp::os::Value(DEFAULT_CAN_RX_BUFFER_SIZE), "CAN bus RX buffer size").asInt();
     canTxBufferSize = config.check("canBusTxBufferSize", yarp::os::Value(DEFAULT_CAN_TX_BUFFER_SIZE), "CAN bus TX buffer size").asInt();
@@ -355,7 +357,7 @@ bool roboticslab::CanBusControlboard::open(yarp::os::Searchable& config)
     yarp::os::Time::delay(2);
 
     //-- Homing
-    if( config.check("home", "perform homing maneuver on start") )
+    if( homing )
     {
         CD_DEBUG("Moving motors to zero.\n");
         for(int i=0; i<nodes.size(); i++)

programs/CMakeLists.txt

@@ -1,6 +1,7 @@
 # Copyright: Universidad Carlos III de Madrid (C) 2013-2014
 # Authors: Juan G. Victores & Raúl de Santos Rico
 
+add_subdirectory(canBusLauncher)
 add_subdirectory(checkCanBus)
 add_subdirectory(dumpCanBus)
 add_subdirectory(grabberControls2Gui)

programs/canBusLauncher/CMakeLists.txt

@@ -0,0 +1,17 @@
+option(ENABLE_canBusLauncher "Enable/disable canBusLauncher program" ON)
+
+if(ENABLE_canBusLauncher)
+
+    add_executable(canBusLauncher main.cpp
+                                  CanBusLauncher.hpp
+                                  CanBusLauncher.cpp)
+
+    target_link_libraries(canBusLauncher YARP::YARP_OS
+                                         YARP::YARP_dev
+                                         YARP::YARP_init
+                                         ROBOTICSLAB::ColorDebug)
+
+    install(TARGETS canBusLauncher
+            DESTINATION ${CMAKE_INSTALL_BINDIR})
+
+endif()

programs/canBusLauncher/CanBusLauncher.cpp

@@ -0,0 +1,161 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "CanBusLauncher.hpp"
+
+#include <cstdio>
+#include <string>
+#include <sstream>
+
+#include <yarp/os/Bottle.h>
+#include <yarp/os/Property.h>
+#include <yarp/os/Value.h>
+
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/dev/Wrapper.h>
+
+#include <ColorDebug.h>
+
+using namespace roboticslab;
+
+/************************************************************************/
+
+bool CanBusLauncher::configure(yarp::os::ResourceFinder &rf)
+{
+    if (rf.check("help"))
+    {
+        std::printf("CanBusLauncher options:\n");
+        std::printf("\t--help (this help)\t--from [file.ini]\t--context [path]\t--homePoss\t--externalEncoderWait [s]\n\n");
+        std::printf("Note: if the Absolute Encoder doesn't respond, use --externalEncoderWait [seconds] parameter for using default relative encoder position\n");
+        CD_DEBUG_NO_HEADER("%s\n", rf.toString().c_str());
+        return false;
+    }
+
+    std::string mode = rf.check("mode", yarp::os::Value("position"), "initial mode of operation").asString();
+    bool homing = rf.check("homePoss", yarp::os::Value(false), "perform initial homing procedure").asBool();
+    int timeEncoderWait = rf.check("externalEncoderWait", yarp::os::Value(0), "wait till absolute encoders are ready").asInt32();
+
+    if (timeEncoderWait != 0)
+    {
+        CD_INFO("Wait time for Absolute Encoder: %d [s]\n", timeEncoderWait);
+    }
+
+    const std::string canDevicePrefix = "devCan";
+    int canDeviceId = 0;
+
+    while (true)
+    {
+        std::ostringstream oss;
+        oss << canDevicePrefix << canDeviceId;
+        std::string canDeviceLabel = oss.str();
+
+        yarp::os::Bottle canDeviceGroup = rf.findGroup(canDeviceLabel);
+
+        if (canDeviceGroup.isNull())
+        {
+            break;
+        }
+
+        CD_DEBUG("%s\n", canDeviceGroup.toString().c_str());
+
+        yarp::os::Property canDeviceOptions;
+        canDeviceOptions.fromString(canDeviceGroup.toString());
+        canDeviceOptions.put("mode", mode);
+        canDeviceOptions.put("waitEncoder", timeEncoderWait);
+        canDeviceOptions.put("home", homing);
+
+        yarp::dev::PolyDriver * canDevice = new yarp::dev::PolyDriver(canDeviceOptions);
+
+        if (!canDevice->isValid())
+        {
+            CD_ERROR("CAN device %s instantiation failure.\n", canDeviceLabel.c_str());
+            return false;
+        }
+
+        canDevices.push(canDevice, canDeviceLabel.c_str());
+        canDeviceId++;
+    }
+
+    if (canDeviceId == 0)
+    {
+        CD_ERROR("Empty CAN device list.\n");
+        return false;
+    }
+
+    const std::string wrapperDevicePrefix = "wrapper";
+    int wrapperDeviceId = 0;
+
+    while (true)
+    {
+        std::ostringstream oss;
+        oss << wrapperDevicePrefix << wrapperDeviceId;
+        std::string wrapperDeviceLabel = oss.str();
+        yarp::os::Bottle wrapperDeviceGroup = rf.findGroup(wrapperDeviceLabel);
+
+        if (wrapperDeviceGroup.isNull())
+        {
+            break;
+        }
+
+        CD_DEBUG("%s\n", wrapperDeviceGroup.toString().c_str());
+
+        yarp::os::Property wrapperDeviceOptions;
+        wrapperDeviceOptions.fromString(wrapperDeviceGroup.toString());
+
+        yarp::dev::PolyDriver * wrapperDevice = new yarp::dev::PolyDriver(wrapperDeviceOptions);
+
+        if (!wrapperDevice->isValid())
+        {
+            CD_ERROR("Wrapper device %s instantiation failure.\n", wrapperDeviceLabel.c_str());
+            return false;
+        }
+
+        wrapperDevices.push(wrapperDevice, wrapperDeviceLabel.c_str());
+
+        yarp::dev::IMultipleWrapper * iMultipleWrapper;
+        wrapperDevice->view(iMultipleWrapper);
+        iMultipleWrapper->attachAll(canDevices);
+
+        wrapperDeviceId++;
+    }
+
+    if (wrapperDeviceId == 0)
+    {
+        CD_ERROR("Empty wrapper device list.\n");
+        return false;
+    }
+
+    return true;
+}
+
+/************************************************************************/
+
+bool CanBusLauncher::updateModule()
+{
+    return true;
+}
+
+/************************************************************************/
+
+double CanBusLauncher::getPeriod()
+{
+    return 3.0;
+}
+
+/************************************************************************/
+
+bool CanBusLauncher::close()
+{
+    for (int i = 0; i < wrapperDevices.size(); i++)
+    {
+        delete wrapperDevices[i]->poly;
+    }
+
+    for (int i = 0; i < canDevices.size(); i++)
+    {
+        delete canDevices[i]->poly;
+    }
+
+    return true;
+}
+
+/************************************************************************/

programs/canBusLauncher/CanBusLauncher.hpp

@@ -0,0 +1,35 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#ifndef __CAN_BUS_LAUNCHER__
+#define __CAN_BUS_LAUNCHER__
+
+#include <yarp/os/RFModule.h>
+#include <yarp/dev/PolyDriverList.h>
+
+namespace roboticslab
+{
+
+/**
+ * @ingroup canBusLauncher
+ *
+ * @brief Launches one or more bus drivers, and controlboardwrapper2 instances
+ * that wrap corresponding nodes. A controlboardwrapper2 may be used through a
+ * YARP remote_controlboard or directly through low-level YARP controlboardwrapper2
+ * RPC commands.
+ */
+class CanBusLauncher : public yarp::os::RFModule
+{
+public:
+    bool configure(yarp::os::ResourceFinder &rf);
+    virtual double getPeriod();
+    virtual bool updateModule();
+    virtual bool close();
+
+private:
+    yarp::dev::PolyDriverList canDevices;
+    yarp::dev::PolyDriverList wrapperDevices;
+};
+
+}  // namespace roboticslab
+
+#endif  // __CAN_BUS_LAUNCHER__

programs/canBusLauncher/main.cpp

@@ -0,0 +1,48 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+/**
+ *
+ * @ingroup yarp_devices_programs
+ * \defgroup canBusLauncher canBusLauncher
+ *
+ * @brief Creates an instance of roboticslab::CanBusLauncher.
+ */
+
+#include <yarp/os/Network.h>
+#include <yarp/os/ResourceFinder.h>
+
+#include <ColorDebug.h>
+
+#include "CanBusLauncher.hpp"
+
+int main(int argc, char *argv[])
+{
+    yarp::os::ResourceFinder rf;
+    rf.setVerbose(true);
+    rf.setDefaultContext("canBusLauncher");
+    rf.setDefaultConfigFile("canBusLauncher.ini");
+    rf.configure(argc, argv);
+
+    CD_INFO("Checking for yarp network... ");
+    yarp::os::Network yarp;
+
+    if (!yarp.checkNetwork())
+    {
+        CD_ERROR_NO_HEADER("[fail]\n");
+        CD_INFO("Found no yarp network (try running \"yarpserver &\"), bye!\n");
+        return 1;
+    }
+
+    CD_SUCCESS_NO_HEADER("[ok]\n");
+
+    roboticslab::CanBusLauncher mod;
+
+    if (mod.configure(rf))
+    {
+        return mod.runModule();
+    }
+    else
+    {
+        return mod.close() ? 0 : 1;
+    }
+}