Руслан Урядинский
/
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/include/uavcan_linux/helpers.hpp@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?
| User | Revision | Line number | New 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 | #pragma once |
| RuslanUrya | 0:dfe6edabb8ec | 6 | |
| RuslanUrya | 0:dfe6edabb8ec | 7 | #include <memory> |
| RuslanUrya | 0:dfe6edabb8ec | 8 | #include <string> |
| RuslanUrya | 0:dfe6edabb8ec | 9 | #include <vector> |
| RuslanUrya | 0:dfe6edabb8ec | 10 | #include <chrono> |
| RuslanUrya | 0:dfe6edabb8ec | 11 | #include <iostream> |
| RuslanUrya | 0:dfe6edabb8ec | 12 | #include <sstream> |
| RuslanUrya | 0:dfe6edabb8ec | 13 | #include <uavcan/uavcan.hpp> |
| RuslanUrya | 0:dfe6edabb8ec | 14 | #include <uavcan/node/sub_node.hpp> |
| RuslanUrya | 0:dfe6edabb8ec | 15 | |
| RuslanUrya | 0:dfe6edabb8ec | 16 | namespace uavcan_linux |
| RuslanUrya | 0:dfe6edabb8ec | 17 | { |
| RuslanUrya | 0:dfe6edabb8ec | 18 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 19 | * Default log sink will dump everything into stderr. |
| RuslanUrya | 0:dfe6edabb8ec | 20 | * It is installed by default. |
| RuslanUrya | 0:dfe6edabb8ec | 21 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 22 | class DefaultLogSink : public uavcan::ILogSink |
| RuslanUrya | 0:dfe6edabb8ec | 23 | { |
| RuslanUrya | 0:dfe6edabb8ec | 24 | void log(const uavcan::protocol::debug::LogMessage& message) override |
| RuslanUrya | 0:dfe6edabb8ec | 25 | { |
| RuslanUrya | 0:dfe6edabb8ec | 26 | const auto tt = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); |
| RuslanUrya | 0:dfe6edabb8ec | 27 | const auto tstr = std::ctime(&tt); |
| RuslanUrya | 0:dfe6edabb8ec | 28 | std::cerr << "### UAVCAN " << tstr << message << std::endl; |
| RuslanUrya | 0:dfe6edabb8ec | 29 | } |
| RuslanUrya | 0:dfe6edabb8ec | 30 | }; |
| RuslanUrya | 0:dfe6edabb8ec | 31 | |
| RuslanUrya | 0:dfe6edabb8ec | 32 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 33 | * Wrapper over uavcan::ServiceClient<> for blocking calls. |
| RuslanUrya | 0:dfe6edabb8ec | 34 | * Blocks on uavcan::Node::spin() internally until the call is complete. |
| RuslanUrya | 0:dfe6edabb8ec | 35 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 36 | template <typename DataType> |
| RuslanUrya | 0:dfe6edabb8ec | 37 | class BlockingServiceClient : public uavcan::ServiceClient<DataType> |
| RuslanUrya | 0:dfe6edabb8ec | 38 | { |
| RuslanUrya | 0:dfe6edabb8ec | 39 | typedef uavcan::ServiceClient<DataType> Super; |
| RuslanUrya | 0:dfe6edabb8ec | 40 | |
| RuslanUrya | 0:dfe6edabb8ec | 41 | typename DataType::Response response_; |
| RuslanUrya | 0:dfe6edabb8ec | 42 | bool call_was_successful_; |
| RuslanUrya | 0:dfe6edabb8ec | 43 | |
| RuslanUrya | 0:dfe6edabb8ec | 44 | void callback(const uavcan::ServiceCallResult<DataType>& res) |
| RuslanUrya | 0:dfe6edabb8ec | 45 | { |
| RuslanUrya | 0:dfe6edabb8ec | 46 | response_ = res.getResponse(); |
| RuslanUrya | 0:dfe6edabb8ec | 47 | call_was_successful_ = res.isSuccessful(); |
| RuslanUrya | 0:dfe6edabb8ec | 48 | } |
| RuslanUrya | 0:dfe6edabb8ec | 49 | |
| RuslanUrya | 0:dfe6edabb8ec | 50 | void setup() |
| RuslanUrya | 0:dfe6edabb8ec | 51 | { |
| RuslanUrya | 0:dfe6edabb8ec | 52 | Super::setCallback(std::bind(&BlockingServiceClient::callback, this, std::placeholders::_1)); |
| RuslanUrya | 0:dfe6edabb8ec | 53 | call_was_successful_ = false; |
| RuslanUrya | 0:dfe6edabb8ec | 54 | response_ = typename DataType::Response(); |
| RuslanUrya | 0:dfe6edabb8ec | 55 | } |
| RuslanUrya | 0:dfe6edabb8ec | 56 | |
| RuslanUrya | 0:dfe6edabb8ec | 57 | public: |
| RuslanUrya | 0:dfe6edabb8ec | 58 | BlockingServiceClient(uavcan::INode& node) |
| RuslanUrya | 0:dfe6edabb8ec | 59 | : uavcan::ServiceClient<DataType>(node) |
| RuslanUrya | 0:dfe6edabb8ec | 60 | , call_was_successful_(false) |
| RuslanUrya | 0:dfe6edabb8ec | 61 | { |
| RuslanUrya | 0:dfe6edabb8ec | 62 | setup(); |
| RuslanUrya | 0:dfe6edabb8ec | 63 | } |
| RuslanUrya | 0:dfe6edabb8ec | 64 | |
| RuslanUrya | 0:dfe6edabb8ec | 65 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 66 | * Performs a blocking service call using default timeout (see the specs). |
| RuslanUrya | 0:dfe6edabb8ec | 67 | * Use @ref getResponse() to get the actual response. |
| RuslanUrya | 0:dfe6edabb8ec | 68 | * Returns negative error code. |
| RuslanUrya | 0:dfe6edabb8ec | 69 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 70 | int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request) |
| RuslanUrya | 0:dfe6edabb8ec | 71 | { |
| RuslanUrya | 0:dfe6edabb8ec | 72 | return blockingCall(server_node_id, request, Super::getDefaultRequestTimeout()); |
| RuslanUrya | 0:dfe6edabb8ec | 73 | } |
| RuslanUrya | 0:dfe6edabb8ec | 74 | |
| RuslanUrya | 0:dfe6edabb8ec | 75 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 76 | * Performs a blocking service call using the specified timeout. Please consider using default timeout instead. |
| RuslanUrya | 0:dfe6edabb8ec | 77 | * Use @ref getResponse() to get the actual response. |
| RuslanUrya | 0:dfe6edabb8ec | 78 | * Returns negative error code. |
| RuslanUrya | 0:dfe6edabb8ec | 79 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 80 | int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request, |
| RuslanUrya | 0:dfe6edabb8ec | 81 | uavcan::MonotonicDuration timeout) |
| RuslanUrya | 0:dfe6edabb8ec | 82 | { |
| RuslanUrya | 0:dfe6edabb8ec | 83 | const auto SpinDuration = uavcan::MonotonicDuration::fromMSec(2); |
| RuslanUrya | 0:dfe6edabb8ec | 84 | setup(); |
| RuslanUrya | 0:dfe6edabb8ec | 85 | Super::setRequestTimeout(timeout); |
| RuslanUrya | 0:dfe6edabb8ec | 86 | const int call_res = Super::call(server_node_id, request); |
| RuslanUrya | 0:dfe6edabb8ec | 87 | if (call_res >= 0) |
| RuslanUrya | 0:dfe6edabb8ec | 88 | { |
| RuslanUrya | 0:dfe6edabb8ec | 89 | while (Super::hasPendingCalls()) |
| RuslanUrya | 0:dfe6edabb8ec | 90 | { |
| RuslanUrya | 0:dfe6edabb8ec | 91 | const int spin_res = Super::getNode().spin(SpinDuration); |
| RuslanUrya | 0:dfe6edabb8ec | 92 | if (spin_res < 0) |
| RuslanUrya | 0:dfe6edabb8ec | 93 | { |
| RuslanUrya | 0:dfe6edabb8ec | 94 | return spin_res; |
| RuslanUrya | 0:dfe6edabb8ec | 95 | } |
| RuslanUrya | 0:dfe6edabb8ec | 96 | } |
| RuslanUrya | 0:dfe6edabb8ec | 97 | } |
| RuslanUrya | 0:dfe6edabb8ec | 98 | return call_res; |
| RuslanUrya | 0:dfe6edabb8ec | 99 | } |
| RuslanUrya | 0:dfe6edabb8ec | 100 | |
| RuslanUrya | 0:dfe6edabb8ec | 101 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 102 | * Whether the last blocking call was successful. |
| RuslanUrya | 0:dfe6edabb8ec | 103 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 104 | bool wasSuccessful() const { return call_was_successful_; } |
| RuslanUrya | 0:dfe6edabb8ec | 105 | |
| RuslanUrya | 0:dfe6edabb8ec | 106 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 107 | * Use this to retrieve the response of the last blocking service call. |
| RuslanUrya | 0:dfe6edabb8ec | 108 | * This method returns default constructed response object if the last service call was unsuccessful. |
| RuslanUrya | 0:dfe6edabb8ec | 109 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 110 | const typename DataType::Response& getResponse() const { return response_; } |
| RuslanUrya | 0:dfe6edabb8ec | 111 | }; |
| RuslanUrya | 0:dfe6edabb8ec | 112 | |
| RuslanUrya | 0:dfe6edabb8ec | 113 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 114 | * Contains all drivers needed for uavcan::Node. |
| RuslanUrya | 0:dfe6edabb8ec | 115 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 116 | struct DriverPack |
| RuslanUrya | 0:dfe6edabb8ec | 117 | { |
| RuslanUrya | 0:dfe6edabb8ec | 118 | SystemClock clock; |
| RuslanUrya | 0:dfe6edabb8ec | 119 | std::shared_ptr<uavcan::ICanDriver> can; |
| RuslanUrya | 0:dfe6edabb8ec | 120 | |
| RuslanUrya | 0:dfe6edabb8ec | 121 | explicit DriverPack(ClockAdjustmentMode clock_adjustment_mode, |
| RuslanUrya | 0:dfe6edabb8ec | 122 | const std::shared_ptr<uavcan::ICanDriver>& can_driver) |
| RuslanUrya | 0:dfe6edabb8ec | 123 | : clock(clock_adjustment_mode) |
| RuslanUrya | 0:dfe6edabb8ec | 124 | , can(can_driver) |
| RuslanUrya | 0:dfe6edabb8ec | 125 | { } |
| RuslanUrya | 0:dfe6edabb8ec | 126 | |
| RuslanUrya | 0:dfe6edabb8ec | 127 | explicit DriverPack(ClockAdjustmentMode clock_adjustment_mode, |
| RuslanUrya | 0:dfe6edabb8ec | 128 | const std::vector<std::string>& iface_names) |
| RuslanUrya | 0:dfe6edabb8ec | 129 | : clock(clock_adjustment_mode) |
| RuslanUrya | 0:dfe6edabb8ec | 130 | { |
| RuslanUrya | 0:dfe6edabb8ec | 131 | std::shared_ptr<SocketCanDriver> socketcan(new SocketCanDriver(clock)); |
| RuslanUrya | 0:dfe6edabb8ec | 132 | can = socketcan; |
| RuslanUrya | 0:dfe6edabb8ec | 133 | for (auto ifn : iface_names) |
| RuslanUrya | 0:dfe6edabb8ec | 134 | { |
| RuslanUrya | 0:dfe6edabb8ec | 135 | if (socketcan->addIface(ifn) < 0) |
| RuslanUrya | 0:dfe6edabb8ec | 136 | { |
| RuslanUrya | 0:dfe6edabb8ec | 137 | throw Exception("Failed to add iface " + ifn); |
| RuslanUrya | 0:dfe6edabb8ec | 138 | } |
| RuslanUrya | 0:dfe6edabb8ec | 139 | } |
| RuslanUrya | 0:dfe6edabb8ec | 140 | } |
| RuslanUrya | 0:dfe6edabb8ec | 141 | }; |
| RuslanUrya | 0:dfe6edabb8ec | 142 | |
| RuslanUrya | 0:dfe6edabb8ec | 143 | typedef std::shared_ptr<DriverPack> DriverPackPtr; |
| RuslanUrya | 0:dfe6edabb8ec | 144 | |
| RuslanUrya | 0:dfe6edabb8ec | 145 | typedef std::shared_ptr<uavcan::INode> INodePtr; |
| RuslanUrya | 0:dfe6edabb8ec | 146 | |
| RuslanUrya | 0:dfe6edabb8ec | 147 | typedef std::shared_ptr<uavcan::Timer> TimerPtr; |
| RuslanUrya | 0:dfe6edabb8ec | 148 | |
| RuslanUrya | 0:dfe6edabb8ec | 149 | template <typename T> |
| RuslanUrya | 0:dfe6edabb8ec | 150 | using SubscriberPtr = std::shared_ptr<uavcan::Subscriber<T>>; |
| RuslanUrya | 0:dfe6edabb8ec | 151 | |
| RuslanUrya | 0:dfe6edabb8ec | 152 | template <typename T> |
| RuslanUrya | 0:dfe6edabb8ec | 153 | using PublisherPtr = std::shared_ptr<uavcan::Publisher<T>>; |
| RuslanUrya | 0:dfe6edabb8ec | 154 | |
| RuslanUrya | 0:dfe6edabb8ec | 155 | template <typename T> |
| RuslanUrya | 0:dfe6edabb8ec | 156 | using ServiceServerPtr = std::shared_ptr<uavcan::ServiceServer<T>>; |
| RuslanUrya | 0:dfe6edabb8ec | 157 | |
| RuslanUrya | 0:dfe6edabb8ec | 158 | template <typename T> |
| RuslanUrya | 0:dfe6edabb8ec | 159 | using ServiceClientPtr = std::shared_ptr<uavcan::ServiceClient<T>>; |
| RuslanUrya | 0:dfe6edabb8ec | 160 | |
| RuslanUrya | 0:dfe6edabb8ec | 161 | template <typename T> |
| RuslanUrya | 0:dfe6edabb8ec | 162 | using BlockingServiceClientPtr = std::shared_ptr<BlockingServiceClient<T>>; |
| RuslanUrya | 0:dfe6edabb8ec | 163 | |
| RuslanUrya | 0:dfe6edabb8ec | 164 | static constexpr std::size_t NodeMemPoolSize = 1024 * 512; ///< This shall be enough for any possible use case |
| RuslanUrya | 0:dfe6edabb8ec | 165 | |
| RuslanUrya | 0:dfe6edabb8ec | 166 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 167 | * Generic wrapper for node objects with some additional convenience functions. |
| RuslanUrya | 0:dfe6edabb8ec | 168 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 169 | template <typename NodeType> |
| RuslanUrya | 0:dfe6edabb8ec | 170 | class NodeBase : public NodeType |
| RuslanUrya | 0:dfe6edabb8ec | 171 | { |
| RuslanUrya | 0:dfe6edabb8ec | 172 | protected: |
| RuslanUrya | 0:dfe6edabb8ec | 173 | DriverPackPtr driver_pack_; |
| RuslanUrya | 0:dfe6edabb8ec | 174 | |
| RuslanUrya | 0:dfe6edabb8ec | 175 | static void enforce(int error, const std::string& msg) |
| RuslanUrya | 0:dfe6edabb8ec | 176 | { |
| RuslanUrya | 0:dfe6edabb8ec | 177 | if (error < 0) |
| RuslanUrya | 0:dfe6edabb8ec | 178 | { |
| RuslanUrya | 0:dfe6edabb8ec | 179 | std::ostringstream os; |
| RuslanUrya | 0:dfe6edabb8ec | 180 | os << msg << " [" << error << "]"; |
| RuslanUrya | 0:dfe6edabb8ec | 181 | throw Exception(os.str()); |
| RuslanUrya | 0:dfe6edabb8ec | 182 | } |
| RuslanUrya | 0:dfe6edabb8ec | 183 | } |
| RuslanUrya | 0:dfe6edabb8ec | 184 | |
| RuslanUrya | 0:dfe6edabb8ec | 185 | template <typename DataType> |
| RuslanUrya | 0:dfe6edabb8ec | 186 | static std::string getDataTypeName() |
| RuslanUrya | 0:dfe6edabb8ec | 187 | { |
| RuslanUrya | 0:dfe6edabb8ec | 188 | return DataType::getDataTypeFullName(); |
| RuslanUrya | 0:dfe6edabb8ec | 189 | } |
| RuslanUrya | 0:dfe6edabb8ec | 190 | |
| RuslanUrya | 0:dfe6edabb8ec | 191 | public: |
| RuslanUrya | 0:dfe6edabb8ec | 192 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 193 | * Simple forwarding constructor, compatible with uavcan::Node. |
| RuslanUrya | 0:dfe6edabb8ec | 194 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 195 | NodeBase(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) : |
| RuslanUrya | 0:dfe6edabb8ec | 196 | NodeType(can_driver, clock) |
| RuslanUrya | 0:dfe6edabb8ec | 197 | { } |
| RuslanUrya | 0:dfe6edabb8ec | 198 | |
| RuslanUrya | 0:dfe6edabb8ec | 199 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 200 | * Takes ownership of the driver container via the shared pointer. |
| RuslanUrya | 0:dfe6edabb8ec | 201 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 202 | explicit NodeBase(DriverPackPtr driver_pack) |
| RuslanUrya | 0:dfe6edabb8ec | 203 | : NodeType(*driver_pack->can, driver_pack->clock) |
| RuslanUrya | 0:dfe6edabb8ec | 204 | , driver_pack_(driver_pack) |
| RuslanUrya | 0:dfe6edabb8ec | 205 | { } |
| RuslanUrya | 0:dfe6edabb8ec | 206 | |
| RuslanUrya | 0:dfe6edabb8ec | 207 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 208 | * Allocates @ref uavcan::Subscriber in the heap using shared pointer. |
| RuslanUrya | 0:dfe6edabb8ec | 209 | * The subscriber will be started immediately. |
| RuslanUrya | 0:dfe6edabb8ec | 210 | * @throws uavcan_linux::Exception. |
| RuslanUrya | 0:dfe6edabb8ec | 211 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 212 | template <typename DataType> |
| RuslanUrya | 0:dfe6edabb8ec | 213 | SubscriberPtr<DataType> makeSubscriber(const typename uavcan::Subscriber<DataType>::Callback& cb) |
| RuslanUrya | 0:dfe6edabb8ec | 214 | { |
| RuslanUrya | 0:dfe6edabb8ec | 215 | SubscriberPtr<DataType> p(new uavcan::Subscriber<DataType>(*this)); |
| RuslanUrya | 0:dfe6edabb8ec | 216 | enforce(p->start(cb), "Subscriber start failure " + getDataTypeName<DataType>()); |
| RuslanUrya | 0:dfe6edabb8ec | 217 | return p; |
| RuslanUrya | 0:dfe6edabb8ec | 218 | } |
| RuslanUrya | 0:dfe6edabb8ec | 219 | |
| RuslanUrya | 0:dfe6edabb8ec | 220 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 221 | * Allocates @ref uavcan::Publisher in the heap using shared pointer. |
| RuslanUrya | 0:dfe6edabb8ec | 222 | * The publisher will be initialized immediately. |
| RuslanUrya | 0:dfe6edabb8ec | 223 | * @throws uavcan_linux::Exception. |
| RuslanUrya | 0:dfe6edabb8ec | 224 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 225 | template <typename DataType> |
| RuslanUrya | 0:dfe6edabb8ec | 226 | PublisherPtr<DataType> makePublisher(uavcan::MonotonicDuration tx_timeout = |
| RuslanUrya | 0:dfe6edabb8ec | 227 | uavcan::Publisher<DataType>::getDefaultTxTimeout()) |
| RuslanUrya | 0:dfe6edabb8ec | 228 | { |
| RuslanUrya | 0:dfe6edabb8ec | 229 | PublisherPtr<DataType> p(new uavcan::Publisher<DataType>(*this)); |
| RuslanUrya | 0:dfe6edabb8ec | 230 | enforce(p->init(), "Publisher init failure " + getDataTypeName<DataType>()); |
| RuslanUrya | 0:dfe6edabb8ec | 231 | p->setTxTimeout(tx_timeout); |
| RuslanUrya | 0:dfe6edabb8ec | 232 | return p; |
| RuslanUrya | 0:dfe6edabb8ec | 233 | } |
| RuslanUrya | 0:dfe6edabb8ec | 234 | |
| RuslanUrya | 0:dfe6edabb8ec | 235 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 236 | * Allocates @ref uavcan::ServiceServer in the heap using shared pointer. |
| RuslanUrya | 0:dfe6edabb8ec | 237 | * The server will be started immediately. |
| RuslanUrya | 0:dfe6edabb8ec | 238 | * @throws uavcan_linux::Exception. |
| RuslanUrya | 0:dfe6edabb8ec | 239 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 240 | template <typename DataType> |
| RuslanUrya | 0:dfe6edabb8ec | 241 | ServiceServerPtr<DataType> makeServiceServer(const typename uavcan::ServiceServer<DataType>::Callback& cb) |
| RuslanUrya | 0:dfe6edabb8ec | 242 | { |
| RuslanUrya | 0:dfe6edabb8ec | 243 | ServiceServerPtr<DataType> p(new uavcan::ServiceServer<DataType>(*this)); |
| RuslanUrya | 0:dfe6edabb8ec | 244 | enforce(p->start(cb), "ServiceServer start failure " + getDataTypeName<DataType>()); |
| RuslanUrya | 0:dfe6edabb8ec | 245 | return p; |
| RuslanUrya | 0:dfe6edabb8ec | 246 | } |
| RuslanUrya | 0:dfe6edabb8ec | 247 | |
| RuslanUrya | 0:dfe6edabb8ec | 248 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 249 | * Allocates @ref uavcan::ServiceClient in the heap using shared pointer. |
| RuslanUrya | 0:dfe6edabb8ec | 250 | * The service client will be initialized immediately. |
| RuslanUrya | 0:dfe6edabb8ec | 251 | * @throws uavcan_linux::Exception. |
| RuslanUrya | 0:dfe6edabb8ec | 252 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 253 | template <typename DataType> |
| RuslanUrya | 0:dfe6edabb8ec | 254 | ServiceClientPtr<DataType> makeServiceClient(const typename uavcan::ServiceClient<DataType>::Callback& cb) |
| RuslanUrya | 0:dfe6edabb8ec | 255 | { |
| RuslanUrya | 0:dfe6edabb8ec | 256 | ServiceClientPtr<DataType> p(new uavcan::ServiceClient<DataType>(*this)); |
| RuslanUrya | 0:dfe6edabb8ec | 257 | enforce(p->init(), "ServiceClient init failure " + getDataTypeName<DataType>()); |
| RuslanUrya | 0:dfe6edabb8ec | 258 | p->setCallback(cb); |
| RuslanUrya | 0:dfe6edabb8ec | 259 | return p; |
| RuslanUrya | 0:dfe6edabb8ec | 260 | } |
| RuslanUrya | 0:dfe6edabb8ec | 261 | |
| RuslanUrya | 0:dfe6edabb8ec | 262 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 263 | * Allocates @ref uavcan_linux::BlockingServiceClient in the heap using shared pointer. |
| RuslanUrya | 0:dfe6edabb8ec | 264 | * The service client will be initialized immediately. |
| RuslanUrya | 0:dfe6edabb8ec | 265 | * @throws uavcan_linux::Exception. |
| RuslanUrya | 0:dfe6edabb8ec | 266 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 267 | template <typename DataType> |
| RuslanUrya | 0:dfe6edabb8ec | 268 | BlockingServiceClientPtr<DataType> makeBlockingServiceClient() |
| RuslanUrya | 0:dfe6edabb8ec | 269 | { |
| RuslanUrya | 0:dfe6edabb8ec | 270 | BlockingServiceClientPtr<DataType> p(new BlockingServiceClient<DataType>(*this)); |
| RuslanUrya | 0:dfe6edabb8ec | 271 | enforce(p->init(), "BlockingServiceClient init failure " + getDataTypeName<DataType>()); |
| RuslanUrya | 0:dfe6edabb8ec | 272 | return p; |
| RuslanUrya | 0:dfe6edabb8ec | 273 | } |
| RuslanUrya | 0:dfe6edabb8ec | 274 | |
| RuslanUrya | 0:dfe6edabb8ec | 275 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 276 | * Allocates @ref uavcan::Timer in the heap using shared pointer. |
| RuslanUrya | 0:dfe6edabb8ec | 277 | * The timer will be started immediately in one-shot mode. |
| RuslanUrya | 0:dfe6edabb8ec | 278 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 279 | TimerPtr makeTimer(uavcan::MonotonicTime deadline, const typename uavcan::Timer::Callback& cb) |
| RuslanUrya | 0:dfe6edabb8ec | 280 | { |
| RuslanUrya | 0:dfe6edabb8ec | 281 | TimerPtr p(new uavcan::Timer(*this)); |
| RuslanUrya | 0:dfe6edabb8ec | 282 | p->setCallback(cb); |
| RuslanUrya | 0:dfe6edabb8ec | 283 | p->startOneShotWithDeadline(deadline); |
| RuslanUrya | 0:dfe6edabb8ec | 284 | return p; |
| RuslanUrya | 0:dfe6edabb8ec | 285 | } |
| RuslanUrya | 0:dfe6edabb8ec | 286 | |
| RuslanUrya | 0:dfe6edabb8ec | 287 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 288 | * Allocates @ref uavcan::Timer in the heap using shared pointer. |
| RuslanUrya | 0:dfe6edabb8ec | 289 | * The timer will be started immediately in periodic mode. |
| RuslanUrya | 0:dfe6edabb8ec | 290 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 291 | TimerPtr makeTimer(uavcan::MonotonicDuration period, const typename uavcan::Timer::Callback& cb) |
| RuslanUrya | 0:dfe6edabb8ec | 292 | { |
| RuslanUrya | 0:dfe6edabb8ec | 293 | TimerPtr p(new uavcan::Timer(*this)); |
| RuslanUrya | 0:dfe6edabb8ec | 294 | p->setCallback(cb); |
| RuslanUrya | 0:dfe6edabb8ec | 295 | p->startPeriodic(period); |
| RuslanUrya | 0:dfe6edabb8ec | 296 | return p; |
| RuslanUrya | 0:dfe6edabb8ec | 297 | } |
| RuslanUrya | 0:dfe6edabb8ec | 298 | |
| RuslanUrya | 0:dfe6edabb8ec | 299 | const DriverPackPtr& getDriverPack() const { return driver_pack_; } |
| RuslanUrya | 0:dfe6edabb8ec | 300 | DriverPackPtr& getDriverPack() { return driver_pack_; } |
| RuslanUrya | 0:dfe6edabb8ec | 301 | }; |
| RuslanUrya | 0:dfe6edabb8ec | 302 | |
| RuslanUrya | 0:dfe6edabb8ec | 303 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 304 | * Wrapper for uavcan::Node with some additional convenience functions. |
| RuslanUrya | 0:dfe6edabb8ec | 305 | * Note that this wrapper adds stderr log sink to @ref uavcan::Logger, which can be removed if needed. |
| RuslanUrya | 0:dfe6edabb8ec | 306 | * Do not instantiate this class directly; instead use the factory functions defined below. |
| RuslanUrya | 0:dfe6edabb8ec | 307 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 308 | class Node : public NodeBase<uavcan::Node<NodeMemPoolSize>> |
| RuslanUrya | 0:dfe6edabb8ec | 309 | { |
| RuslanUrya | 0:dfe6edabb8ec | 310 | typedef NodeBase<uavcan::Node<NodeMemPoolSize>> Base; |
| RuslanUrya | 0:dfe6edabb8ec | 311 | |
| RuslanUrya | 0:dfe6edabb8ec | 312 | DefaultLogSink log_sink_; |
| RuslanUrya | 0:dfe6edabb8ec | 313 | |
| RuslanUrya | 0:dfe6edabb8ec | 314 | public: |
| RuslanUrya | 0:dfe6edabb8ec | 315 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 316 | * Simple forwarding constructor, compatible with uavcan::Node. |
| RuslanUrya | 0:dfe6edabb8ec | 317 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 318 | Node(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) : |
| RuslanUrya | 0:dfe6edabb8ec | 319 | Base(can_driver, clock) |
| RuslanUrya | 0:dfe6edabb8ec | 320 | { |
| RuslanUrya | 0:dfe6edabb8ec | 321 | getLogger().setExternalSink(&log_sink_); |
| RuslanUrya | 0:dfe6edabb8ec | 322 | } |
| RuslanUrya | 0:dfe6edabb8ec | 323 | |
| RuslanUrya | 0:dfe6edabb8ec | 324 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 325 | * Takes ownership of the driver container via the shared pointer. |
| RuslanUrya | 0:dfe6edabb8ec | 326 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 327 | explicit Node(DriverPackPtr driver_pack) : |
| RuslanUrya | 0:dfe6edabb8ec | 328 | Base(driver_pack) |
| RuslanUrya | 0:dfe6edabb8ec | 329 | { |
| RuslanUrya | 0:dfe6edabb8ec | 330 | getLogger().setExternalSink(&log_sink_); |
| RuslanUrya | 0:dfe6edabb8ec | 331 | } |
| RuslanUrya | 0:dfe6edabb8ec | 332 | }; |
| RuslanUrya | 0:dfe6edabb8ec | 333 | |
| RuslanUrya | 0:dfe6edabb8ec | 334 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 335 | * Wrapper for uavcan::SubNode with some additional convenience functions. |
| RuslanUrya | 0:dfe6edabb8ec | 336 | * Do not instantiate this class directly; instead use the factory functions defined below. |
| RuslanUrya | 0:dfe6edabb8ec | 337 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 338 | class SubNode : public NodeBase<uavcan::SubNode<NodeMemPoolSize>> |
| RuslanUrya | 0:dfe6edabb8ec | 339 | { |
| RuslanUrya | 0:dfe6edabb8ec | 340 | typedef NodeBase<uavcan::SubNode<NodeMemPoolSize>> Base; |
| RuslanUrya | 0:dfe6edabb8ec | 341 | |
| RuslanUrya | 0:dfe6edabb8ec | 342 | public: |
| RuslanUrya | 0:dfe6edabb8ec | 343 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 344 | * Simple forwarding constructor, compatible with uavcan::Node. |
| RuslanUrya | 0:dfe6edabb8ec | 345 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 346 | SubNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) : Base(can_driver, clock) { } |
| RuslanUrya | 0:dfe6edabb8ec | 347 | |
| RuslanUrya | 0:dfe6edabb8ec | 348 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 349 | * Takes ownership of the driver container via the shared pointer. |
| RuslanUrya | 0:dfe6edabb8ec | 350 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 351 | explicit SubNode(DriverPackPtr driver_pack) : Base(driver_pack) { } |
| RuslanUrya | 0:dfe6edabb8ec | 352 | }; |
| RuslanUrya | 0:dfe6edabb8ec | 353 | |
| RuslanUrya | 0:dfe6edabb8ec | 354 | typedef std::shared_ptr<Node> NodePtr; |
| RuslanUrya | 0:dfe6edabb8ec | 355 | typedef std::shared_ptr<SubNode> SubNodePtr; |
| RuslanUrya | 0:dfe6edabb8ec | 356 | |
| RuslanUrya | 0:dfe6edabb8ec | 357 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 358 | * Use this function to create a node instance with default SocketCAN driver. |
| RuslanUrya | 0:dfe6edabb8ec | 359 | * It accepts the list of interface names to use for the new node, e.g. "can1", "vcan2", "slcan0". |
| RuslanUrya | 0:dfe6edabb8ec | 360 | * Clock adjustment mode will be detected automatically unless provided explicitly. |
| RuslanUrya | 0:dfe6edabb8ec | 361 | * @throws uavcan_linux::Exception. |
| RuslanUrya | 0:dfe6edabb8ec | 362 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 363 | static inline NodePtr makeNode(const std::vector<std::string>& iface_names, |
| RuslanUrya | 0:dfe6edabb8ec | 364 | ClockAdjustmentMode clock_adjustment_mode = |
| RuslanUrya | 0:dfe6edabb8ec | 365 | SystemClock::detectPreferredClockAdjustmentMode()) |
| RuslanUrya | 0:dfe6edabb8ec | 366 | { |
| RuslanUrya | 0:dfe6edabb8ec | 367 | DriverPackPtr dp(new DriverPack(clock_adjustment_mode, iface_names)); |
| RuslanUrya | 0:dfe6edabb8ec | 368 | return NodePtr(new Node(dp)); |
| RuslanUrya | 0:dfe6edabb8ec | 369 | } |
| RuslanUrya | 0:dfe6edabb8ec | 370 | |
| RuslanUrya | 0:dfe6edabb8ec | 371 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 372 | * Use this function to create a node instance with a custom driver. |
| RuslanUrya | 0:dfe6edabb8ec | 373 | * Clock adjustment mode will be detected automatically unless provided explicitly. |
| RuslanUrya | 0:dfe6edabb8ec | 374 | * @throws uavcan_linux::Exception. |
| RuslanUrya | 0:dfe6edabb8ec | 375 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 376 | static inline NodePtr makeNode(const std::shared_ptr<uavcan::ICanDriver>& can_driver, |
| RuslanUrya | 0:dfe6edabb8ec | 377 | ClockAdjustmentMode clock_adjustment_mode = |
| RuslanUrya | 0:dfe6edabb8ec | 378 | SystemClock::detectPreferredClockAdjustmentMode()) |
| RuslanUrya | 0:dfe6edabb8ec | 379 | { |
| RuslanUrya | 0:dfe6edabb8ec | 380 | DriverPackPtr dp(new DriverPack(clock_adjustment_mode, can_driver)); |
| RuslanUrya | 0:dfe6edabb8ec | 381 | return NodePtr(new Node(dp)); |
| RuslanUrya | 0:dfe6edabb8ec | 382 | } |
| RuslanUrya | 0:dfe6edabb8ec | 383 | |
| RuslanUrya | 0:dfe6edabb8ec | 384 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 385 | * This function extends the other two overloads in such a way that it instantiates and initializes |
| RuslanUrya | 0:dfe6edabb8ec | 386 | * the node immediately; if initialization fails, it throws. |
| RuslanUrya | 0:dfe6edabb8ec | 387 | * |
| RuslanUrya | 0:dfe6edabb8ec | 388 | * If NodeID is not provided, it will not be initialized, and therefore the node will be started in |
| RuslanUrya | 0:dfe6edabb8ec | 389 | * listen-only (i.e. silent) mode. The node can be switched to normal (i.e. non-silent) mode at any |
| RuslanUrya | 0:dfe6edabb8ec | 390 | * later time by calling setNodeID() explicitly. Read the Node class docs for more info. |
| RuslanUrya | 0:dfe6edabb8ec | 391 | * |
| RuslanUrya | 0:dfe6edabb8ec | 392 | * Clock adjustment mode will be detected automatically unless provided explicitly. |
| RuslanUrya | 0:dfe6edabb8ec | 393 | * |
| RuslanUrya | 0:dfe6edabb8ec | 394 | * @throws uavcan_linux::Exception, uavcan_linux::LibuavcanErrorException. |
| RuslanUrya | 0:dfe6edabb8ec | 395 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 396 | template <typename DriverType> |
| RuslanUrya | 0:dfe6edabb8ec | 397 | static inline NodePtr makeNode(const DriverType& driver, |
| RuslanUrya | 0:dfe6edabb8ec | 398 | const uavcan::NodeStatusProvider::NodeName& name, |
| RuslanUrya | 0:dfe6edabb8ec | 399 | const uavcan::protocol::SoftwareVersion& software_version, |
| RuslanUrya | 0:dfe6edabb8ec | 400 | const uavcan::protocol::HardwareVersion& hardware_version, |
| RuslanUrya | 0:dfe6edabb8ec | 401 | const uavcan::NodeID node_id = uavcan::NodeID(), |
| RuslanUrya | 0:dfe6edabb8ec | 402 | const uavcan::TransferPriority node_status_transfer_priority = |
| RuslanUrya | 0:dfe6edabb8ec | 403 | uavcan::TransferPriority::Default, |
| RuslanUrya | 0:dfe6edabb8ec | 404 | ClockAdjustmentMode clock_adjustment_mode = |
| RuslanUrya | 0:dfe6edabb8ec | 405 | SystemClock::detectPreferredClockAdjustmentMode()) |
| RuslanUrya | 0:dfe6edabb8ec | 406 | { |
| RuslanUrya | 0:dfe6edabb8ec | 407 | NodePtr node = makeNode(driver, clock_adjustment_mode); |
| RuslanUrya | 0:dfe6edabb8ec | 408 | |
| RuslanUrya | 0:dfe6edabb8ec | 409 | node->setName(name); |
| RuslanUrya | 0:dfe6edabb8ec | 410 | node->setSoftwareVersion(software_version); |
| RuslanUrya | 0:dfe6edabb8ec | 411 | node->setHardwareVersion(hardware_version); |
| RuslanUrya | 0:dfe6edabb8ec | 412 | |
| RuslanUrya | 0:dfe6edabb8ec | 413 | if (node_id.isValid()) |
| RuslanUrya | 0:dfe6edabb8ec | 414 | { |
| RuslanUrya | 0:dfe6edabb8ec | 415 | node->setNodeID(node_id); |
| RuslanUrya | 0:dfe6edabb8ec | 416 | } |
| RuslanUrya | 0:dfe6edabb8ec | 417 | |
| RuslanUrya | 0:dfe6edabb8ec | 418 | const auto res = node->start(node_status_transfer_priority); |
| RuslanUrya | 0:dfe6edabb8ec | 419 | if (res < 0) |
| RuslanUrya | 0:dfe6edabb8ec | 420 | { |
| RuslanUrya | 0:dfe6edabb8ec | 421 | throw LibuavcanErrorException(res); |
| RuslanUrya | 0:dfe6edabb8ec | 422 | } |
| RuslanUrya | 0:dfe6edabb8ec | 423 | |
| RuslanUrya | 0:dfe6edabb8ec | 424 | return node; |
| RuslanUrya | 0:dfe6edabb8ec | 425 | } |
| RuslanUrya | 0:dfe6edabb8ec | 426 | |
| RuslanUrya | 0:dfe6edabb8ec | 427 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 428 | * Use this function to create a sub-node instance with default SocketCAN driver. |
| RuslanUrya | 0:dfe6edabb8ec | 429 | * It accepts the list of interface names to use for the new node, e.g. "can1", "vcan2", "slcan0". |
| RuslanUrya | 0:dfe6edabb8ec | 430 | * Clock adjustment mode will be detected automatically unless provided explicitly. |
| RuslanUrya | 0:dfe6edabb8ec | 431 | * @throws uavcan_linux::Exception. |
| RuslanUrya | 0:dfe6edabb8ec | 432 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 433 | static inline SubNodePtr makeSubNode(const std::vector<std::string>& iface_names, |
| RuslanUrya | 0:dfe6edabb8ec | 434 | ClockAdjustmentMode clock_adjustment_mode = |
| RuslanUrya | 0:dfe6edabb8ec | 435 | SystemClock::detectPreferredClockAdjustmentMode()) |
| RuslanUrya | 0:dfe6edabb8ec | 436 | { |
| RuslanUrya | 0:dfe6edabb8ec | 437 | DriverPackPtr dp(new DriverPack(clock_adjustment_mode, iface_names)); |
| RuslanUrya | 0:dfe6edabb8ec | 438 | return SubNodePtr(new SubNode(dp)); |
| RuslanUrya | 0:dfe6edabb8ec | 439 | } |
| RuslanUrya | 0:dfe6edabb8ec | 440 | |
| RuslanUrya | 0:dfe6edabb8ec | 441 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 442 | * Use this function to create a sub-node instance with a custom driver. |
| RuslanUrya | 0:dfe6edabb8ec | 443 | * Clock adjustment mode will be detected automatically unless provided explicitly. |
| RuslanUrya | 0:dfe6edabb8ec | 444 | * @throws uavcan_linux::Exception. |
| RuslanUrya | 0:dfe6edabb8ec | 445 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 446 | static inline SubNodePtr makeSubNode(const std::shared_ptr<uavcan::ICanDriver>& can_driver, |
| RuslanUrya | 0:dfe6edabb8ec | 447 | ClockAdjustmentMode clock_adjustment_mode = |
| RuslanUrya | 0:dfe6edabb8ec | 448 | SystemClock::detectPreferredClockAdjustmentMode()) |
| RuslanUrya | 0:dfe6edabb8ec | 449 | { |
| RuslanUrya | 0:dfe6edabb8ec | 450 | DriverPackPtr dp(new DriverPack(clock_adjustment_mode, can_driver)); |
| RuslanUrya | 0:dfe6edabb8ec | 451 | return SubNodePtr(new SubNode(dp)); |
| RuslanUrya | 0:dfe6edabb8ec | 452 | } |
| RuslanUrya | 0:dfe6edabb8ec | 453 | |
| RuslanUrya | 0:dfe6edabb8ec | 454 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 455 | * This function extends the other two overloads in such a way that it instantiates the node |
| RuslanUrya | 0:dfe6edabb8ec | 456 | * and sets its Node ID immediately. |
| RuslanUrya | 0:dfe6edabb8ec | 457 | * |
| RuslanUrya | 0:dfe6edabb8ec | 458 | * Clock adjustment mode will be detected automatically unless provided explicitly. |
| RuslanUrya | 0:dfe6edabb8ec | 459 | * |
| RuslanUrya | 0:dfe6edabb8ec | 460 | * @throws uavcan_linux::Exception, uavcan_linux::LibuavcanErrorException. |
| RuslanUrya | 0:dfe6edabb8ec | 461 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 462 | template <typename DriverType> |
| RuslanUrya | 0:dfe6edabb8ec | 463 | static inline SubNodePtr makeSubNode(const DriverType& driver, |
| RuslanUrya | 0:dfe6edabb8ec | 464 | const uavcan::NodeID node_id, |
| RuslanUrya | 0:dfe6edabb8ec | 465 | ClockAdjustmentMode clock_adjustment_mode = |
| RuslanUrya | 0:dfe6edabb8ec | 466 | SystemClock::detectPreferredClockAdjustmentMode()) |
| RuslanUrya | 0:dfe6edabb8ec | 467 | { |
| RuslanUrya | 0:dfe6edabb8ec | 468 | SubNodePtr sub_node = makeSubNode(driver, clock_adjustment_mode); |
| RuslanUrya | 0:dfe6edabb8ec | 469 | sub_node->setNodeID(node_id); |
| RuslanUrya | 0:dfe6edabb8ec | 470 | return sub_node; |
| RuslanUrya | 0:dfe6edabb8ec | 471 | } |
| RuslanUrya | 0:dfe6edabb8ec | 472 | |
| RuslanUrya | 0:dfe6edabb8ec | 473 | } |