helpers.hpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #pragma once
00006 
00007 #include <uavcan/node/subscriber.hpp>
00008 #include <uavcan/node/timer.hpp>
00009 #include <uavcan/node/service_client.hpp>
00010 #include <uavcan/util/method_binder.hpp>
00011 #include "../node/test_node.hpp"
00012 
00013 
00014 template <typename DataType>
00015 class SubscriptionCollector : uavcan::Noncopyable
00016 {
00017     void handler(const DataType& msg)
00018     {
00019         this->msg.reset(new DataType(msg));
00020     }
00021 
00022 public:
00023     std::auto_ptr<DataType> msg;
00024 
00025     typedef uavcan::MethodBinder<SubscriptionCollector*,
00026                                  void (SubscriptionCollector::*)(const DataType&)> Binder;
00027 
00028     Binder bind() { return Binder(this, &SubscriptionCollector::handler); }
00029 };
00030 
00031 
00032 template <typename DataType>
00033 struct SubscriberWithCollector
00034 {
00035     typedef SubscriptionCollector<DataType> Collector;
00036     typedef uavcan::Subscriber<DataType, typename Collector::Binder> Subscriber;
00037 
00038     Collector collector;
00039     Subscriber subscriber;
00040 
00041     SubscriberWithCollector(uavcan::INode& node)
00042         : subscriber(node)
00043     { }
00044 
00045     int start() { return subscriber.start(collector.bind()); }
00046 };
00047 
00048 
00049 template <typename DataType>
00050 class ServiceCallResultCollector : uavcan::Noncopyable
00051 {
00052     typedef uavcan::ServiceCallResult<DataType> ServiceCallResult;
00053 
00054 public:
00055     class Result
00056     {
00057         const typename ServiceCallResult::Status status_;
00058         uavcan::ServiceCallID call_id_;
00059         typename DataType::Response response_;
00060 
00061     public:
00062         Result(typename ServiceCallResult::Status arg_status,
00063                uavcan::ServiceCallID arg_call_id,
00064                const typename DataType::Response& arg_response)
00065             : status_(arg_status)
00066             , call_id_(arg_call_id)
00067             , response_(arg_response)
00068         { }
00069 
00070         bool isSuccessful() const { return status_ == ServiceCallResult::Success; }
00071 
00072         typename ServiceCallResult::Status getStatus() const { return status_; }
00073 
00074         uavcan::ServiceCallID getCallID() const { return call_id_; }
00075 
00076         const typename DataType::Response& getResponse() const { return response_; }
00077         typename DataType::Response& getResponse() { return response_; }
00078     };
00079 
00080 private:
00081     void handler(const uavcan::ServiceCallResult<DataType>& tmp_result)
00082     {
00083         std::cout << tmp_result << std::endl;
00084         result.reset(new Result(tmp_result.getStatus(), tmp_result.getCallID(), tmp_result.getResponse()));
00085     }
00086 
00087 public:
00088     std::auto_ptr<Result> result;
00089 
00090     typedef uavcan::MethodBinder<ServiceCallResultCollector*,
00091                                  void (ServiceCallResultCollector::*)(const uavcan::ServiceCallResult<DataType>&)>
00092             Binder;
00093 
00094     Binder bind() { return Binder(this, &ServiceCallResultCollector::handler); }
00095 };
00096 
00097 
00098 template <typename DataType>
00099 struct ServiceClientWithCollector
00100 {
00101     typedef ServiceCallResultCollector<DataType> Collector;
00102     typedef uavcan::ServiceClient<DataType, typename Collector::Binder> ServiceClient;
00103 
00104     Collector collector;
00105     ServiceClient client;
00106 
00107     ServiceClientWithCollector(uavcan::INode& node)
00108         : client(node)
00109     { }
00110 
00111     int call(uavcan::NodeID node_id, const typename DataType::Request& request)
00112     {
00113         client.setCallback(collector.bind());
00114         return client.call(node_id, request);
00115     }
00116 };
00117 
00118 
00119 struct BackgroundSpinner : uavcan::TimerBase
00120 {
00121     uavcan::INode& spinning_node;
00122 
00123     BackgroundSpinner(uavcan::INode& spinning_node, uavcan::INode& running_node)
00124         : uavcan::TimerBase(running_node)
00125         , spinning_node(spinning_node)
00126     { }
00127 
00128     virtual void handleTimerEvent(const uavcan::TimerEvent&)
00129     {
00130         spinning_node.spin(uavcan::MonotonicDuration::fromMSec(1));
00131     }
00132 };
00133 
00134 template <typename CanDriver, typename MessageType>
00135 static inline void emulateSingleFrameBroadcastTransfer(CanDriver& can, uavcan::NodeID node_id,
00136                                                        const MessageType& message, uavcan::TransferID tid)
00137 {
00138     uavcan::StaticTransferBuffer<100> buffer;
00139     uavcan::BitStream bitstream(buffer);
00140     uavcan::ScalarCodec codec(bitstream);
00141 
00142     // Manual message publication
00143     ASSERT_LT(0, MessageType::encode(message, codec));
00144     ASSERT_GE(8, buffer.getMaxWritePos());
00145 
00146     // DataTypeID data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
00147     // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
00148     uavcan::Frame frame(MessageType::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
00149                         node_id, uavcan::NodeID::Broadcast, tid);
00150     frame.setStartOfTransfer(true);
00151     frame.setEndOfTransfer(true);
00152 
00153     ASSERT_EQ(buffer.getMaxWritePos(), frame.setPayload(buffer.getRawPtr(), buffer.getMaxWritePos()));
00154 
00155     uavcan::CanFrame can_frame;
00156     ASSERT_TRUE(frame.compile(can_frame));
00157 
00158     can.pushRxToAllIfaces(can_frame);
00159 }