helpers.hpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #pragma once
00006 
00007 #include <memory>
00008 #include <string>
00009 #include <vector>
00010 #include <chrono>
00011 #include <iostream>
00012 #include <sstream>
00013 #include <uavcan/uavcan.hpp>
00014 #include <uavcan/node/sub_node.hpp>
00015 
00016 namespace uavcan_linux
00017 {
00018 /**
00019  * Default log sink will dump everything into stderr.
00020  * It is installed by default.
00021  */
00022 class DefaultLogSink : public uavcan::ILogSink
00023 {
00024     void log(const uavcan::protocol::debug::LogMessage& message) override
00025     {
00026         const auto tt = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
00027         const auto tstr = std::ctime(&tt);
00028         std::cerr << "### UAVCAN " << tstr << message << std::endl;
00029     }
00030 };
00031 
00032 /**
00033  * Wrapper over uavcan::ServiceClient<> for blocking calls.
00034  * Blocks on uavcan::Node::spin() internally until the call is complete.
00035  */
00036 template <typename DataType>
00037 class BlockingServiceClient : public uavcan::ServiceClient<DataType>
00038 {
00039     typedef uavcan::ServiceClient<DataType> Super;
00040 
00041     typename DataType::Response response_;
00042     bool call_was_successful_;
00043 
00044     void callback(const uavcan::ServiceCallResult<DataType>& res)
00045     {
00046         response_ = res.getResponse();
00047         call_was_successful_ = res.isSuccessful();
00048     }
00049 
00050     void setup()
00051     {
00052         Super::setCallback(std::bind(&BlockingServiceClient::callback, this, std::placeholders::_1));
00053         call_was_successful_ = false;
00054         response_ = typename DataType::Response();
00055     }
00056 
00057 public:
00058     BlockingServiceClient(uavcan::INode& node)
00059         : uavcan::ServiceClient<DataType>(node)
00060         , call_was_successful_(false)
00061     {
00062         setup();
00063     }
00064 
00065     /**
00066      * Performs a blocking service call using default timeout (see the specs).
00067      * Use @ref getResponse() to get the actual response.
00068      * Returns negative error code.
00069      */
00070     int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request)
00071     {
00072         return blockingCall(server_node_id, request, Super::getDefaultRequestTimeout());
00073     }
00074 
00075     /**
00076      * Performs a blocking service call using the specified timeout. Please consider using default timeout instead.
00077      * Use @ref getResponse() to get the actual response.
00078      * Returns negative error code.
00079      */
00080     int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request,
00081                      uavcan::MonotonicDuration timeout)
00082     {
00083         const auto SpinDuration = uavcan::MonotonicDuration::fromMSec(2);
00084         setup();
00085         Super::setRequestTimeout(timeout);
00086         const int call_res = Super::call(server_node_id, request);
00087         if (call_res >= 0)
00088         {
00089             while (Super::hasPendingCalls())
00090             {
00091                 const int spin_res = Super::getNode().spin(SpinDuration);
00092                 if (spin_res < 0)
00093                 {
00094                     return spin_res;
00095                 }
00096             }
00097         }
00098         return call_res;
00099     }
00100 
00101     /**
00102      * Whether the last blocking call was successful.
00103      */
00104     bool wasSuccessful() const { return call_was_successful_; }
00105 
00106     /**
00107      * Use this to retrieve the response of the last blocking service call.
00108      * This method returns default constructed response object if the last service call was unsuccessful.
00109      */
00110     const typename DataType::Response& getResponse() const { return response_; }
00111 };
00112 
00113 /**
00114  * Contains all drivers needed for uavcan::Node.
00115  */
00116 struct DriverPack
00117 {
00118     SystemClock clock;
00119     std::shared_ptr<uavcan::ICanDriver> can;
00120 
00121     explicit DriverPack(ClockAdjustmentMode clock_adjustment_mode,
00122                         const std::shared_ptr<uavcan::ICanDriver>& can_driver)
00123         : clock(clock_adjustment_mode)
00124         , can(can_driver)
00125     { }
00126 
00127     explicit DriverPack(ClockAdjustmentMode clock_adjustment_mode,
00128                         const std::vector<std::string>& iface_names)
00129         : clock(clock_adjustment_mode)
00130     {
00131         std::shared_ptr<SocketCanDriver> socketcan(new SocketCanDriver(clock));
00132         can = socketcan;
00133         for (auto ifn : iface_names)
00134         {
00135             if (socketcan->addIface(ifn) < 0)
00136             {
00137                 throw Exception("Failed to add iface " + ifn);
00138             }
00139         }
00140     }
00141 };
00142 
00143 typedef std::shared_ptr<DriverPack> DriverPackPtr;
00144 
00145 typedef std::shared_ptr<uavcan::INode> INodePtr;
00146 
00147 typedef std::shared_ptr<uavcan::Timer> TimerPtr;
00148 
00149 template <typename T>
00150 using SubscriberPtr = std::shared_ptr<uavcan::Subscriber<T>>;
00151 
00152 template <typename T>
00153 using PublisherPtr = std::shared_ptr<uavcan::Publisher<T>>;
00154 
00155 template <typename T>
00156 using ServiceServerPtr = std::shared_ptr<uavcan::ServiceServer<T>>;
00157 
00158 template <typename T>
00159 using ServiceClientPtr = std::shared_ptr<uavcan::ServiceClient<T>>;
00160 
00161 template <typename T>
00162 using BlockingServiceClientPtr = std::shared_ptr<BlockingServiceClient<T>>;
00163 
00164 static constexpr std::size_t NodeMemPoolSize = 1024 * 512;  ///< This shall be enough for any possible use case
00165 
00166 /**
00167  * Generic wrapper for node objects with some additional convenience functions.
00168  */
00169 template <typename NodeType>
00170 class NodeBase : public NodeType
00171 {
00172 protected:
00173     DriverPackPtr driver_pack_;
00174 
00175     static void enforce(int error, const std::string& msg)
00176     {
00177         if (error < 0)
00178         {
00179             std::ostringstream os;
00180             os << msg << " [" << error << "]";
00181             throw Exception(os.str());
00182         }
00183     }
00184 
00185     template <typename DataType>
00186     static std::string getDataTypeName()
00187     {
00188         return DataType::getDataTypeFullName();
00189     }
00190 
00191 public:
00192     /**
00193      * Simple forwarding constructor, compatible with uavcan::Node.
00194      */
00195     NodeBase(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) :
00196         NodeType(can_driver, clock)
00197     { }
00198 
00199     /**
00200      * Takes ownership of the driver container via the shared pointer.
00201      */
00202     explicit NodeBase(DriverPackPtr driver_pack)
00203         : NodeType(*driver_pack->can, driver_pack->clock)
00204         , driver_pack_(driver_pack)
00205     { }
00206 
00207     /**
00208      * Allocates @ref uavcan::Subscriber in the heap using shared pointer.
00209      * The subscriber will be started immediately.
00210      * @throws uavcan_linux::Exception.
00211      */
00212     template <typename DataType>
00213     SubscriberPtr<DataType> makeSubscriber(const typename uavcan::Subscriber<DataType>::Callback& cb)
00214     {
00215         SubscriberPtr<DataType> p(new uavcan::Subscriber<DataType>(*this));
00216         enforce(p->start(cb), "Subscriber start failure " + getDataTypeName<DataType>());
00217         return p;
00218     }
00219 
00220     /**
00221      * Allocates @ref uavcan::Publisher in the heap using shared pointer.
00222      * The publisher will be initialized immediately.
00223      * @throws uavcan_linux::Exception.
00224      */
00225     template <typename DataType>
00226     PublisherPtr<DataType> makePublisher(uavcan::MonotonicDuration tx_timeout =
00227                                              uavcan::Publisher<DataType>::getDefaultTxTimeout())
00228     {
00229         PublisherPtr<DataType> p(new uavcan::Publisher<DataType>(*this));
00230         enforce(p->init(), "Publisher init failure " + getDataTypeName<DataType>());
00231         p->setTxTimeout(tx_timeout);
00232         return p;
00233     }
00234 
00235     /**
00236      * Allocates @ref uavcan::ServiceServer in the heap using shared pointer.
00237      * The server will be started immediately.
00238      * @throws uavcan_linux::Exception.
00239      */
00240     template <typename DataType>
00241     ServiceServerPtr<DataType> makeServiceServer(const typename uavcan::ServiceServer<DataType>::Callback& cb)
00242     {
00243         ServiceServerPtr<DataType> p(new uavcan::ServiceServer<DataType>(*this));
00244         enforce(p->start(cb), "ServiceServer start failure " + getDataTypeName<DataType>());
00245         return p;
00246     }
00247 
00248     /**
00249      * Allocates @ref uavcan::ServiceClient in the heap using shared pointer.
00250      * The service client will be initialized immediately.
00251      * @throws uavcan_linux::Exception.
00252      */
00253     template <typename DataType>
00254     ServiceClientPtr<DataType> makeServiceClient(const typename uavcan::ServiceClient<DataType>::Callback& cb)
00255     {
00256         ServiceClientPtr<DataType> p(new uavcan::ServiceClient<DataType>(*this));
00257         enforce(p->init(), "ServiceClient init failure " + getDataTypeName<DataType>());
00258         p->setCallback(cb);
00259         return p;
00260     }
00261 
00262     /**
00263      * Allocates @ref uavcan_linux::BlockingServiceClient in the heap using shared pointer.
00264      * The service client will be initialized immediately.
00265      * @throws uavcan_linux::Exception.
00266      */
00267     template <typename DataType>
00268     BlockingServiceClientPtr<DataType> makeBlockingServiceClient()
00269     {
00270         BlockingServiceClientPtr<DataType> p(new BlockingServiceClient<DataType>(*this));
00271         enforce(p->init(), "BlockingServiceClient init failure " + getDataTypeName<DataType>());
00272         return p;
00273     }
00274 
00275     /**
00276      * Allocates @ref uavcan::Timer in the heap using shared pointer.
00277      * The timer will be started immediately in one-shot mode.
00278      */
00279     TimerPtr makeTimer(uavcan::MonotonicTime deadline, const typename uavcan::Timer::Callback& cb)
00280     {
00281         TimerPtr p(new uavcan::Timer(*this));
00282         p->setCallback(cb);
00283         p->startOneShotWithDeadline(deadline);
00284         return p;
00285     }
00286 
00287     /**
00288      * Allocates @ref uavcan::Timer in the heap using shared pointer.
00289      * The timer will be started immediately in periodic mode.
00290      */
00291     TimerPtr makeTimer(uavcan::MonotonicDuration period, const typename uavcan::Timer::Callback& cb)
00292     {
00293         TimerPtr p(new uavcan::Timer(*this));
00294         p->setCallback(cb);
00295         p->startPeriodic(period);
00296         return p;
00297     }
00298 
00299     const DriverPackPtr& getDriverPack() const { return driver_pack_; }
00300     DriverPackPtr& getDriverPack() { return driver_pack_; }
00301 };
00302 
00303 /**
00304  * Wrapper for uavcan::Node with some additional convenience functions.
00305  * Note that this wrapper adds stderr log sink to @ref uavcan::Logger, which can be removed if needed.
00306  * Do not instantiate this class directly; instead use the factory functions defined below.
00307  */
00308 class Node : public NodeBase<uavcan::Node<NodeMemPoolSize>>
00309 {
00310     typedef NodeBase<uavcan::Node<NodeMemPoolSize>> Base ;
00311 
00312     DefaultLogSink log_sink_;
00313 
00314 public:
00315     /**
00316      * Simple forwarding constructor, compatible with uavcan::Node.
00317      */
00318     Node(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) :
00319         Base (can_driver, clock)
00320     {
00321         getLogger().setExternalSink(&log_sink_);
00322     }
00323 
00324     /**
00325      * Takes ownership of the driver container via the shared pointer.
00326      */
00327     explicit Node(DriverPackPtr driver_pack) :
00328         Base (driver_pack)
00329     {
00330         getLogger().setExternalSink(&log_sink_);
00331     }
00332 };
00333 
00334 /**
00335  * Wrapper for uavcan::SubNode with some additional convenience functions.
00336  * Do not instantiate this class directly; instead use the factory functions defined below.
00337  */
00338 class SubNode : public NodeBase<uavcan::SubNode<NodeMemPoolSize>>
00339 {
00340     typedef NodeBase<uavcan::SubNode<NodeMemPoolSize>> Base ;
00341 
00342 public:
00343     /**
00344      * Simple forwarding constructor, compatible with uavcan::Node.
00345      */
00346     SubNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock) : Base (can_driver, clock) { }
00347 
00348     /**
00349      * Takes ownership of the driver container via the shared pointer.
00350      */
00351     explicit SubNode(DriverPackPtr driver_pack) : Base (driver_pack) { }
00352 };
00353 
00354 typedef std::shared_ptr<Node> NodePtr;
00355 typedef std::shared_ptr<SubNode> SubNodePtr;
00356 
00357 /**
00358  * Use this function to create a node instance with default SocketCAN driver.
00359  * It accepts the list of interface names to use for the new node, e.g. "can1", "vcan2", "slcan0".
00360  * Clock adjustment mode will be detected automatically unless provided explicitly.
00361  * @throws uavcan_linux::Exception.
00362  */
00363 static inline NodePtr makeNode(const std::vector<std::string>& iface_names,
00364                                ClockAdjustmentMode clock_adjustment_mode =
00365                                    SystemClock::detectPreferredClockAdjustmentMode())
00366 {
00367     DriverPackPtr dp(new DriverPack(clock_adjustment_mode, iface_names));
00368     return NodePtr(new Node(dp));
00369 }
00370 
00371 /**
00372  * Use this function to create a node instance with a custom driver.
00373  * Clock adjustment mode will be detected automatically unless provided explicitly.
00374  * @throws uavcan_linux::Exception.
00375  */
00376 static inline NodePtr makeNode(const std::shared_ptr<uavcan::ICanDriver>& can_driver,
00377                                ClockAdjustmentMode clock_adjustment_mode =
00378                                    SystemClock::detectPreferredClockAdjustmentMode())
00379 {
00380     DriverPackPtr dp(new DriverPack(clock_adjustment_mode, can_driver));
00381     return NodePtr(new Node(dp));
00382 }
00383 
00384 /**
00385  * This function extends the other two overloads in such a way that it instantiates and initializes
00386  * the node immediately; if initialization fails, it throws.
00387  *
00388  * If NodeID is not provided, it will not be initialized, and therefore the node will be started in
00389  * listen-only (i.e. silent) mode. The node can be switched to normal (i.e. non-silent) mode at any
00390  * later time by calling setNodeID() explicitly. Read the Node class docs for more info.
00391  *
00392  * Clock adjustment mode will be detected automatically unless provided explicitly.
00393  *
00394  * @throws uavcan_linux::Exception, uavcan_linux::LibuavcanErrorException.
00395  */
00396 template <typename DriverType>
00397 static inline NodePtr makeNode(const DriverType& driver,
00398                                const uavcan::NodeStatusProvider::NodeName& name,
00399                                const uavcan::protocol::SoftwareVersion& software_version,
00400                                const uavcan::protocol::HardwareVersion& hardware_version,
00401                                const uavcan::NodeID node_id = uavcan::NodeID(),
00402                                const uavcan::TransferPriority node_status_transfer_priority =
00403                                    uavcan::TransferPriority::Default,
00404                                ClockAdjustmentMode clock_adjustment_mode =
00405                                    SystemClock::detectPreferredClockAdjustmentMode())
00406 {
00407     NodePtr node = makeNode(driver, clock_adjustment_mode);
00408 
00409     node->setName(name);
00410     node->setSoftwareVersion(software_version);
00411     node->setHardwareVersion(hardware_version);
00412 
00413     if (node_id.isValid())
00414     {
00415         node->setNodeID(node_id);
00416     }
00417 
00418     const auto res = node->start(node_status_transfer_priority);
00419     if (res < 0)
00420     {
00421         throw LibuavcanErrorException(res);
00422     }
00423 
00424     return node;
00425 }
00426 
00427 /**
00428  * Use this function to create a sub-node instance with default SocketCAN driver.
00429  * It accepts the list of interface names to use for the new node, e.g. "can1", "vcan2", "slcan0".
00430  * Clock adjustment mode will be detected automatically unless provided explicitly.
00431  * @throws uavcan_linux::Exception.
00432  */
00433 static inline SubNodePtr makeSubNode(const std::vector<std::string>& iface_names,
00434                                   ClockAdjustmentMode clock_adjustment_mode =
00435                                       SystemClock::detectPreferredClockAdjustmentMode())
00436 {
00437     DriverPackPtr dp(new DriverPack(clock_adjustment_mode, iface_names));
00438     return SubNodePtr(new SubNode(dp));
00439 }
00440 
00441 /**
00442  * Use this function to create a sub-node instance with a custom driver.
00443  * Clock adjustment mode will be detected automatically unless provided explicitly.
00444  * @throws uavcan_linux::Exception.
00445  */
00446 static inline SubNodePtr makeSubNode(const std::shared_ptr<uavcan::ICanDriver>& can_driver,
00447                                      ClockAdjustmentMode clock_adjustment_mode =
00448                                          SystemClock::detectPreferredClockAdjustmentMode())
00449 {
00450     DriverPackPtr dp(new DriverPack(clock_adjustment_mode, can_driver));
00451     return SubNodePtr(new SubNode(dp));
00452 }
00453 
00454 /**
00455  * This function extends the other two overloads in such a way that it instantiates the node
00456  * and sets its Node ID immediately.
00457  *
00458  * Clock adjustment mode will be detected automatically unless provided explicitly.
00459  *
00460  * @throws uavcan_linux::Exception, uavcan_linux::LibuavcanErrorException.
00461  */
00462 template <typename DriverType>
00463 static inline SubNodePtr makeSubNode(const DriverType& driver,
00464                                      const uavcan::NodeID node_id,
00465                                      ClockAdjustmentMode clock_adjustment_mode =
00466                                          SystemClock::detectPreferredClockAdjustmentMode())
00467 {
00468     SubNodePtr sub_node = makeSubNode(driver, clock_adjustment_mode);
00469     sub_node->setNodeID(node_id);
00470     return sub_node;
00471 }
00472 
00473 }