update MAIS reading modules to use MAS

src/libraries/icubmod/imuFilter/ImuFilter.h

@@ -55,7 +55,7 @@ namespace yarp {
 
 This device driver applies filtering to remove gyro bias. \n
 At startup it tries to attach directly to the IMU devcice of the specified robot
-or through a MultipleAnalogSensorClient(using the subdevice mechanism).
+or through a MultipleAnalogSensorsClient(using the subdevice mechanism).
 
 \section parameters_sec Parameters
 

src/libraries/icubmod/imuFilter/PassThroughInertial.cpp

@@ -36,7 +36,7 @@ bool PassThroughInertial::open(yarp::os::Searchable& config) //TODO: still to be
         auto subdeviceName = config.find("subdevice").asString();
         yarp::os::Property masClientProp;
         masClientProp.fromString(config.toString());
-        masClientProp.put("device", subdeviceName); // "multipleanalogsensorclient"
+        masClientProp.put("device", subdeviceName); // "multipleanalogsensorsclient"
         masClientProp.put("local", config.find("proxy_local").asString());
         masClientProp.put("remote", config.find("proxy_remote").asString());
 

src/libraries/perceptiveModels/include/iCub/perception/sensors.h

@@ -161,7 +161,7 @@ class Sensor
 /**
 * @ingroup percmod_Implementations 
 *  
-* This class implements the reading of motor joints encoders. 
+* This class implements the reading of joints encoders. 
 */
 class SensorEncoders : public Sensor
 {
@@ -185,7 +185,44 @@ class SensorEncoders : public Sensor
 
     /**
     * Retrieve the sensor joint value.
-    * @param in a value containing the current joint position.
+    * @param in a value containing the encoder value.
+    * @return true/false on success/failure.
+    */
+    bool getOutput(yarp::os::Value &in) const;
+};
+
+
+/**
+* @ingroup percmod_Implementations 
+*  
+* This class implements the reading of encoders through
+* MultipleAnalogSensors (MAS) interfaces.
+*/
+class SensorEncoderArrays : public Sensor
+{
+protected:
+    int num_arrays;
+    int index_array;
+    int index_element;
+
+public:
+    /**
+    * Configure the sensor. 
+    * @param source a pointer to the yarp::dev::IEncoderArrays
+    *               interface.
+    * @param options a Property containing the configuration 
+    *                parameters. Available options are:\n
+    * <b>name</b>: the name of the sensor.\n 
+    * <b>num_arrays</b>: the number of available arrays.\n
+    * <b>index_array</b>: the index of the array to be sensed.
+    * <b>index_element</b>: the index of the element of the array.
+    * @return true/false on success/failure.
+    */
+    bool configure(void *source, const yarp::os::Property &options);
+
+    /**
+    * Retrieve the sensor encoder value.
+    * @param in a value containing the encoder value.
     * @return true/false on success/failure.
     */
     bool getOutput(yarp::os::Value &in) const;

src/libraries/perceptiveModels/include/iCub/perception/springyFingers.h

@@ -208,13 +208,13 @@ class SpringyFingersModel : public virtual Model
     std::string robot;
     std::string carrier;    
 
-    SensorEncoders  sensEncs[5];
-    SensorPort      sensPort[12];
-    SpringyFinger   fingers[5];
+    SensorEncoders      sensEncs[5];
+    SensorEncoderArrays sensEncArrays[12];
+    SpringyFinger       fingers[5];
     bool configured;
 
-    yarp::os::BufferedPort<yarp::os::Bottle> *port;
-    yarp::dev::PolyDriver                     driver;
+    yarp::dev::PolyDriver drvEncs;
+    yarp::dev::PolyDriver drvMAIS;
 
     std::mutex mtx;
 

src/libraries/perceptiveModels/src/sensors.cpp

@@ -18,6 +18,7 @@
 #include <yarp/os/Bottle.h>
 #include <yarp/os/BufferedPort.h>
 #include <yarp/dev/ControlBoardInterfaces.h>
+#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
 #include <yarp/sig/Vector.h>
 
 #include <iCub/perception/private/ports.h>
@@ -59,7 +60,7 @@ bool SensorEncoders::configure(void *source, const Property &options)
 bool SensorEncoders::getOutput(Value &in) const
 {
     if (!configured)
-        return false;    
+        return false;
 
     Vector vect(size);
     static_cast<IEncoders*>(source)->getEncoders(vect.data());
@@ -69,6 +70,48 @@ bool SensorEncoders::getOutput(Value &in) const
 }
 
 
+/************************************************************************/
+bool SensorEncoderArrays::configure(void *source, const Property &options)
+{
+    if ((source==NULL) || !options.check("name") || !options.check("num_arrays") ||
+        !options.check("index_array") || !options.check("index_element"))
+        return false;
+
+    this->source=source;
+    name=options.find("name").asString();
+    num_arrays=options.find("num_arrays").asInt32();
+    index_array=options.find("index_array").asInt32();
+    index_element=options.find("index_element").asInt32();
+
+    return configured=true;
+}
+
+
+/************************************************************************/
+bool SensorEncoderArrays::getOutput(Value &in) const
+{
+    auto* iencarray=static_cast<IEncoderArrays*>(source);
+    if (!configured)
+        return false;
+
+    if ((index_array<0) || (index_array>=iencarray->getNrOfEncoderArrays()))
+        return false;
+
+    if ((index_element<0) || (index_element>=iencarray->getEncoderArraySize(index_array)))
+        return false;
+
+    if (iencarray->getEncoderArrayStatus(index_array)!=MAS_OK)
+        return false;
+
+    double stamp;
+    Vector vect(iencarray->getEncoderArraySize(index_array));
+    iencarray->getEncoderArrayMeasure(index_array,vect,stamp);
+    in=Value(vect[index_element]);
+
+    return true;
+}
+
+
 /************************************************************************/
 bool SensorPort::configure(void *source, const Property &options)
 {

src/libraries/perceptiveModels/src/springyFingers.cpp

@@ -260,7 +260,6 @@ bool SpringyFinger::calibrate(const Property &options)
 /************************************************************************/
 SpringyFingersModel::SpringyFingersModel()
 {
-    port=new iCub::perception::Port;
     configured=false;
 }
 
@@ -286,26 +285,28 @@ bool SpringyFingersModel::fromProperty(const Property &options)
     string part_motor=string(type+"_arm");
     string part_analog=string(type+"_hand");
 
-    Property prop;
-    prop.put("device","remote_controlboard");
-    prop.put("remote","/"+robot+"/"+part_motor);
-    prop.put("local","/"+name+"/"+part_motor);
-    if (!driver.open(prop))
+    Property propEncs;
+    propEncs.put("device","remote_controlboard");
+    propEncs.put("remote","/"+robot+"/"+part_motor);
+    propEncs.put("local","/"+name+"/"+part_motor);
+    if (!drvEncs.open(propEncs))
         return false;
-
-    port->open("/"+name+"/"+part_analog+"/analog:i");
-    string analogPortName("/"+robot+"/"+part_analog+"/analog:o");
-    if (!Network::connect(analogPortName,port->getName(),carrier))
+
+    Property propMAIS;
+    propMAIS.put("device","multipleanalogsensorsclient");
+    propMAIS.put("remote","/"+robot+"/"+part_analog+"/MAIS");
+    propMAIS.put("local","/"+name+"/"+part_analog+"/MAIS");
+    if (!drvMAIS.open(propMAIS))
     {
-        printMessage(log::error,1,"unable to connect to %s",analogPortName.c_str());
+        printMessage(log::error,1,"unable to connect to %s",propMAIS.find("remote").asString().c_str());
         close();
         return false;
     }
-
-    IEncoders *ienc; driver.view(ienc);
+
+    printMessage(log::info,1,"configuring joint encoders sensors ...");    
+    IEncoders *ienc; drvEncs.view(ienc);
     int nAxes; ienc->getAxes(&nAxes);
 
-    printMessage(log::info,1,"configuring interface-based sensors ...");
     Property propGen;
     propGen.put("name","In_0");
     propGen.put("size",nAxes);
@@ -324,33 +325,39 @@ bool SpringyFingersModel::fromProperty(const Property &options)
     sensors_ok&=sensEncs[3].configure(pEncs,propRing);
     sensors_ok&=sensEncs[4].configure(pEncs,propLittle);
 
-    printMessage(log::info,1,"configuring port-based sensors ...");
-    Property thumb_mp(  "(name Out_0) (index 1)" );
-    Property thumb_ip(  "(name Out_1) (index 2)" );
-    Property index_mp(  "(name Out_0) (index 4)" );
-    Property index_ip(  "(name Out_1) (index 5)" );
-    Property middle_mp( "(name Out_0) (index 7)" );
-    Property middle_ip( "(name Out_1) (index 8)" );
-    Property ring_mp(   "(name Out_0) (index 9)" );
-    Property ring_pip(  "(name Out_1) (index 10)");
-    Property ring_dip(  "(name Out_2) (index 11)");
-    Property little_mp( "(name Out_0) (index 12)");
-    Property little_pip("(name Out_1) (index 13)");
-    Property little_dip("(name Out_2) (index 14)");
-
-    void *pPort=static_cast<void*>(port);
-    sensors_ok&=sensPort[0].configure(pPort,thumb_mp);
-    sensors_ok&=sensPort[1].configure(pPort,thumb_ip);
-    sensors_ok&=sensPort[2].configure(pPort,index_mp);
-    sensors_ok&=sensPort[3].configure(pPort,index_ip);
-    sensors_ok&=sensPort[4].configure(pPort,middle_mp);
-    sensors_ok&=sensPort[5].configure(pPort,middle_ip);
-    sensors_ok&=sensPort[6].configure(pPort,ring_mp);
-    sensors_ok&=sensPort[7].configure(pPort,ring_pip);
-    sensors_ok&=sensPort[8].configure(pPort,ring_dip);
-    sensors_ok&=sensPort[9].configure(pPort,little_mp);
-    sensors_ok&=sensPort[10].configure(pPort,little_pip);
-    sensors_ok&=sensPort[11].configure(pPort,little_dip);
+    printMessage(log::info,1,"configuring analog encoders sensors ...");
+    IEncoderArrays *iencarray; drvMAIS.view(iencarray);
+
+    propGen.clear();
+    propGen.put("num_arrays",(int)iencarray->getNrOfEncoderArrays());
+    propGen.put("index_array",0);
+
+    Property thumb_mp=propGen;   thumb_mp.put(  "name","Out_0");   thumb_mp.put("index_element","1");
+    Property thumb_ip=propGen;   thumb_ip.put(  "name","Out_1");   thumb_ip.put("index_element","2");
+    Property index_mp=propGen;   index_mp.put(  "name","Out_0");   index_mp.put("index_element","4");
+    Property index_ip=propGen;   index_ip.put(  "name","Out_1");   index_ip.put("index_element","5");
+    Property middle_mp=propGen;  middle_mp.put( "name","Out_0");  middle_mp.put("index_element","7");
+    Property middle_ip=propGen;  middle_ip.put( "name","Out_1");  middle_ip.put("index_element","8");
+    Property ring_mp=propGen;    ring_mp.put(   "name","Out_0");    ring_mp.put("index_element","9");
+    Property ring_pip=propGen;   ring_pip.put(  "name","Out_1");   ring_pip.put("index_element","10");
+    Property ring_dip=propGen;   ring_dip.put(  "name","Out_2");   ring_dip.put("index_element","11");
+    Property little_mp=propGen;  little_mp.put( "name","Out_0");  little_mp.put("index_element","12");
+    Property little_pip=propGen; little_pip.put("name","Out_1"); little_pip.put("index_element","13");
+    Property little_dip=propGen; little_dip.put("name","Out_2"); little_dip.put("index_element","14");
+
+    void *pEncArrays=static_cast<void*>(iencarray);
+    sensors_ok&=sensEncArrays[0].configure(pEncArrays,thumb_mp);
+    sensors_ok&=sensEncArrays[1].configure(pEncArrays,thumb_ip);
+    sensors_ok&=sensEncArrays[2].configure(pEncArrays,index_mp);
+    sensors_ok&=sensEncArrays[3].configure(pEncArrays,index_ip);
+    sensors_ok&=sensEncArrays[4].configure(pEncArrays,middle_mp);
+    sensors_ok&=sensEncArrays[5].configure(pEncArrays,middle_ip);
+    sensors_ok&=sensEncArrays[6].configure(pEncArrays,ring_mp);
+    sensors_ok&=sensEncArrays[7].configure(pEncArrays,ring_pip);
+    sensors_ok&=sensEncArrays[8].configure(pEncArrays,ring_dip);
+    sensors_ok&=sensEncArrays[9].configure(pEncArrays,little_mp);
+    sensors_ok&=sensEncArrays[10].configure(pEncArrays,little_pip);
+    sensors_ok&=sensEncArrays[11].configure(pEncArrays,little_dip);
 
     if (!sensors_ok)
     {
@@ -382,26 +389,26 @@ bool SpringyFingersModel::fromProperty(const Property &options)
 
     printMessage(log::info,1,"attaching sensors to fingers ...");
     fingers[0].attachSensor(sensEncs[0]);
-    fingers[0].attachSensor(sensPort[0]);
-    fingers[0].attachSensor(sensPort[1]);
+    fingers[0].attachSensor(sensEncArrays[0]);
+    fingers[0].attachSensor(sensEncArrays[1]);
 
     fingers[1].attachSensor(sensEncs[1]);
-    fingers[1].attachSensor(sensPort[2]);
-    fingers[1].attachSensor(sensPort[3]);
+    fingers[1].attachSensor(sensEncArrays[2]);
+    fingers[1].attachSensor(sensEncArrays[3]);
 
     fingers[2].attachSensor(sensEncs[2]);
-    fingers[2].attachSensor(sensPort[4]);
-    fingers[2].attachSensor(sensPort[5]);
+    fingers[2].attachSensor(sensEncArrays[4]);
+    fingers[2].attachSensor(sensEncArrays[5]);
 
     fingers[3].attachSensor(sensEncs[3]);
-    fingers[3].attachSensor(sensPort[6]);
-    fingers[3].attachSensor(sensPort[7]);
-    fingers[3].attachSensor(sensPort[8]);
+    fingers[3].attachSensor(sensEncArrays[6]);
+    fingers[3].attachSensor(sensEncArrays[7]);
+    fingers[3].attachSensor(sensEncArrays[8]);
 
     fingers[4].attachSensor(sensEncs[4]);
-    fingers[4].attachSensor(sensPort[9]);
-    fingers[4].attachSensor(sensPort[10]);
-    fingers[4].attachSensor(sensPort[11]);
+    fingers[4].attachSensor(sensEncArrays[9]);
+    fingers[4].attachSensor(sensEncArrays[10]);
+    fingers[4].attachSensor(sensEncArrays[11]);
 
     attachNode(fingers[0]);
     attachNode(fingers[1]);
@@ -506,10 +513,10 @@ bool SpringyFingersModel::calibrate(const Property &options)
             return false;
         }
 
-        IControlMode     *imod; driver.view(imod);
-        IControlLimits   *ilim; driver.view(ilim);
-        IEncoders        *ienc; driver.view(ienc);
-        IPositionControl *ipos; driver.view(ipos);
+        IControlMode     *imod; drvEncs.view(imod);
+        IControlLimits   *ilim; drvEncs.view(ilim);
+        IEncoders        *ienc; drvEncs.view(ienc);
+        IPositionControl *ipos; drvEncs.view(ipos);
 
         int nAxes; ienc->getAxes(&nAxes);
         Vector qmin(nAxes),qmax(nAxes),vel(nAxes),acc(nAxes);
@@ -726,9 +733,9 @@ void SpringyFingersModel::calibrateFinger(SpringyFinger &finger, const int joint
     double timeout=2.0*(_max-_min)/finger.getCalibVel();
 
     mtx.lock();
-    IControlMode     *imod; driver.view(imod);
-    IEncoders        *ienc; driver.view(ienc);
-    IPositionControl *ipos; driver.view(ipos);
+    IControlMode     *imod; drvEncs.view(imod);
+    IEncoders        *ienc; drvEncs.view(ienc);
+    IPositionControl *ipos; drvEncs.view(ipos);
     mtx.unlock();
 
     // workaround
@@ -812,11 +819,11 @@ void SpringyFingersModel::close()
 {
     printMessage(log::info,1,"closing ...");
 
-    if (driver.isValid())
-        driver.close();
+    if (drvEncs.isValid())
+        drvEncs.close();
 
-    if (!port->isClosed())
-        port->close();
+    if (drvMAIS.isValid())
+        drvMAIS.close();
 
     nodes.clear();
 
@@ -828,7 +835,6 @@ void SpringyFingersModel::close()
 SpringyFingersModel::~SpringyFingersModel()
 {
     close();
-    delete port;
 }
 
 

src/modules/wholeBodyDynamics/main.cpp

@@ -96,16 +96,6 @@ By launching the following command:
 wholeBodyDynamics --rate 10
 \endcode
 
-the module will create the listening port
-/wholeBodyDynamics/right_arm/FT:i for the acquisition of data
-vector coming for istance from the right arm analog port.
-
-Try now the following:
-
-\code
-yarp connect /icub/right_arm/analog:o /wholeBodyDynamics/right_arm/FT:i
-\endcode
-
 \author Matteo Fumagalli
 
 This file can be edited at src/wholeBodyDynamics/main.cpp.

src/modules/wholeBodyDynamics/observerThread.h

@@ -43,7 +43,7 @@ using namespace std;
 
 constexpr int8_t MAX_JN = 12;
 constexpr int8_t MAX_FILTER_ORDER = 6;
-constexpr float_t SKIN_EVENTS_TIMEOUT = 0.2;     // max time (in sec) a contact is kept without reading anything from the skin events port
+constexpr float_t SKIN_EVENTS_TIMEOUT = 0.2F;     // max time (in sec) a contact is kept without reading anything from the skin events port
 
 enum thread_status_enum {STATUS_OK=0, STATUS_DISCONNECTED};
 enum calib_enum {CALIB_ALL=0, CALIB_ARMS, CALIB_LEGS, CALIB_FEET};