diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index 408fff617c..b33af01b8e 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -60,6 +60,9 @@ target_link_libraries(test_system_utils ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INI add_executable(test_posix apps/test_posix.cpp) target_link_libraries(test_posix ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) +add_executable(test_dynamic_node_id_client apps/test_dynamic_node_id_client.cpp) +target_link_libraries(test_dynamic_node_id_client ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) + # # Tools # diff --git a/libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp b/libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp new file mode 100644 index 0000000000..8743577730 --- /dev/null +++ b/libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp @@ -0,0 +1,121 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include "debug.hpp" +#include +#include + +namespace +{ + +uavcan_linux::NodePtr initNodeWithDynamicID(const std::vector& ifaces, + const std::uint8_t instance_id, + const uavcan::NodeID preferred_node_id, + const std::string& name) +{ + /* + * Initializing the node object + */ + auto node = uavcan_linux::makeNode(ifaces); + + 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, instance_id); + + uavcan::protocol::HardwareVersion hwver; + std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin()); + std::cout << hwver << std::endl; + + node->setHardwareVersion(hwver); + } + + /* + * Starting the node + */ + const int start_res = node->start(); + ENFORCE(0 == start_res); + + /* + * Running the dynamic node ID client until it's done + */ + uavcan::DynamicNodeIDClient client(*node); + + ENFORCE(0 <= client.start(node->getNodeStatusProvider().getHardwareVersion(), preferred_node_id)); + + std::cout << "Waiting for dynamic node ID allocation..." << std::endl; + + while (!client.isAllocationComplete()) + { + const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100)); + if (res < 0) + { + std::cerr << "Spin error: " << res << std::endl; + } + } + + std::cout << "Node ID " << int(client.getAllocatedNodeID().get()) + << " allocated by " << int(client.getAllocatorNodeID().get()) << std::endl; + + /* + * Finishing the node initialization + */ + node->setNodeID(client.getAllocatedNodeID()); + + node->setStatusOk(); + + return node; +} + +void runForever(const uavcan_linux::NodePtr& node) +{ + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100)); + if (res < 0) + { + std::cerr << "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...]\n" + << "Where is used to augment the unique node ID and also indicates\n" + << "the preferred node ID value. Valid range is [0, 127]." + << std::endl; + return 1; + } + + const int instance_id = std::stoi(argv[1]); + if (instance_id < 0 || instance_id > 127) + { + std::cerr << "Invalid instance ID: " << instance_id << std::endl; + std::exit(1); + } + + uavcan_linux::NodePtr node = initNodeWithDynamicID(std::vector(argv + 2, argv + argc), + std::uint8_t(instance_id), + std::uint8_t(instance_id), + "org.uavcan.linux_test_dynamic_node_id_client"); + runForever(node); + + return 0; + } + catch (const std::exception& ex) + { + std::cerr << "Error: " << ex.what() << std::endl; + return 1; + } +}