From bedf3ea86a94523aa53f217e7d929ae72a47de2c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Mon, 22 Jan 2018 21:39:58 +0100 Subject: [PATCH 1/4] Use own set of general-purpose vocabs (ok/fail...) Partially reverts 7d2e073 (#118). --- .../CartesianControlClient.hpp | 2 -- .../ICartesianControlImpl.cpp | 17 +++++----- .../CartesianControlServer/RpcResponder.cpp | 34 +++++++++---------- libraries/YarpPlugins/ICartesianControl.h | 18 +++++++++- 4 files changed, 41 insertions(+), 30 deletions(-) diff --git a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp index cb84e71e9..9eb87d914 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 diff --git a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp index 84f235a2f..2f777646c 100644 --- a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp @@ -3,7 +3,6 @@ #include "CartesianControlClient.hpp" #include -#include // vocabs: OK, FAIL, SET, GET #include @@ -17,7 +16,7 @@ bool roboticslab::CartesianControlClient::handleRpcRunnableCmd(int vocab) rpcClient.write(cmd, response); - if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED) + if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_CC_FAILED) { return false; } @@ -40,7 +39,7 @@ bool roboticslab::CartesianControlClient::handleRpcConsumerCmd(int vocab, const rpcClient.write(cmd, response); - if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED) + if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_CC_FAILED) { return false; } @@ -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 (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_CC_FAILED) { 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 (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_CC_FAILED) { return false; } @@ -232,13 +231,13 @@ 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) + if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_CC_FAILED) { return false; } @@ -252,12 +251,12 @@ bool roboticslab::CartesianControlClient::getParameter(int vocab, double * value { yarp::os::Bottle cmd, response; - cmd.addVocab(VOCAB_GET); + cmd.addVocab(VOCAB_CC_GET); cmd.addVocab(vocab); rpcClient.write(cmd, response); - if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_FAILED) + if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_CC_FAILED) { return false; } diff --git a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp index 06441c85c..2ff96c56a 100644 --- a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp +++ b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp @@ -2,8 +2,6 @@ #include "CartesianControlServer.hpp" -#include // vocabs: OK, FAIL, SET, GET - #include // ------------------- RpcResponder Related ------------------------------------ @@ -35,9 +33,9 @@ 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: + case VOCAB_CC_SET: return handleParameterSetter(in, out); - case VOCAB_GET: + case VOCAB_CC_GET: return handleParameterGetter(in, out); default: return DeviceResponder::respond(in, out); @@ -73,7 +71,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 +86,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 +97,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 +122,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 +152,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 +166,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 +182,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 +208,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 +218,7 @@ 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; } } diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h index 5976164d1..907597e21 100644 --- a/libraries/YarpPlugins/ICartesianControl.h +++ b/libraries/YarpPlugins/ICartesianControl.h @@ -14,6 +14,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,7 +87,7 @@ /** * @name Controller configuration vocabs * - * Used by @ref ICartesianControl_config_commands "Configuration accessors". + * Used by @ref ICartesianControl_config_commands "configuration accessors". * * @{ */ From 698bb2205ab58777851232ba63876efa161e0571 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Mon, 22 Jan 2018 22:56:28 +0100 Subject: [PATCH 2/4] Add group config param accessors, implement comms --- .../AmorCartesianControl.hpp | 4 + .../ICartesianControlImpl.cpp | 14 +++ .../BasicCartesianControl.hpp | 4 + .../ICartesianControlImpl.cpp | 14 +++ .../CartesianControlClient.hpp | 4 + .../ICartesianControlImpl.cpp | 80 +++++++++++++---- .../CartesianControlServer.hpp | 3 + .../CartesianControlServer/RpcResponder.cpp | 88 ++++++++++++++++++- libraries/YarpPlugins/ICartesianControl.h | 24 +++++ 9 files changed, 212 insertions(+), 23 deletions(-) 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..6770769f2 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp @@ -419,3 +419,17 @@ bool roboticslab::AmorCartesianControl::getParameter(int vocab, double * value) } // ----------------------------------------------------------------------------- + +bool roboticslab::AmorCartesianControl::setParameters(const std::map & params) +{ + return true; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::AmorCartesianControl::getParameters(std::map & params) +{ + 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..c2cb3ca36 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -512,3 +512,17 @@ bool roboticslab::BasicCartesianControl::getParameter(int vocab, double * value) } // ----------------------------------------------------------------------------- + +bool roboticslab::BasicCartesianControl::setParameters(const std::map & params) +{ + return true; +} + +// ----------------------------------------------------------------------------- + +bool roboticslab::BasicCartesianControl::getParameters(std::map & params) +{ + return true; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp index 9eb87d914..79b05ca79 100644 --- a/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp +++ b/libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp @@ -93,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 2f777646c..0aae27ddc 100644 --- a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp @@ -6,6 +6,16 @@ #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) @@ -16,12 +26,7 @@ bool roboticslab::CartesianControlClient::handleRpcRunnableCmd(int vocab) rpcClient.write(cmd, response); - if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_CC_FAILED) - { - return false; - } - - return true; + return checkSuccess(response); } // ----------------------------------------------------------------------------- @@ -39,12 +44,7 @@ bool roboticslab::CartesianControlClient::handleRpcConsumerCmd(int vocab, const rpcClient.write(cmd, response); - if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_CC_FAILED) - { - return false; - } - - return true; + return checkSuccess(response); } // ----------------------------------------------------------------------------- @@ -62,7 +62,7 @@ bool roboticslab::CartesianControlClient::handleRpcFunctionCmd(int vocab, const rpcClient.write(cmd, response); - if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_CC_FAILED) + if (!checkSuccess(response)) { return false; } @@ -132,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_CC_FAILED) + if (!checkSuccess(response)) { return false; } @@ -237,31 +237,73 @@ bool roboticslab::CartesianControlClient::setParameter(int vocab, double value) rpcClient.write(cmd, response); - if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_CC_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_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); + cmd.addVocab(VOCAB_CC_CONFIG_PARAMS); rpcClient.write(cmd, response); - if (response.get(0).isVocab() && response.get(0).asVocab() == VOCAB_CC_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 2ff96c56a..745044c58 100644 --- a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp +++ b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp @@ -4,6 +4,16 @@ #include +// ----------------------------------------------------------------------------- + +namespace +{ + inline bool isGroupParam(const yarp::os::Bottle& in) + { + return in.size() > 1 && in.get(1).isVocab() && in.get(1).asVocab() == VOCAB_CC_CONFIG_PARAMS; + } +} + // ------------------- RpcResponder Related ------------------------------------ bool roboticslab::RpcResponder::respond(const yarp::os::Bottle& in, yarp::os::Bottle& out) @@ -34,9 +44,9 @@ bool roboticslab::RpcResponder::respond(const yarp::os::Bottle& in, yarp::os::Bo case VOCAB_CC_TOOL: return handleConsumerCmdMsg(in, out, &ICartesianControl::tool); case VOCAB_CC_SET: - return handleParameterSetter(in, out); + return isGroupParam(in) ? handleParameterSetterGroup(in, out) : handleParameterSetter(in, out); case VOCAB_CC_GET: - return handleParameterGetter(in, out); + return isGroupParam(in) ? handleParameterGetterGroup(in, out) : handleParameterGetter(in, out); default: return DeviceResponder::respond(in, out); } @@ -56,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"); } // ----------------------------------------------------------------------------- @@ -225,6 +237,74 @@ bool roboticslab::RpcResponder::handleParameterGetter(const yarp::os::Bottle& in // ----------------------------------------------------------------------------- +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"); + 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; + } + + 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; + } +} + +// ----------------------------------------------------------------------------- + bool roboticslab::RpcTransformResponder::transformIncomingData(std::vector& vin) { return KinRepresentation::encodePose(vin, vin, KinRepresentation::CARTESIAN, orient, KinRepresentation::DEGREES); diff --git a/libraries/YarpPlugins/ICartesianControl.h b/libraries/YarpPlugins/ICartesianControl.h index 907597e21..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 @@ -93,6 +94,7 @@ */ // 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 @@ -336,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; + /** @} */ }; From 14ea8046ca0bbe6c1e58c52e75d68e53a29d5397 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Mon, 22 Jan 2018 23:03:36 +0100 Subject: [PATCH 3/4] Implement group config param accessors in CC impls --- .../AmorCartesianControl/ICartesianControlImpl.cpp | 12 +++++++++++- .../ICartesianControlImpl.cpp | 14 +++++++++++++- 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp index 6770769f2..45cbc5921 100644 --- a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp @@ -422,13 +422,23 @@ bool roboticslab::AmorCartesianControl::getParameter(int vocab, double * value) bool roboticslab::AmorCartesianControl::setParameters(const std::map & params) { - return true; + 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/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp index c2cb3ca36..b5f1223fc 100644 --- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp +++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp @@ -515,13 +515,25 @@ bool roboticslab::BasicCartesianControl::getParameter(int vocab, double * value) bool roboticslab::BasicCartesianControl::setParameters(const std::map & params) { - return true; + 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; } From f8dd1fb6bab8290deb1e7af0a036cf6c2db4e1f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bartek=20=C5=81ukawski?= Date: Tue, 23 Jan 2018 01:37:39 +0100 Subject: [PATCH 4/4] Fix checks and missing acks, allow string cmds That is, when not checking isVocab(), CLI users may pass 'prms' instead of '[prms]'. --- .../YarpPlugins/CartesianControlServer/RpcResponder.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp index 745044c58..bc2c3e2f5 100644 --- a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp +++ b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp @@ -10,7 +10,7 @@ namespace { inline bool isGroupParam(const yarp::os::Bottle& in) { - return in.size() > 1 && in.get(1).isVocab() && in.get(1).asVocab() == VOCAB_CC_CONFIG_PARAMS; + return in.size() > 1 && in.get(1).asVocab() == VOCAB_CC_CONFIG_PARAMS; } } @@ -248,6 +248,7 @@ bool roboticslab::RpcResponder::handleParameterSetterGroup(const yarp::os::Bottl if (!in.get(i).isList() || in.get(i).asList()->size() != 2) { CD_ERROR("bottle format error\n"); + out.addVocab(VOCAB_CC_FAILED); return false; } @@ -262,6 +263,7 @@ bool roboticslab::RpcResponder::handleParameterSetterGroup(const yarp::os::Bottl return false; } + out.addVocab(VOCAB_CC_OK); return true; } else @@ -276,7 +278,7 @@ bool roboticslab::RpcResponder::handleParameterSetterGroup(const yarp::os::Bottl bool roboticslab::RpcResponder::handleParameterGetterGroup(const yarp::os::Bottle& in, yarp::os::Bottle& out) { - if (in.size() != 2) + if (in.size() == 2) { std::map params;