diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp index 0e39e85b8..3f7ba1acc 100644 --- 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 & params); + + virtual bool getParameters(std::map & params); + // -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp -------- /** diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp index e3b1a90fd..45cbc5921 100644 --- 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 & params) +{ + bool ok = true; + + for (std::map::const_iterator it = params.begin(); it != params.end(); ++it) + { + ok &= setParameter(it->first, it->second); + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::AmorCartesianControl::getParameters(std::map & params) +{ + params.insert(std::pair(VOCAB_CC_CONFIG_GAIN, gain)); + params.insert(std::pair(VOCAB_CC_CONFIG_MAX_JOINT_VEL, maxJointVelocity)); + params.insert(std::pair(VOCAB_CC_CONFIG_FRAME, referenceFrame)); + return true; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp index d0c776fd5..df4ceb783 100644 --- 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 & params); + + virtual bool getParameters(std::map & 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 index 15c11ca5e..b5f1223fc 100644 --- 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 & params) +{ + bool ok = true; + + for (std::map::const_iterator it = params.begin(); it != params.end(); ++it) + { + ok &= setParameter(it->first, it->second); + } + + return ok; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::BasicCartesianControl::getParameters(std::map & params) +{ + params.insert(std::pair(VOCAB_CC_CONFIG_GAIN, gain)); + params.insert(std::pair(VOCAB_CC_CONFIG_MAX_JOINT_VEL, maxJointVelocity)); + params.insert(std::pair(VOCAB_CC_CONFIG_TRAJ_DURATION, duration)); + params.insert(std::pair(VOCAB_CC_CONFIG_CMC_RATE, cmcRateMs)); + params.insert(std::pair(VOCAB_CC_CONFIG_FRAME, referenceFrame)); + return true; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp index cb84e71e9..79b05ca79 100644 --- a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp +++ b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp @@ -10,8 +10,6 @@ #include #include -#include -#include // VOCAB_OK, VOCAB_FAILED #include @@ -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 & params); + + virtual bool getParameters(std::map & params); + // -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp -------- /** diff --git a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp index 84f235a2f..0aae27ddc 100644 --- a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp @@ -3,10 +3,19 @@ #include "CartesianControlClient.hpp" #include -#include // vocabs: OK, FAIL, SET, GET #include +// ----------------------------------------------------------------------------- + +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 & 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 & 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::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 & 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 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 index 6dec4bb09..3f8de1a78 100644 --- 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& vin) { return true; diff --git a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp index 06441c85c..bc2c3e2f5 100644 --- a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp +++ b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp @@ -2,10 +2,18 @@ #include "CartesianControlServer.hpp" -#include // vocabs: OK, FAIL, SET, GET - #include +// ----------------------------------------------------------------------------- + +namespace +{ + inline bool isGroupParam(const yarp::os::Bottle& in) + { + return in.size() > 1 && in.get(1).asVocab() == VOCAB_CC_CONFIG_PARAMS; + } +} + // ------------------- RpcResponder Related ------------------------------------ bool roboticslab::RpcResponder::respond(const yarp::os::Bottle& in, yarp::os::Bottle& out) @@ -35,10 +43,10 @@ bool roboticslab::RpcResponder::respond(const yarp::os::Bottle& in, yarp::os::Bo return handleRunnableCmdMsg(in, out, &ICartesianControl::stopControl); case VOCAB_CC_TOOL: return handleConsumerCmdMsg(in, out, &ICartesianControl::tool); - case VOCAB_SET: - return handleParameterSetter(in, out); - case VOCAB_GET: - return handleParameterGetter(in, out); + case VOCAB_CC_SET: + return isGroupParam(in) ? handleParameterSetterGroup(in, out) : handleParameterSetter(in, out); + case VOCAB_CC_GET: + return isGroupParam(in) ? handleParameterGetterGroup(in, out) : handleParameterGetter(in, out); default: return DeviceResponder::respond(in, out); } @@ -58,8 +66,10 @@ void roboticslab::RpcResponder::makeUsage() addUsage("[forc] coord1 coord2 ...", "enable torque control, apply input forces (cartesian space)"); addUsage("[stop]", "stop control"); addUsage("[tool] coord1 coord2 ...", "append fixed link to end effector"); - addUsage("[cps] vocab value", "set configuration parameter"); - addUsage("[cpg] vocab", "get configuration parameter"); + addUsage("[set] vocab value", "set configuration parameter"); + addUsage("[get] vocab", "get configuration parameter"); + addUsage("[set] [prms] (vocab value) ...", "set multiple configuration parameters"); + addUsage("[get] [prms]", "get all configuration parameters"); } // ----------------------------------------------------------------------------- @@ -73,7 +83,7 @@ bool roboticslab::RpcResponder::handleStatMsg(const yarp::os::Bottle& in, yarp:: { if (!transformOutgoingData(x)) { - out.addVocab(VOCAB_FAILED); + out.addVocab(VOCAB_CC_FAILED); return false; } @@ -88,7 +98,7 @@ bool roboticslab::RpcResponder::handleStatMsg(const yarp::os::Bottle& in, yarp:: } else { - out.addVocab(VOCAB_FAILED); + out.addVocab(VOCAB_CC_FAILED); return false; } } @@ -99,12 +109,12 @@ bool roboticslab::RpcResponder::handleRunnableCmdMsg(const yarp::os::Bottle& in, { if ((iCartesianControl->*cmd)()) { - out.addVocab(VOCAB_OK); + out.addVocab(VOCAB_CC_OK); return true; } else { - out.addVocab(VOCAB_FAILED); + out.addVocab(VOCAB_CC_FAILED); return false; } } @@ -124,17 +134,17 @@ bool roboticslab::RpcResponder::handleConsumerCmdMsg(const yarp::os::Bottle& in, if (!transformIncomingData(vin) || !(iCartesianControl->*cmd)(vin)) { - out.addVocab(VOCAB_FAILED); + out.addVocab(VOCAB_CC_FAILED); return false; } - out.addVocab(VOCAB_OK); + out.addVocab(VOCAB_CC_OK); return true; } else { CD_ERROR("size error\n"); - out.addVocab(VOCAB_FAILED); + out.addVocab(VOCAB_CC_FAILED); return false; } } @@ -154,7 +164,7 @@ bool roboticslab::RpcResponder::handleFunctionCmdMsg(const yarp::os::Bottle& in, if (!transformIncomingData(vin) || !(iCartesianControl->*cmd)(vin, vout)) { - out.addVocab(VOCAB_FAILED); + out.addVocab(VOCAB_CC_FAILED); return false; } @@ -168,7 +178,7 @@ bool roboticslab::RpcResponder::handleFunctionCmdMsg(const yarp::os::Bottle& in, else { CD_ERROR("size error\n"); - out.addVocab(VOCAB_FAILED); + out.addVocab(VOCAB_CC_FAILED); return false; } } @@ -184,17 +194,17 @@ bool roboticslab::RpcResponder::handleParameterSetter(const yarp::os::Bottle& in if (!iCartesianControl->setParameter(vocab, value)) { - out.addVocab(VOCAB_FAILED); + out.addVocab(VOCAB_CC_FAILED); return false; } - out.addVocab(VOCAB_OK); + out.addVocab(VOCAB_CC_OK); return true; } else { CD_ERROR("size error\n"); - out.addVocab(VOCAB_FAILED); + out.addVocab(VOCAB_CC_FAILED); return false; } } @@ -210,7 +220,7 @@ bool roboticslab::RpcResponder::handleParameterGetter(const yarp::os::Bottle& in if (!iCartesianControl->getParameter(vocab, &value)) { - out.addVocab(VOCAB_FAILED); + out.addVocab(VOCAB_CC_FAILED); return false; } @@ -220,7 +230,77 @@ bool roboticslab::RpcResponder::handleParameterGetter(const yarp::os::Bottle& in else { CD_ERROR("size error\n"); - out.addVocab(VOCAB_FAILED); + out.addVocab(VOCAB_CC_FAILED); + return false; + } +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::RpcResponder::handleParameterSetterGroup(const yarp::os::Bottle& in, yarp::os::Bottle& out) +{ + if (in.size() > 2) + { + std::map params; + + for (int i = 2; i < in.size(); i++) + { + if (!in.get(i).isList() || in.get(i).asList()->size() != 2) + { + CD_ERROR("bottle format error\n"); + out.addVocab(VOCAB_CC_FAILED); + return false; + } + + yarp::os::Bottle * b = in.get(i).asList(); + std::pair el(b->get(0).asVocab(), b->get(1).asDouble()); + params.insert(el); + } + + if (!iCartesianControl->setParameters(params)) + { + out.addVocab(VOCAB_CC_FAILED); + return false; + } + + out.addVocab(VOCAB_CC_OK); + return true; + } + else + { + CD_ERROR("size error\n"); + out.addVocab(VOCAB_CC_FAILED); + return false; + } +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::RpcResponder::handleParameterGetterGroup(const yarp::os::Bottle& in, yarp::os::Bottle& out) +{ + if (in.size() == 2) + { + std::map params; + + if (!iCartesianControl->getParameters(params)) + { + out.addVocab(VOCAB_CC_FAILED); + return false; + } + + for (std::map::const_iterator it = params.begin(); it != params.end(); ++it) + { + yarp::os::Bottle & b = out.addList(); + b.addVocab(it->first); + b.addDouble(it->second); + } + + return true; + } + else + { + CD_ERROR("size error\n"); + out.addVocab(VOCAB_CC_FAILED); return false; } } diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h index 5976164d1..75d718295 100644 --- a/libraries/YarpPlugins/ICartesianControl.h +++ b/libraries/YarpPlugins/ICartesianControl.h @@ -3,6 +3,7 @@ #ifndef __I_CARTESIAN_CONTROL__ #define __I_CARTESIAN_CONTROL__ +#include #include #include @@ -14,6 +15,22 @@ * @{ */ +/** + * @name General-purpose vocabs + * + * Used in acknowledge responses, @ref ICartesianControl_config_commands "configuration accessors", etc.. + * + * @{ + */ + +// General-purpose vocabs +#define VOCAB_CC_OK VOCAB2('o','k') ///< Success +#define VOCAB_CC_FAILED VOCAB4('f','a','i','l') ///< Failure +#define VOCAB_CC_SET VOCAB3('s','e','t') ///< Setter +#define VOCAB_CC_GET VOCAB3('g','e','t') ///< Getter + + /** @} */ + /** * @name RPC vocabs * @@ -71,12 +88,13 @@ /** * @name Controller configuration vocabs * - * Used by @ref ICartesianControl_config_commands "Configuration accessors". + * Used by @ref ICartesianControl_config_commands "configuration accessors". * * @{ */ // Controller configuration (parameter keys) +#define VOCAB_CC_CONFIG_PARAMS VOCAB4('p','r','m','s') ///< Parameter group #define VOCAB_CC_CONFIG_GAIN VOCAB4('c','p','c','g') ///< Controller gain #define VOCAB_CC_CONFIG_MAX_JOINT_VEL VOCAB4('c','p','j','v') ///< Maximum joint velocity #define VOCAB_CC_CONFIG_TRAJ_DURATION VOCAB4('c','p','t','d') ///< Trajectory duration @@ -320,6 +338,28 @@ class ICartesianControl */ virtual bool getParameter(int vocab, double * value) = 0; + /** + * @brief Set multiple configuration parameters. + * + * Ask the controller to store or update multiple parameters at once. + * + * @param params Dictionary of YARP-encoded vocabs as keys and their values. + * + * @return true on success, false otherwise + */ + virtual bool setParameters(const std::map & params) = 0; + + /** + * @brief Retrieve multiple configuration parameters. + * + * Ask the controller to retrieve all available parameters at once. + * + * @param params Dictionary of YARP-encoded vocabs as keys and their values. + * + * @return true on success, false otherwise + */ + virtual bool getParameters(std::map & params) = 0; + /** @} */ };