From 4a2df2975a0b7fe818dcc68ec6ba771c83c68693 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 28 Apr 2014 01:51:13 +0400 Subject: [PATCH] Linux: New test util --- libuavcan_drivers/linux/CMakeLists.txt | 3 + libuavcan_drivers/linux/test/test_param.cpp | 228 ++++++++++++++++++++ 2 files changed, 231 insertions(+) create mode 100644 libuavcan_drivers/linux/test/test_param.cpp diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index 4252ec1b3e..3fa03f9aaf 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -45,3 +45,6 @@ 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) diff --git a/libuavcan_drivers/linux/test/test_param.cpp b/libuavcan_drivers/linux/test/test_param.cpp new file mode 100644 index 0000000000..0dd3ddb38d --- /dev/null +++ b/libuavcan_drivers/linux/test/test_param.cpp @@ -0,0 +1,228 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "debug.hpp" + +#include +#include + +namespace +{ + +class StdinLineReader +{ + mutable std::mutex mutex_; + std::thread thread_; + std::queue queue_; + + void worker() + { + while (true) + { + std::string input; + std::getline(std::cin, input); + std::lock_guard lock(mutex_); + queue_.push(input); + } + } + +public: + StdinLineReader() + : thread_(&StdinLineReader::worker, this) + { + thread_.detach(); + } + + bool hasPendingInput() const + { + std::lock_guard lock(mutex_); + return !queue_.empty(); + } + + std::string getLine() + { + std::lock_guard lock(mutex_); + if (queue_.empty()) + { + throw std::runtime_error("Input queue is empty"); + } + auto ret = queue_.front(); + queue_.pop(); + return ret; + } + + std::vector getSplitLine() + { + std::istringstream iss(getLine()); + std::vector out; + std::copy(std::istream_iterator(iss), std::istream_iterator(), + std::back_inserter(out)); + return out; + } +}; + + +std::string paramValueToString(const uavcan::protocol::param::Value& value) +{ + if (!value.value_bool.empty()) + { + return value.value_bool[0] ? "true" : "false"; + } + else if (!value.value_int.empty()) + { + return std::to_string(value.value_int[0]); + } + else if (!value.value_float.empty()) + { + return std::to_string(value.value_float[0]); + } + else + { + return "?"; + } +} + +void printGetSetResponseHeader() +{ + std::cout + << "Name Value Default Min Max\n" + << "--------------------------------------------------------------------------------------------------" + << std::endl; +} + +void printGetSetResponse(const uavcan::protocol::param::GetSet::Response& resp) +{ + std::cout << std::setw(41) << std::left << resp.name.c_str(); + std::cout << std::setw(15) << paramValueToString(resp.value); + std::cout << std::setw(15) << paramValueToString(resp.default_value); + std::cout << std::setw(15) << paramValueToString(resp.min_value); + std::cout << std::setw(15) << paramValueToString(resp.max_value); + std::cout << std::endl; +} + +uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::NodeID nid, const std::string& name) +{ + auto node = uavcan_linux::makeNode(ifaces); + node->setNodeID(nid); + node->setName(name.c_str()); + ENFORCE(0 == node->start()); // This node doesn't check its network compatibility + node->setStatusOk(); + return node; +} + +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); + if (!stdin_reader.hasPendingInput()) + { + continue; + } + + const auto words = stdin_reader.getSplitLine(); + 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) + { + (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++; + } + } + 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()) + { + std::cout << restart.getResponse() << std::endl; + } + else + { + std::cout << "" << std::endl; + } + } + else + { + std::cout << "Invalid command" << std::endl; + } + std::cout << "> " << std::flush; + } +} + +} + +int main(int argc, const char** argv) +{ + if (argc < 3) + { + std::cout << "Usage:\n\t" << argv[0] << " [can-iface-name-N...]" << std::endl; + return 1; + } + const int self_node_id = std::stoi(argv[1]); + std::vector iface_names; + for (int i = 2; i < argc; i++) + { + iface_names.emplace_back(argv[i]); + } + uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.linux_test_node_status_monitor"); + runForever(node); + return 0; +}