Руслан Урядинский / libuavcan

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   UAVCAN UAVCAN_Subscriber

libuavcan_drivers/linux/apps/test_node.cpp@0:dfe6edabb8ec, 2018-04-14 (annotated)

Committer:
RuslanUrya
Date:
Sat Apr 14 10:25:32 2018 +0000
Revision:
0:dfe6edabb8ec
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RuslanUrya 0:dfe6edabb8ec 1 /*
RuslanUrya 0:dfe6edabb8ec 2 * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
RuslanUrya 0:dfe6edabb8ec 3 */
RuslanUrya 0:dfe6edabb8ec 4
RuslanUrya 0:dfe6edabb8ec 5 #include <iostream>
RuslanUrya 0:dfe6edabb8ec 6 #include <uavcan_linux/uavcan_linux.hpp>
RuslanUrya 0:dfe6edabb8ec 7 #include <uavcan/protocol/node_status_monitor.hpp>
RuslanUrya 0:dfe6edabb8ec 8 #include "debug.hpp"
RuslanUrya 0:dfe6edabb8ec 9
RuslanUrya 0:dfe6edabb8ec 10 static uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid,
RuslanUrya 0:dfe6edabb8ec 11 const std::string& name)
RuslanUrya 0:dfe6edabb8ec 12 {
RuslanUrya 0:dfe6edabb8ec 13 auto node = uavcan_linux::makeNode(ifaces);
RuslanUrya 0:dfe6edabb8ec 14
RuslanUrya 0:dfe6edabb8ec 15 /*
RuslanUrya 0:dfe6edabb8ec 16 * Configuring the node.
RuslanUrya 0:dfe6edabb8ec 17 */
RuslanUrya 0:dfe6edabb8ec 18 node->setNodeID(nid);
RuslanUrya 0:dfe6edabb8ec 19 node->setName(name.c_str());
RuslanUrya 0:dfe6edabb8ec 20
RuslanUrya 0:dfe6edabb8ec 21 node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
RuslanUrya 0:dfe6edabb8ec 22
RuslanUrya 0:dfe6edabb8ec 23 /*
RuslanUrya 0:dfe6edabb8ec 24 * Starting the node.
RuslanUrya 0:dfe6edabb8ec 25 */
RuslanUrya 0:dfe6edabb8ec 26 std::cout << "Starting the node..." << std::endl;
RuslanUrya 0:dfe6edabb8ec 27 const int start_res = node->start();
RuslanUrya 0:dfe6edabb8ec 28 std::cout << "Start returned: " << start_res << std::endl;
RuslanUrya 0:dfe6edabb8ec 29 ENFORCE(0 == start_res);
RuslanUrya 0:dfe6edabb8ec 30
RuslanUrya 0:dfe6edabb8ec 31 std::cout << "Node started successfully" << std::endl;
RuslanUrya 0:dfe6edabb8ec 32
RuslanUrya 0:dfe6edabb8ec 33 /*
RuslanUrya 0:dfe6edabb8ec 34 * Say Hi to the world.
RuslanUrya 0:dfe6edabb8ec 35 */
RuslanUrya 0:dfe6edabb8ec 36 node->setModeOperational();
RuslanUrya 0:dfe6edabb8ec 37 node->logInfo("init", "Hello world! I'm [%*], NID %*",
RuslanUrya 0:dfe6edabb8ec 38 node->getNodeStatusProvider().getName().c_str(), int(node->getNodeID().get()));
RuslanUrya 0:dfe6edabb8ec 39 return node;
RuslanUrya 0:dfe6edabb8ec 40 }
RuslanUrya 0:dfe6edabb8ec 41
RuslanUrya 0:dfe6edabb8ec 42 static void runForever(const uavcan_linux::NodePtr& node)
RuslanUrya 0:dfe6edabb8ec 43 {
RuslanUrya 0:dfe6edabb8ec 44 /*
RuslanUrya 0:dfe6edabb8ec 45 * Subscribing to the UAVCAN logging topic
RuslanUrya 0:dfe6edabb8ec 46 */
RuslanUrya 0:dfe6edabb8ec 47 auto log_handler = [](const uavcan::ReceivedDataStructure<uavcan::protocol::debug::LogMessage>& msg)
RuslanUrya 0:dfe6edabb8ec 48 {
RuslanUrya 0:dfe6edabb8ec 49 std::cout << msg << std::endl;
RuslanUrya 0:dfe6edabb8ec 50 };
RuslanUrya 0:dfe6edabb8ec 51 auto log_sub = node->makeSubscriber<uavcan::protocol::debug::LogMessage>(log_handler);
RuslanUrya 0:dfe6edabb8ec 52
RuslanUrya 0:dfe6edabb8ec 53 /*
RuslanUrya 0:dfe6edabb8ec 54 * Printing when other nodes enter the network or change status
RuslanUrya 0:dfe6edabb8ec 55 */
RuslanUrya 0:dfe6edabb8ec 56 struct NodeStatusMonitor : public uavcan::NodeStatusMonitor
RuslanUrya 0:dfe6edabb8ec 57 {
RuslanUrya 0:dfe6edabb8ec 58 explicit NodeStatusMonitor(uavcan::INode& node) : uavcan::NodeStatusMonitor(node) { }
RuslanUrya 0:dfe6edabb8ec 59
RuslanUrya 0:dfe6edabb8ec 60 void handleNodeStatusChange(const NodeStatusChangeEvent& event) override
RuslanUrya 0:dfe6edabb8ec 61 {
RuslanUrya 0:dfe6edabb8ec 62 std::cout << "Remote node NID " << int(event.node_id.get()) << " changed status: "
RuslanUrya 0:dfe6edabb8ec 63 << event.old_status.toString() << " --> "
RuslanUrya 0:dfe6edabb8ec 64 << event.status.toString() << std::endl;
RuslanUrya 0:dfe6edabb8ec 65 }
RuslanUrya 0:dfe6edabb8ec 66 };
RuslanUrya 0:dfe6edabb8ec 67
RuslanUrya 0:dfe6edabb8ec 68 NodeStatusMonitor nsm(*node);
RuslanUrya 0:dfe6edabb8ec 69 ENFORCE(0 == nsm.start());
RuslanUrya 0:dfe6edabb8ec 70
RuslanUrya 0:dfe6edabb8ec 71 /*
RuslanUrya 0:dfe6edabb8ec 72 * Adding a stupid timer that does nothing once a minute
RuslanUrya 0:dfe6edabb8ec 73 */
RuslanUrya 0:dfe6edabb8ec 74 auto do_nothing_once_a_minute = [&node](const uavcan::TimerEvent&)
RuslanUrya 0:dfe6edabb8ec 75 {
RuslanUrya 0:dfe6edabb8ec 76 node->logInfo("timer", "Another minute passed...");
RuslanUrya 0:dfe6edabb8ec 77 // coverity[dont_call]
RuslanUrya 0:dfe6edabb8ec 78 node->setVendorSpecificStatusCode(static_cast<std::uint16_t>(std::rand())); // Setting to an arbitrary value
RuslanUrya 0:dfe6edabb8ec 79 };
RuslanUrya 0:dfe6edabb8ec 80 auto timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(60000), do_nothing_once_a_minute);
RuslanUrya 0:dfe6edabb8ec 81
RuslanUrya 0:dfe6edabb8ec 82 /*
RuslanUrya 0:dfe6edabb8ec 83 * Spinning forever
RuslanUrya 0:dfe6edabb8ec 84 */
RuslanUrya 0:dfe6edabb8ec 85 while (true)
RuslanUrya 0:dfe6edabb8ec 86 {
RuslanUrya 0:dfe6edabb8ec 87 const int res = node->spin(uavcan::MonotonicDuration::getInfinite());
RuslanUrya 0:dfe6edabb8ec 88 if (res < 0)
RuslanUrya 0:dfe6edabb8ec 89 {
RuslanUrya 0:dfe6edabb8ec 90 node->logError("spin", "Error %*", res);
RuslanUrya 0:dfe6edabb8ec 91 }
RuslanUrya 0:dfe6edabb8ec 92 }
RuslanUrya 0:dfe6edabb8ec 93 }
RuslanUrya 0:dfe6edabb8ec 94
RuslanUrya 0:dfe6edabb8ec 95 int main(int argc, const char** argv)
RuslanUrya 0:dfe6edabb8ec 96 {
RuslanUrya 0:dfe6edabb8ec 97 try
RuslanUrya 0:dfe6edabb8ec 98 {
RuslanUrya 0:dfe6edabb8ec 99 if (argc < 3)
RuslanUrya 0:dfe6edabb8ec 100 {
RuslanUrya 0:dfe6edabb8ec 101 std::cerr << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
RuslanUrya 0:dfe6edabb8ec 102 return 1;
RuslanUrya 0:dfe6edabb8ec 103 }
RuslanUrya 0:dfe6edabb8ec 104 const int self_node_id = std::stoi(argv[1]);
RuslanUrya 0:dfe6edabb8ec 105 std::vector<std::string> iface_names;
RuslanUrya 0:dfe6edabb8ec 106 for (int i = 2; i < argc; i++)
RuslanUrya 0:dfe6edabb8ec 107 {
RuslanUrya 0:dfe6edabb8ec 108 iface_names.emplace_back(argv[i]);
RuslanUrya 0:dfe6edabb8ec 109 }
RuslanUrya 0:dfe6edabb8ec 110 uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.linux_test_node");
RuslanUrya 0:dfe6edabb8ec 111 std::cout << "Node initialized successfully" << std::endl;
RuslanUrya 0:dfe6edabb8ec 112 runForever(node);
RuslanUrya 0:dfe6edabb8ec 113 return 0;
RuslanUrya 0:dfe6edabb8ec 114 }
RuslanUrya 0:dfe6edabb8ec 115 catch (const std::exception& ex)
RuslanUrya 0:dfe6edabb8ec 116 {
RuslanUrya 0:dfe6edabb8ec 117 std::cerr << "Exception: " << ex.what() << std::endl;
RuslanUrya 0:dfe6edabb8ec 118 return 1;
RuslanUrya 0:dfe6edabb8ec 119 }
RuslanUrya 0:dfe6edabb8ec 120 }