From ae4acbcd12b3221a4e6b1cd1936c57234260e66a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 28 Apr 2014 14:26:56 +0400 Subject: [PATCH] Refactored nodetool --- libuavcan_drivers/linux/CMakeLists.txt | 4 +- .../{test_param.cpp => test_nodetool.cpp} | 147 ++++++++++-------- 2 files changed, 86 insertions(+), 65 deletions(-) rename libuavcan_drivers/linux/test/{test_param.cpp => test_nodetool.cpp} (60%) diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index 3fa03f9aaf..c7fd39ea71 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -46,5 +46,5 @@ target_link_libraries(test_node_status_monitor ${UAVCAN_LIB} rt) add_executable(test_time_sync test/test_time_sync.cpp) target_link_libraries(test_time_sync ${UAVCAN_LIB} rt) -add_executable(test_param test/test_param.cpp) -target_link_libraries(test_param ${UAVCAN_LIB} rt) +add_executable(test_nodetool test/test_nodetool.cpp) +target_link_libraries(test_nodetool ${UAVCAN_LIB} rt) diff --git a/libuavcan_drivers/linux/test/test_param.cpp b/libuavcan_drivers/linux/test/test_nodetool.cpp similarity index 60% rename from libuavcan_drivers/linux/test/test_param.cpp rename to libuavcan_drivers/linux/test/test_nodetool.cpp index 0dd3ddb38d..25eb5a0c25 100644 --- a/libuavcan_drivers/linux/test/test_param.cpp +++ b/libuavcan_drivers/linux/test/test_nodetool.cpp @@ -119,20 +119,86 @@ uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::N return node; } +template +typename DataType::Response call(uavcan_linux::BlockingServiceClient& client, + uavcan::NodeID server_node_id, const typename DataType::Request& request) +{ + const int res = client.blockingCall(server_node_id, request, uavcan::MonotonicDuration::fromMSec(100)); + ENFORCE(res >= 0); + ENFORCE(client.wasSuccessful()); + return client.getResponse(); +} + +void executeCommand(const uavcan_linux::NodePtr& node, const std::string& cmd, + const uavcan::NodeID node_id, const std::vector& args) +{ + if (cmd == "param") + { + uavcan_linux::BlockingServiceClient get_set(*node); + printGetSetResponseHeader(); + uavcan::protocol::param::GetSet::Request request; + if (args.empty()) + { + while (true) + { + auto response = call(get_set, node_id, request); + if (response.name.empty()) + { + break; + } + printGetSetResponse(response); + request.index++; + } + } + else + { + 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)); + } + } + else if (cmd == "param_save" || cmd == "param_erase") + { + uavcan_linux::BlockingServiceClient save_erase(*node); + 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; + } + else if (cmd == "restart") + { + uavcan_linux::BlockingServiceClient restart(*node); + uavcan::protocol::RestartNode::Request request; + request.magic_number = request.MAGIC_NUMBER; + (void)restart.blockingCall(node_id, request); + if (restart.wasSuccessful()) + { + std::cout << restart.getResponse() << std::endl; + } + else + { + std::cout << "" << std::endl; + } + } + else if (cmd == "info") + { + uavcan_linux::BlockingServiceClient client(*node); + 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; + } + else + { + std::cout << "Invalid command" << std::endl; + } +} + void runForever(const uavcan_linux::NodePtr& node) { - uavcan_linux::BlockingServiceClient get_set(*node); - uavcan_linux::BlockingServiceClient save_erase(*node); - uavcan_linux::BlockingServiceClient restart(*node); - - ENFORCE(get_set.init() >= 0); - ENFORCE(save_erase.init() >= 0); - ENFORCE(restart.init() >= 0); - StdinLineReader stdin_reader; - std::cout << "> " << std::flush; - while (true) { ENFORCE(node->spin(uavcan::MonotonicDuration::fromMSec(10)) >= 0); @@ -140,68 +206,23 @@ void runForever(const uavcan_linux::NodePtr& node) { continue; } - const auto words = stdin_reader.getSplitLine(); - if (words.size() < 2) + if (words.size() >= 2) { - std::cout << " [args...]" << std::endl; - continue; - } - - const auto cmd = words.at(0); - const uavcan::NodeID node_id(std::stoi(words.at(1))); - - if (cmd == "read") - { - printGetSetResponseHeader(); - uavcan::protocol::param::GetSet::Request request; - while (true) + const auto cmd = words.at(0); + const uavcan::NodeID node_id(std::stoi(words.at(1))); + try { - (void)get_set.blockingCall(node_id, request, uavcan::MonotonicDuration::fromMSec(100)); - ENFORCE(get_set.wasSuccessful()); - if (get_set.getResponse().name.empty()) - { - break; - } - printGetSetResponse(get_set.getResponse()); - request.index++; + executeCommand(node, cmd, node_id, std::vector(words.begin() + 2, words.end())); } - } - else if (cmd == "set") - { - printGetSetResponseHeader(); - uavcan::protocol::param::GetSet::Request request; - request.name = words.at(2).c_str(); - request.value.value_float.push_back(std::stof(words.at(3))); - (void)get_set.blockingCall(node_id, request, uavcan::MonotonicDuration::fromMSec(100)); - ENFORCE(get_set.wasSuccessful()); - printGetSetResponse(get_set.getResponse()); - } - else if (cmd == "save" || cmd == "erase") - { - uavcan::protocol::param::SaveErase::Request request; - request.opcode = (cmd == "save") ? request.OPCODE_SAVE : request.OPCODE_ERASE; - (void)save_erase.blockingCall(node_id, request, uavcan::MonotonicDuration::fromMSec(100)); - ENFORCE(save_erase.wasSuccessful()); - std::cout << save_erase.getResponse() << std::endl; - } - else if (cmd == "restart") - { - uavcan::protocol::RestartNode::Request request; - request.magic_number = request.MAGIC_NUMBER; - (void)restart.blockingCall(node_id, request, uavcan::MonotonicDuration::fromMSec(100)); - if (restart.wasSuccessful()) + catch (std::exception& ex) { - std::cout << restart.getResponse() << std::endl; - } - else - { - std::cout << "" << std::endl; + std::cout << "FAILURE\n" << ex.what() << std::endl; } } else { - std::cout << "Invalid command" << std::endl; + std::cout << " [args...]" << std::endl; } std::cout << "> " << std::flush; }