diff --git a/libuavcan_drivers/linux/apps/test_multithreading.cpp b/libuavcan_drivers/linux/apps/test_multithreading.cpp index fb143e3cf2..30136ec3ed 100644 --- a/libuavcan_drivers/linux/apps/test_multithreading.cpp +++ b/libuavcan_drivers/linux/apps/test_multithreading.cpp @@ -392,16 +392,9 @@ static uavcan_linux::NodePtr initMainNode(const std::vector& ifaces { std::cout << "Initializing main node" << std::endl; - auto node = uavcan_linux::makeNode(ifaces); - - node->setNodeID(nid); - node->setName(name.c_str()); - + auto node = uavcan_linux::makeNode(ifaces, name.c_str(), + uavcan::protocol::SoftwareVersion(), uavcan::protocol::HardwareVersion(), nid); node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); - - const int start_res = node->start(); - ENFORCE(0 == start_res); - node->setModeOperational(); return node; } @@ -414,8 +407,8 @@ static uavcan_linux::SubNodePtr initSubNode(unsigned num_ifaces, uavcan::INode& typedef VirtualCanDriver Driver; std::shared_ptr driver(new Driver(num_ifaces)); - auto node = uavcan_linux::makeSubNode(driver); - node->setNodeID(main_node.getNodeID()); + + auto node = uavcan_linux::makeSubNode(driver, main_node.getNodeID()); main_node.getDispatcher().installRxFrameListener(driver.get()); diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 5aa4d6d26d..91b39211dc 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -32,25 +32,15 @@ constexpr int MinUpdateInterval = 100; uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::NodeID nid, const std::string& name) { - auto node = uavcan_linux::makeNode(ifaces); + const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, nid.get()); + + uavcan::protocol::HardwareVersion hwver; + std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin()); + std::cout << hwver << std::endl; + + auto node = uavcan_linux::makeNode(ifaces, name.c_str(), uavcan::protocol::SoftwareVersion(), hwver, nid); - node->setNodeID(nid); - node->setName(name.c_str()); node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); - - { - const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, nid.get()); - - uavcan::protocol::HardwareVersion hwver; - std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin()); - std::cout << hwver << std::endl; - - node->setHardwareVersion(hwver); - } - - const int start_res = node->start(); - ENFORCE(0 == start_res); - node->setModeOperational(); return node; diff --git a/libuavcan_drivers/linux/apps/uavcan_monitor.cpp b/libuavcan_drivers/linux/apps/uavcan_monitor.cpp index afe139115a..d5d39e7c24 100644 --- a/libuavcan_drivers/linux/apps/uavcan_monitor.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_monitor.cpp @@ -137,9 +137,8 @@ public: static uavcan_linux::NodePtr initNodeInPassiveMode(const std::vector& ifaces, const std::string& node_name) { - auto node = uavcan_linux::makeNode(ifaces); - node->setName(node_name.c_str()); - ENFORCE(0 == node->start()); + auto node = uavcan_linux::makeNode(ifaces, node_name.c_str(), + uavcan::protocol::SoftwareVersion(), uavcan::protocol::HardwareVersion()); node->setModeOperational(); return node; } diff --git a/libuavcan_drivers/linux/apps/uavcan_nodetool.cpp b/libuavcan_drivers/linux/apps/uavcan_nodetool.cpp index 068b78bc72..a949336e54 100644 --- a/libuavcan_drivers/linux/apps/uavcan_nodetool.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_nodetool.cpp @@ -74,10 +74,9 @@ public: 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 + auto node = uavcan_linux::makeNode(ifaces, name.c_str(), + uavcan::protocol::SoftwareVersion(), uavcan::protocol::HardwareVersion(), + nid); node->setModeOperational(); return node; } diff --git a/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp index 394da2a816..041d1a196e 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp @@ -35,6 +35,28 @@ public: int getErrno() const { return errno_; } }; +/** + * This type is thrown when a Libuavcan API method exits with error. + * The error code is stored in the exception object and is avialable via @ref getLibuavcanErrorCode(). + */ +class LibuavcanErrorException : public Exception +{ + const std::int16_t error_; + + static std::string makeErrorString(std::int16_t e) + { + return "Libuavcan error (" + std::to_string(e) + ")"; + } + +public: + explicit LibuavcanErrorException(std::int16_t uavcan_error_code) : + Exception(makeErrorString(uavcan_error_code)), + error_(std::abs(uavcan_error_code)) + { } + + std::int16_t getLibuavcanErrorCode() const { return error_; } +}; + /** * This exception is thrown when all available interfaces become down. */ diff --git a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp index dda854e112..b3d39d0451 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp @@ -381,6 +381,49 @@ static inline NodePtr makeNode(const std::shared_ptr& can_dr return NodePtr(new Node(dp)); } +/** + * This function extends the other two overloads in such a way that it instantiates and initializes + * the node immediately; if initialization fails, it throws. + * + * If NodeID is not provided, it will not be initialized, and therefore the node will be started in + * listen-only (i.e. silent) mode. The node can be switched to normal (i.e. non-silent) mode at any + * later time by calling setNodeID() explicitly. Read the Node class docs for more info. + * + * Clock adjustment mode will be detected automatically unless provided explicitly. + * + * @throws uavcan_linux::Exception, uavcan_linux::LibuavcanErrorException. + */ +template +static inline NodePtr makeNode(const DriverType& driver, + const uavcan::NodeStatusProvider::NodeName& name, + const uavcan::protocol::SoftwareVersion& software_version, + const uavcan::protocol::HardwareVersion& hardware_version, + const uavcan::NodeID node_id = uavcan::NodeID(), + const uavcan::TransferPriority node_status_transfer_priority = + uavcan::TransferPriority::Default, + ClockAdjustmentMode clock_adjustment_mode = + SystemClock::detectPreferredClockAdjustmentMode()) +{ + NodePtr node = makeNode(driver, clock_adjustment_mode); + + node->setName(name); + node->setSoftwareVersion(software_version); + node->setHardwareVersion(hardware_version); + + if (node_id.isValid()) + { + node->setNodeID(node_id); + } + + const auto res = node->start(node_status_transfer_priority); + if (res < 0) + { + throw LibuavcanErrorException(res); + } + + return node; +} + /** * Use this function to create a sub-node instance with default SocketCAN driver. * It accepts the list of interface names to use for the new node, e.g. "can1", "vcan2", "slcan0". @@ -408,4 +451,25 @@ static inline SubNodePtr makeSubNode(const std::shared_ptr& return SubNodePtr(new SubNode(dp)); } +/** + * This function extends the other two overloads in such a way that it instantiates the node + * and sets its Node ID immediately. + * + * Clock adjustment mode will be detected automatically unless provided explicitly. + * + * @throws uavcan_linux::Exception, uavcan_linux::LibuavcanErrorException. + */ +template +static inline SubNodePtr makeSubNode(const DriverType& driver, + const uavcan::NodeID node_id, + const uavcan::TransferPriority node_status_transfer_priority = + uavcan::TransferPriority::Default, + ClockAdjustmentMode clock_adjustment_mode = + SystemClock::detectPreferredClockAdjustmentMode()) +{ + SubNodePtr sub_node = makeSubNode(driver, clock_adjustment_mode); + sub_node->setNodeID(node_id); + return sub_node; +} + }