Merge branch 'group-get-set-params' into develop

roboticslab-uc3m · Jan 23, 2018 · 5ad5eea · 5ad5eea
2 parents 7d2e073 + f8dd1fb
commit 5ad5eea
Show file tree

Hide file tree

Showing 9 changed files with 271 additions and 47 deletions.
diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp
@@ -78,6 +78,10 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
 
     virtual bool getParameter(int vocab, double * value);
 
+    virtual bool setParameters(const std::map<int, double> & params);
+
+    virtual bool getParameters(std::map<int, double> & params);
+
     // -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp --------
 
     /**

diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp
@@ -419,3 +419,27 @@ bool roboticslab::AmorCartesianControl::getParameter(int vocab, double * value)
 }
 
 // -----------------------------------------------------------------------------
+
+bool roboticslab::AmorCartesianControl::setParameters(const std::map<int, double> & params)
+{
+    bool ok = true;
+
+    for (std::map<int, double>::const_iterator it = params.begin(); it != params.end(); ++it)
+    {
+        ok &= setParameter(it->first, it->second);
+    }
+
+    return ok;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::AmorCartesianControl::getParameters(std::map<int, double> & params)
+{
+    params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_GAIN, gain));
+    params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_MAX_JOINT_VEL, maxJointVelocity));
+    params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_FRAME, referenceFrame));
+    return true;
+}
+
+// -----------------------------------------------------------------------------
diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp
@@ -145,6 +145,10 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC
 
         virtual bool getParameter(int vocab, double * value);
 
+        virtual bool setParameters(const std::map<int, double> & params);
+
+        virtual bool getParameters(std::map<int, double> & params);
+
         // -------- RateThread declarations. Implementation in RateThreadImpl.cpp --------
 
         /** Loop function. This is the thread itself. */

diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp
@@ -512,3 +512,29 @@ bool roboticslab::BasicCartesianControl::getParameter(int vocab, double * value)
 }
 
 // -----------------------------------------------------------------------------
+
+bool roboticslab::BasicCartesianControl::setParameters(const std::map<int, double> & params)
+{
+    bool ok = true;
+
+    for (std::map<int, double>::const_iterator it = params.begin(); it != params.end(); ++it)
+    {
+        ok &= setParameter(it->first, it->second);
+    }
+
+    return ok;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::BasicCartesianControl::getParameters(std::map<int, double> & params)
+{
+    params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_GAIN, gain));
+    params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_MAX_JOINT_VEL, maxJointVelocity));
+    params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_TRAJ_DURATION, duration));
+    params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_CMC_RATE, cmcRateMs));
+    params.insert(std::pair<int, double>(VOCAB_CC_CONFIG_FRAME, referenceFrame));
+    return true;
+}
+
+// -----------------------------------------------------------------------------
diff --git a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp
@@ -10,8 +10,6 @@
 #include <yarp/os/Semaphore.h>
 
 #include <yarp/dev/Drivers.h>
-#include <yarp/dev/PolyDriver.h>
-#include <yarp/dev/ControlBoardInterfaces.h>  // VOCAB_OK, VOCAB_FAILED
 
 #include <vector>
 
@@ -95,6 +93,10 @@ class CartesianControlClient : public yarp::dev::DeviceDriver, public ICartesian
 
     virtual bool getParameter(int vocab, double * value);
 
+    virtual bool setParameters(const std::map<int, double> & params);
+
+    virtual bool getParameters(std::map<int, double> & params);
+
     // -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp --------
 
     /**

diff --git a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp
@@ -3,10 +3,19 @@
 #include "CartesianControlClient.hpp"
 
 #include <yarp/os/Time.h>
-#include <yarp/dev/ControlBoardInterfaces.h> // vocabs: OK, FAIL, SET, GET
 
 #include <ColorDebug.hpp>
 
+// -----------------------------------------------------------------------------
+
+namespace
+{
+    inline bool checkSuccess(const yarp::os::Bottle & response)
+    {
+        return !response.get(0).isVocab() || response.get(0).asVocab() != VOCAB_CC_FAILED;
+    }
+}
+
 // ------------------- ICartesianControl Related ------------------------------------
 
 bool roboticslab::CartesianControlClient::handleRpcRunnableCmd(int vocab)
@@ -17,12 +26,7 @@ bool roboticslab::CartesianControlClient::handleRpcRunnableCmd(int vocab)
 
     rpcClient.write(cmd, response);
 
-    if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED)
-    {
-        return false;
-    }
-
-    return true;
+    return checkSuccess(response);
 }
 
 // -----------------------------------------------------------------------------
@@ -40,12 +44,7 @@ bool roboticslab::CartesianControlClient::handleRpcConsumerCmd(int vocab, const
 
     rpcClient.write(cmd, response);
 
-    if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED)
-    {
-        return false;
-    }
-
-    return true;
+    return checkSuccess(response);
 }
 
 // -----------------------------------------------------------------------------
@@ -63,7 +62,7 @@ bool roboticslab::CartesianControlClient::handleRpcFunctionCmd(int vocab, const
 
     rpcClient.write(cmd, response);
 
-    if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED)
+    if (!checkSuccess(response))
     {
         return false;
     }
@@ -133,7 +132,7 @@ bool roboticslab::CartesianControlClient::stat(int &state, std::vector<double> &
 
     rpcClient.write(cmd, response);
 
-    if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED)
+    if (!checkSuccess(response))
     {
         return false;
     }
@@ -232,37 +231,79 @@ bool roboticslab::CartesianControlClient::setParameter(int vocab, double value)
 {
     yarp::os::Bottle cmd, response;
 
-    cmd.addVocab(VOCAB_SET);
+    cmd.addVocab(VOCAB_CC_SET);
     cmd.addVocab(vocab);
     cmd.addDouble(value);
 
     rpcClient.write(cmd, response);
 
-    if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED)
+    return checkSuccess(response);
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::CartesianControlClient::getParameter(int vocab, double * value)
+{
+    yarp::os::Bottle cmd, response;
+
+    cmd.addVocab(VOCAB_CC_GET);
+    cmd.addVocab(vocab);
+
+    rpcClient.write(cmd, response);
+
+    if (!checkSuccess(response))
     {
         return false;
     }
 
+    *value = response.get(0).asDouble();
+
     return true;
 }
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::CartesianControlClient::getParameter(int vocab, double * value)
+bool roboticslab::CartesianControlClient::setParameters(const std::map<int, double> & params)
 {
     yarp::os::Bottle cmd, response;
 
-    cmd.addVocab(VOCAB_GET);
-    cmd.addVocab(vocab);
+    cmd.addVocab(VOCAB_CC_SET);
+    cmd.addVocab(VOCAB_CC_CONFIG_PARAMS);
+
+    for (std::map<int, double>::const_iterator it = params.begin(); it != params.end(); ++it)
+    {
+        yarp::os::Bottle & b = cmd.addList();
+        b.addVocab(it->first);
+        b.addDouble(it->second);
+    }
+
+    rpcClient.write(cmd, response);
+
+    return checkSuccess(response);
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::CartesianControlClient::getParameters(std::map<int, double> & params)
+{
+    yarp::os::Bottle cmd, response;
+
+    cmd.addVocab(VOCAB_CC_GET);
+    cmd.addVocab(VOCAB_CC_CONFIG_PARAMS);
 
     rpcClient.write(cmd, response);
 
-    if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED)
+    if (!checkSuccess(response))
     {
         return false;
     }
 
-    *value = response.get(0).asDouble();
+    for (int i = 0; i < response.size(); i++)
+    {
+        yarp::os::Bottle * b = response.get(i).asList();
+        std::pair<int, double> el(b->get(0).asVocab(), b->get(1).asDouble());
+        params.insert(el);
+    }
 
     return true;
 }

diff --git a/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp b/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp
@@ -133,6 +133,9 @@ class RpcResponder : public yarp::dev::DeviceResponder
     bool handleParameterSetter(const yarp::os::Bottle& in, yarp::os::Bottle& out);
     bool handleParameterGetter(const yarp::os::Bottle& in, yarp::os::Bottle& out);
 
+    bool handleParameterSetterGroup(const yarp::os::Bottle& in, yarp::os::Bottle& out);
+    bool handleParameterGetterGroup(const yarp::os::Bottle& in, yarp::os::Bottle& out);
+
     virtual bool transformIncomingData(std::vector<double>& vin)
     {
         return true;