diff --git a/libraries/YarpPlugins/CartesianControlServer/CMakeLists.txt b/libraries/YarpPlugins/CartesianControlServer/CMakeLists.txt index 141f66e97..bb6e93ed9 100644 --- a/libraries/YarpPlugins/CartesianControlServer/CMakeLists.txt +++ b/libraries/YarpPlugins/CartesianControlServer/CMakeLists.txt @@ -10,6 +10,7 @@ if(NOT SKIP_CartesianControlServer) yarp_add_plugin(CartesianControlServer CartesianControlServer.hpp DeviceDriverImpl.cpp + IWrapperImpl.cpp PeriodicThreadImpl.cpp RpcResponder.cpp StreamResponder.cpp diff --git a/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp b/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp index 93296ee23..1ce71d805 100644 --- a/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp +++ b/libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp @@ -12,6 +12,7 @@ #include #include +#include #include "ICartesianControl.h" #include "KinematicRepresentation.hpp" @@ -35,6 +36,7 @@ class StreamResponder; * @brief The CartesianControlServer class implements ICartesianControl server side. */ class CartesianControlServer : public yarp::dev::DeviceDriver, + public yarp::dev::WrapperSingle, public yarp::os::PeriodicThread { public: @@ -42,26 +44,29 @@ class CartesianControlServer : public yarp::dev::DeviceDriver, {} // -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp -------- - bool open(yarp::os::Searchable & config) override; bool close() override; + // -------- IWrapper declarations. Implementation in IWrapperImpl.cpp -------- + bool attach(yarp::dev::PolyDriver * poly) override; + bool detach() override; + // -------- PeriodicThread declarations. Implementation in PeriodicThreadImpl.cpp -------- void run() override; protected: + bool configureHandle(); + yarp::dev::PolyDriver cartesianControlDevice; yarp::os::RpcServer rpcServer, rpcTransformServer; yarp::os::BufferedPort fkOutPort, commandPort; - roboticslab::ICartesianControl * iCartesianControl {nullptr}; + ICartesianControl * iCartesianControl {nullptr}; RpcResponder * rpcResponder {nullptr}; RpcResponder * rpcTransformResponder {nullptr}; StreamResponder * streamResponder {nullptr}; - - bool fkStreamEnabled; }; /** @@ -71,24 +76,16 @@ class CartesianControlServer : public yarp::dev::DeviceDriver, class RpcResponder : public yarp::dev::DeviceResponder { public: - RpcResponder(roboticslab::ICartesianControl * _iCartesianControl) - : iCartesianControl(_iCartesianControl) + RpcResponder() { // shadows DeviceResponder::makeUsage(), which was already called by the base constructor makeUsage(); } - /** - * Respond to a message. - * @param in the message - * @param out the response - * @return true if there was no critical failure - */ - bool respond(const yarp::os::Bottle & in, yarp::os::Bottle & out) override; + void setHandle(ICartesianControl * _iCartesianControl) + { iCartesianControl = _iCartesianControl; } - /** - * Generate command usage information. - */ + bool respond(const yarp::os::Bottle & in, yarp::os::Bottle & out) override; void makeUsage(); protected: @@ -117,7 +114,7 @@ class RpcResponder : public yarp::dev::DeviceResponder bool handleParameterSetterGroup(const yarp::os::Bottle & in, yarp::os::Bottle & out); bool handleParameterGetterGroup(const yarp::os::Bottle & in, yarp::os::Bottle & out); - roboticslab::ICartesianControl * iCartesianControl; + ICartesianControl * iCartesianControl; }; /** @@ -127,12 +124,10 @@ class RpcResponder : public yarp::dev::DeviceResponder class RpcTransformResponder : public RpcResponder { public: - RpcTransformResponder(roboticslab::ICartesianControl * iCartesianControl, - KinRepresentation::coordinate_system coord, + RpcTransformResponder(KinRepresentation::coordinate_system coord, KinRepresentation::orientation_system orient, KinRepresentation::angular_units units) - : RpcResponder(iCartesianControl), - coord(coord), + : coord(coord), orient(orient), units(units) {} @@ -153,9 +148,8 @@ class RpcTransformResponder : public RpcResponder class StreamResponder : public yarp::os::TypedReaderCallback { public: - StreamResponder(roboticslab::ICartesianControl * _iCartesianControl) - : iCartesianControl(_iCartesianControl) - {} + void setHandle(ICartesianControl * _iCartesianControl) + { iCartesianControl = _iCartesianControl;} void onRead(yarp::os::Bottle & b) override; @@ -166,7 +160,7 @@ class StreamResponder : public yarp::os::TypedReaderCallback void handleConsumerCmdMsg(const yarp::os::Bottle & in, ConsumerFun cmd); void handleBiConsumerCmdMsg(const yarp::os::Bottle & in, BiConsumerFun cmd); - roboticslab::ICartesianControl * iCartesianControl; + ICartesianControl * iCartesianControl {nullptr}; }; } // namespace roboticslab diff --git a/libraries/YarpPlugins/CartesianControlServer/DeviceDriverImpl.cpp b/libraries/YarpPlugins/CartesianControlServer/DeviceDriverImpl.cpp index c1c5170fc..0328dc5e4 100644 --- a/libraries/YarpPlugins/CartesianControlServer/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/CartesianControlServer/DeviceDriverImpl.cpp @@ -40,36 +40,37 @@ bool CartesianControlServer::open(yarp::os::Searchable& config) if (!cartesianControlDevice.isValid()) { - yCError(CCS) << "Cannot make" << name->toString(); + yCError(CCS) << "Cartesian control device not valid"; + return false; + } + + if (!cartesianControlDevice.view(iCartesianControl)) + { + yCError(CCS) << "iCartesianControl view failed"; + return false; } } else { - yCError(CCS) << "Subdevice option not set in CartesianControlServer"; - return false; + yCInfo(CCS) << "Subdevice option not set, will use attach() later"; } - if (!cartesianControlDevice.isValid()) + auto prefix = config.check("name", yarp::os::Value(DEFAULT_PREFIX), "local port prefix").asString(); + + if (!rpcServer.open(prefix + "/rpc:s")) { - yCError(CCS) << "Cartesian control device not valid"; + yCError(CCS) << "Failed to open RPC port"; return false; } - if (!cartesianControlDevice.view(iCartesianControl)) + if (!commandPort.open(prefix + "/command:i")) { - yCError(CCS) << "iCartesianControl view failed"; + yCError(CCS) << "Failed to open command port"; return false; } - rpcResponder = new RpcResponder(iCartesianControl); - streamResponder = new StreamResponder(iCartesianControl); - - auto prefix = config.check("name", yarp::os::Value(DEFAULT_PREFIX), "local port prefix").asString(); - - bool ok = true; - - ok &= rpcServer.open(prefix + "/rpc:s"); - ok &= commandPort.open(prefix + "/command:i"); + rpcResponder = new RpcResponder(); + streamResponder = new StreamResponder(); rpcServer.setReader(*rpcResponder); commandPort.useCallback(*streamResponder); @@ -78,15 +79,13 @@ bool CartesianControlServer::open(yarp::os::Searchable& config) if (periodInMs > 0) { - fkStreamEnabled = true; - ok &= fkOutPort.open(prefix + "/state:o"); - yarp::os::PeriodicThread::setPeriod(periodInMs * 0.001); - yarp::os::PeriodicThread::start(); - } - else - { - fkStreamEnabled = false; + + if (!fkOutPort.open(prefix + "/state:o")) + { + yCError(CCS) << "Failed to open FK stream port"; + return false; + } } yarp::os::Value * angleRepr, * coordRepr, * angularUnits; @@ -143,22 +142,26 @@ bool CartesianControlServer::open(yarp::os::Searchable& config) if (openTransformPort) { - rpcTransformResponder = new RpcTransformResponder(iCartesianControl, coord, orient, units); - ok &= rpcTransformServer.open(prefix + "/rpc_transform:s"); + rpcTransformResponder = new RpcTransformResponder(coord, orient, units); rpcTransformServer.setReader(*rpcTransformResponder); + + if (!rpcTransformServer.open(prefix + "/rpc_transform:s")) + { + yCError(CCS) << "Failed to open transform RPC port"; + return false; + } } - return ok; + return !cartesianControlDevice.isValid() || configureHandle(); } // ----------------------------------------------------------------------------- bool CartesianControlServer::close() { - if (fkStreamEnabled) + if (!fkOutPort.isClosed()) { yarp::os::PeriodicThread::stop(); - fkOutPort.interrupt(); fkOutPort.close(); } @@ -181,7 +184,7 @@ bool CartesianControlServer::close() delete streamResponder; streamResponder = nullptr; - return cartesianControlDevice.close(); + return !cartesianControlDevice.isValid() || cartesianControlDevice.close(); } // ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/CartesianControlServer/IWrapperImpl.cpp b/libraries/YarpPlugins/CartesianControlServer/IWrapperImpl.cpp new file mode 100644 index 000000000..c0e2db9cc --- /dev/null +++ b/libraries/YarpPlugins/CartesianControlServer/IWrapperImpl.cpp @@ -0,0 +1,71 @@ +// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- + +#include "CartesianControlServer.hpp" + +#include + +#include "LogComponent.hpp" + +using namespace roboticslab; + +// ----------------------------------------------------------------------------- + +bool CartesianControlServer::attach(yarp::dev::PolyDriver * poly) +{ + if (poly == nullptr) + { + yCError(CCS) << "attach() received nullptr"; + return false; + } + + if (!poly->isValid()) + { + yCError(CCS) << "attach() received invalid PolyDriver"; + return false; + } + + if (!poly->view(iCartesianControl)) + { + yCError(CCS) << "attach() failed to obtain ICartesianControl interface"; + return false; + } + + return configureHandle(); +} + +// ----------------------------------------------------------------------------- + +bool CartesianControlServer::configureHandle() +{ + if (!iCartesianControl) + { + yCError(CCS) << "Invalid ICartesianControl interface"; + return false; + } + + rpcResponder->setHandle(iCartesianControl); + streamResponder->setHandle(iCartesianControl); + + if (rpcTransformResponder) + { + rpcTransformResponder->setHandle(iCartesianControl); + } + + if (!fkOutPort.isClosed()) + { + return yarp::os::PeriodicThread::start(); + } + + return true; +} + +// ----------------------------------------------------------------------------- + +bool CartesianControlServer::detach() +{ + yarp::os::PeriodicThread::stop(); + iCartesianControl = nullptr; + return true; +} + +// ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/CartesianControlServer/PeriodicThreadImpl.cpp b/libraries/YarpPlugins/CartesianControlServer/PeriodicThreadImpl.cpp index 8e14f292f..d3d055d58 100644 --- a/libraries/YarpPlugins/CartesianControlServer/PeriodicThreadImpl.cpp +++ b/libraries/YarpPlugins/CartesianControlServer/PeriodicThreadImpl.cpp @@ -2,37 +2,35 @@ #include "CartesianControlServer.hpp" -#include - using namespace roboticslab; // ------------------- PeriodicThread related ------------------------------------ void CartesianControlServer::run() { - std::vector x; - int state; - double timestamp; - - if (!iCartesianControl->stat(x, &state, ×tamp)) + if (iCartesianControl) { - return; + std::vector x; + int state; + double timestamp; + + if (!iCartesianControl->stat(x, &state, ×tamp)) + { + return; + } + + yarp::os::Bottle & out = fkOutPort.prepare(); + out.clear(); + out.addVocab32(state); + + for (auto i = 0; i < x.size(); i++) + { + out.addFloat64(x[i]); + } + + out.addFloat64(timestamp); + fkOutPort.write(); } - - yarp::os::Bottle & out = fkOutPort.prepare(); - out.clear(); - out.addVocab32(state); - - for (size_t i = 0; i < x.size(); i++) - { - out.addFloat64(x[i]); - } - - out.addFloat64(timestamp); - - fkOutPort.write(); - - return; } // ----------------------------------------------------------------------------- diff --git a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp index 035cf5e3e..eea5ffe70 100644 --- a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp +++ b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp @@ -51,6 +51,13 @@ namespace bool RpcResponder::respond(const yarp::os::Bottle& in, yarp::os::Bottle& out) { + if (!iCartesianControl) + { + yCError(CCS) << "Invalid ICartesianControl interface"; + out.addVocab32(VOCAB_CC_FAILED); + return false; + } + switch (in.get(0).asVocab32()) { case VOCAB_CC_STAT: diff --git a/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp index d1f57cf13..325d5bad0 100644 --- a/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp +++ b/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp @@ -3,7 +3,6 @@ #include "CartesianControlServer.hpp" #include // std::invoke -#include #include @@ -17,6 +16,12 @@ void StreamResponder::onRead(yarp::os::Bottle& b) { yCDebug(CCS, "Got: %s", b.toString().c_str()); + if (!iCartesianControl) + { + yCError(CCS) << "Invalid ICartesianControl interface"; + return; + } + switch (b.get(0).asVocab32()) { case VOCAB_CC_POSE: