From 25371abe3b678a0661816e212b5b69b43104f1f3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 8 Jun 2015 14:59:44 +0300 Subject: [PATCH] Partially complete test of a multithreaded application Linux. It works, but it doesn't use iface sharing yet. --- libuavcan_drivers/linux/CMakeLists.txt | 3 + .../linux/apps/test_multithreading.cpp | 135 ++++++++++++++++++ .../linux/include/uavcan_linux/helpers.hpp | 103 ++++++++++--- 3 files changed, 221 insertions(+), 20 deletions(-) create mode 100644 libuavcan_drivers/linux/apps/test_multithreading.cpp diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index d4be18302c..0e35644862 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -66,6 +66,9 @@ target_link_libraries(test_dynamic_node_id_client ${UAVCAN_LIB} rt ${CMAKE_THREA add_executable(test_file_server apps/test_file_server.cpp) target_link_libraries(test_file_server ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) +add_executable(test_multithreading apps/test_multithreading.cpp) +target_link_libraries(test_multithreading ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) + # # Tools # diff --git a/libuavcan_drivers/linux/apps/test_multithreading.cpp b/libuavcan_drivers/linux/apps/test_multithreading.cpp new file mode 100644 index 0000000000..acbeda3ec1 --- /dev/null +++ b/libuavcan_drivers/linux/apps/test_multithreading.cpp @@ -0,0 +1,135 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include "debug.hpp" + +static uavcan_linux::NodePtr initMainNode(const std::vector& ifaces, uavcan::NodeID nid, + const std::string& name) +{ + std::cout << "Initializing main node" << std::endl; + + auto node = uavcan_linux::makeNode(ifaces); + + node->setNodeID(nid); + node->setName(name.c_str()); + + node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + + const int start_res = node->start(); + ENFORCE(0 == start_res); + + uavcan::NetworkCompatibilityCheckResult init_result; + ENFORCE(0 == node->checkNetworkCompatibility(init_result)); + if (!init_result.isOk()) + { + throw std::runtime_error("Network conflict with node " + std::to_string(init_result.conflicting_node.get())); + } + + node->setStatusOk(); + return node; +} + +static uavcan_linux::SubNodePtr initSubNode(const std::vector& ifaces, uavcan::NodeID nid) +{ + std::cout << "Initializing sub node" << std::endl; + auto node = uavcan_linux::makeSubNode(ifaces); + node->setNodeID(nid); + return node; +} + +static void runMainNode(const uavcan_linux::NodePtr& node) +{ + std::cout << "Running main node" << std::endl; + + auto do_nothing_once_a_minute = [&node](const uavcan::TimerEvent&) + { + node->logInfo("timer", "Another minute passed..."); + node->setVendorSpecificStatusCode(static_cast(std::rand())); // Setting to an arbitrary value + }; + auto timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(60000), do_nothing_once_a_minute); + + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::getInfinite()); + if (res < 0) + { + node->logError("spin", "Error %*", res); + } + } +} + +static void runSubNode(const uavcan_linux::SubNodePtr& node) +{ + std::cout << "Running sub node" << std::endl; + + auto log_handler = [](const uavcan::ReceivedDataStructure& msg) + { + std::cout << msg << std::endl; + }; + auto log_sub = node->makeSubscriber(log_handler); + + struct NodeStatusMonitor : public uavcan::NodeStatusMonitor + { + explicit NodeStatusMonitor(uavcan::INode& node) : uavcan::NodeStatusMonitor(node) { } + + virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event) override + { + std::cout << "Remote node NID " << int(event.node_id.get()) << " changed status: " + << int(event.old_status.status_code) << " --> " + << int(event.status.status_code) << std::endl; + } + }; + + NodeStatusMonitor nsm(*node); + ENFORCE(0 == nsm.start()); + + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::getInfinite()); + if (res < 0) + { + std::cerr << "SubNode spin error: " << res << std::endl; + } + } +} + +int main(int argc, const char** argv) +{ + try + { + if (argc < 3) + { + std::cerr << "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(argv + 2, argv + argc); + + auto node = initMainNode(iface_names, self_node_id, "org.uavcan.linux_test_node"); + auto sub_node = initSubNode(iface_names, self_node_id); + + std::thread sub_thread([&sub_node](){ runSubNode(sub_node); }); + + runMainNode(node); + + if (sub_thread.joinable()) + { + std::cout << "Waiting for the sub thread to join" << std::endl; + sub_thread.join(); + } + + return 0; + } + catch (const std::exception& ex) + { + std::cerr << "Exception: " << ex.what() << std::endl; + return 1; + } +} diff --git a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp index d27cad4833..8e94cd0992 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp @@ -11,6 +11,7 @@ #include #include #include +#include namespace uavcan_linux { @@ -125,18 +126,19 @@ struct DriverPack typedef std::shared_ptr DriverPackPtr; +typedef std::shared_ptr INodePtr; typedef std::shared_ptr TimerPtr; static constexpr std::size_t NodeMemPoolSize = 1024 * 512; ///< This shall be enough for any possible use case /** - * Wrapper for uavcan::Node with some additional convenience functions. - * Note that this wrapper adds stderr log sink to @ref uavcan::Logger, which can be removed if needed. + * Generic wrapper for node objects with some additional convenience functions. */ -class Node : public uavcan::Node +template +class NodeBase : public NodeType { +protected: DriverPackPtr driver_pack_; - DefaultLogSink log_sink_; static void enforce(int error, const std::string& msg) { @@ -158,21 +160,17 @@ public: /** * Simple forwarding constructor, compatible with uavcan::Node. */ - Node(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) - : uavcan::Node(can_driver, clock) - { - getLogger().setExternalSink(&log_sink_); - } + NodeBase(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) : + NodeType(can_driver, clock) + { } /** * Takes ownership of the driver container via the shared pointer. */ - explicit Node(DriverPackPtr driver_pack) - : uavcan::Node(driver_pack->can, driver_pack->clock) + explicit NodeBase(DriverPackPtr driver_pack) + : NodeType(driver_pack->can, driver_pack->clock) , driver_pack_(driver_pack) - { - getLogger().setExternalSink(&log_sink_); - } + { } /** * Allocates @ref uavcan::Subscriber in the heap using shared pointer. @@ -274,14 +272,66 @@ public: DriverPackPtr& getDriverPack() { return driver_pack_; } }; -typedef std::shared_ptr NodePtr; +/** + * Wrapper for uavcan::Node with some additional convenience functions. + * Note that this wrapper adds stderr log sink to @ref uavcan::Logger, which can be removed if needed. + */ +class Node : public NodeBase> +{ + typedef NodeBase> Base; + + DefaultLogSink log_sink_; + +public: + /** + * Simple forwarding constructor, compatible with uavcan::Node. + */ + Node(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) : + Base(can_driver, clock) + { + getLogger().setExternalSink(&log_sink_); + } + + /** + * Takes ownership of the driver container via the shared pointer. + */ + explicit Node(DriverPackPtr driver_pack) : + Base(driver_pack) + { + getLogger().setExternalSink(&log_sink_); + } +}; /** - * Constructs Node with explicitly specified ClockAdjustmentMode. - * Please consider using the overload with fewer parameters instead. + * Wrapper for uavcan::SubNode with some additional convenience functions. + */ +class SubNode : public NodeBase> +{ + typedef NodeBase> Base; + +public: + /** + * Simple forwarding constructor, compatible with uavcan::Node. + */ + SubNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) : Base(can_driver, clock) { } + + /** + * Takes ownership of the driver container via the shared pointer. + */ + explicit SubNode(DriverPackPtr driver_pack) : Base(driver_pack) { } +}; + +typedef std::shared_ptr NodePtr; +typedef std::shared_ptr SubNodePtr; + +/** + * Constructs a node object of specified type with explicitly specified ClockAdjustmentMode. + * Please consider using the static instantiation methods instead. * @throws uavcan_linux::Exception. */ -static inline NodePtr makeNode(const std::vector& iface_names, ClockAdjustmentMode clock_adjustment_mode) +template +static inline std::shared_ptr makeAnyNodeWithCustomClockAdjustmentMode(const std::vector& iface_names, + ClockAdjustmentMode clock_adjustment_mode) { DriverPackPtr dp(new DriverPack(clock_adjustment_mode)); for (auto ifn : iface_names) @@ -291,7 +341,7 @@ static inline NodePtr makeNode(const std::vector& iface_names, Cloc throw Exception("Failed to add iface " + ifn); } } - return NodePtr(new Node(dp)); + return std::shared_ptr(new N(dp)); } /** @@ -302,7 +352,20 @@ static inline NodePtr makeNode(const std::vector& iface_names, Cloc */ static inline NodePtr makeNode(const std::vector& iface_names) { - return makeNode(iface_names, SystemClock::detectPreferredClockAdjustmentMode()); + return makeAnyNodeWithCustomClockAdjustmentMode(iface_names, + SystemClock::detectPreferredClockAdjustmentMode()); +} + +/** + * Use this function to create a sub-node instance. + * It accepts the list of interface names to use for the new node, e.g. "can1", "vcan2", "slcan0". + * Clock adjustment mode will be detected automatically. + * @throws uavcan_linux::Exception. + */ +static inline SubNodePtr makeSubNode(const std::vector& iface_names) +{ + return makeAnyNodeWithCustomClockAdjustmentMode(iface_names, + SystemClock::detectPreferredClockAdjustmentMode()); } }