diff --git a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp index 164e183c9c..e4be9f38d4 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp @@ -27,6 +27,70 @@ class DefaultLogSink : public uavcan::ILogSink } }; +/** + * Wrapper over uavcan::ServiceClient<> for blocking calls. + * Calls spin() internally. + */ +template +class BlockingServiceClient : public uavcan::ServiceClient +{ + typedef uavcan::ServiceClient Super; + + typename DataType::Response response_; + bool call_was_successful_; + + void callback(const uavcan::ServiceCallResult& res) + { + response_ = res.response; + call_was_successful_ = res.isSuccessful(); + } + + void setup() + { + Super::setCallback(std::bind(&BlockingServiceClient::callback, this, std::placeholders::_1)); + call_was_successful_ = false; + response_ = typename DataType::Response(); + } + +public: + BlockingServiceClient(uavcan::INode& node) + : uavcan::ServiceClient(node) + , call_was_successful_(false) + { + setup(); + } + + int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request) + { + const auto SpinDuration = uavcan::MonotonicDuration::fromMSec(2); + setup(); + const int call_res = Super::call(server_node_id, request); + if (call_res >= 0) + { + while (Super::isPending()) + { + const int spin_res = Super::getNode().spin(SpinDuration); + if (spin_res < 0) + { + return spin_res; + } + } + } + return call_res; + } + + int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request, + uavcan::MonotonicDuration timeout) + { + Super::setRequestTimeout(timeout); + return blockingCall(server_node_id, request); + } + + bool wasSuccessful() const { return call_was_successful_; } + + const typename DataType::Response& getResponse() const { return response_; } +}; + /** * Contains all drivers needed for uavcan::Node. */ @@ -129,6 +193,15 @@ public: return p; } + template + std::shared_ptr> + makeBlockingServiceClient() + { + std::shared_ptr> p(new BlockingServiceClient(*this)); + enforce(p->init(), "BlockingServiceClient init failure " + getDataTypeName()); + return p; + } + TimerPtr makeTimer(uavcan::MonotonicTime deadline, const typename uavcan::Timer::Callback& cb) { TimerPtr p(new uavcan::Timer(*this)); @@ -172,68 +245,4 @@ static inline NodePtr makeNode(const std::vector& iface_names) return makeNode(iface_names, SystemClock::detectPreferredClockAdjustmentMode()); } -/** - * Wrapper over uavcan::ServiceClient<> for blocking calls. - * Calls spin() internally. - */ -template -class BlockingServiceClient : public uavcan::ServiceClient -{ - typedef uavcan::ServiceClient Super; - - typename DataType::Response response_; - bool call_was_successful_; - - void callback(const uavcan::ServiceCallResult& res) - { - response_ = res.response; - call_was_successful_ = res.isSuccessful(); - } - - void setup() - { - Super::setCallback(std::bind(&BlockingServiceClient::callback, this, std::placeholders::_1)); - call_was_successful_ = false; - response_ = typename DataType::Response(); - } - -public: - BlockingServiceClient(uavcan::INode& node) - : uavcan::ServiceClient(node) - , call_was_successful_(false) - { - setup(); - } - - int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request) - { - const auto SpinDuration = uavcan::MonotonicDuration::fromMSec(2); - setup(); - const int call_res = Super::call(server_node_id, request); - if (call_res >= 0) - { - while (Super::isPending()) - { - const int spin_res = Super::getNode().spin(SpinDuration); - if (spin_res < 0) - { - return spin_res; - } - } - } - return call_res; - } - - int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request, - uavcan::MonotonicDuration timeout) - { - Super::setRequestTimeout(timeout); - return blockingCall(server_node_id, request); - } - - bool wasSuccessful() const { return call_was_successful_; } - - const typename DataType::Response& getResponse() const { return response_; } -}; - } diff --git a/libuavcan_drivers/linux/test/test_nodetool.cpp b/libuavcan_drivers/linux/test/test_nodetool.cpp index b16039522a..7c932559df 100644 --- a/libuavcan_drivers/linux/test/test_nodetool.cpp +++ b/libuavcan_drivers/linux/test/test_nodetool.cpp @@ -135,14 +135,14 @@ void executeCommand(const uavcan_linux::NodePtr& node, const std::string& cmd, { if (cmd == "param") { - uavcan_linux::BlockingServiceClient get_set(*node); + auto client = node->makeBlockingServiceClient(); printGetSetResponseHeader(); uavcan::protocol::param::GetSet::Request request; if (args.empty()) { while (true) { - auto response = call(get_set, node_id, request); + auto response = call(*client, node_id, request); if (response.name.empty()) { break; @@ -155,25 +155,25 @@ void executeCommand(const uavcan_linux::NodePtr& node, const std::string& cmd, { request.name = args.at(0).c_str(); request.value.value_float.push_back(std::stof(args.at(1))); - printGetSetResponse(call(get_set, node_id, request)); + printGetSetResponse(call(*client, node_id, request)); } } else if (cmd == "param_save" || cmd == "param_erase") { - uavcan_linux::BlockingServiceClient save_erase(*node); + auto client = node->makeBlockingServiceClient(); uavcan::protocol::param::SaveErase::Request request; request.opcode = (cmd == "param_save") ? request.OPCODE_SAVE : request.OPCODE_ERASE; - std::cout << call(save_erase, node_id, request) << std::endl; + std::cout << call(*client, node_id, request) << std::endl; } else if (cmd == "restart") { - uavcan_linux::BlockingServiceClient restart(*node); + auto client = node->makeBlockingServiceClient(); uavcan::protocol::RestartNode::Request request; request.magic_number = request.MAGIC_NUMBER; - (void)restart.blockingCall(node_id, request); - if (restart.wasSuccessful()) + (void)client->blockingCall(node_id, request); + if (client->wasSuccessful()) { - std::cout << restart.getResponse() << std::endl; + std::cout << client->getResponse() << std::endl; } else { @@ -182,13 +182,13 @@ void executeCommand(const uavcan_linux::NodePtr& node, const std::string& cmd, } else if (cmd == "info") { - uavcan_linux::BlockingServiceClient client(*node); - std::cout << call(client, node_id, uavcan::protocol::GetNodeInfo::Request()) << std::endl; + auto client = node->makeBlockingServiceClient(); + std::cout << call(*client, node_id, uavcan::protocol::GetNodeInfo::Request()) << std::endl; } else if (cmd == "tstat") { - uavcan_linux::BlockingServiceClient client(*node); - std::cout << call(client, node_id, uavcan::protocol::GetTransportStats::Request()) << std::endl; + auto client = node->makeBlockingServiceClient(); + std::cout << call(*client, node_id, uavcan::protocol::GetTransportStats::Request()) << std::endl; } else if (cmd == "hardpoint") {