mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-30 21:54:08 +08:00
106 lines
2.7 KiB
C++
106 lines
2.7 KiB
C++
/*
|
|
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <uavcan/node/subscriber.hpp>
|
|
#include <uavcan/node/timer.hpp>
|
|
#include <uavcan/node/service_client.hpp>
|
|
#include <uavcan/util/method_binder.hpp>
|
|
#include "../node/test_node.hpp"
|
|
|
|
|
|
template <typename DataType>
|
|
class SubscriptionCollector : uavcan::Noncopyable
|
|
{
|
|
typedef uavcan::ReceivedDataStructure<DataType> ReceivedDataStructType;
|
|
|
|
void handler(const ReceivedDataStructType& msg)
|
|
{
|
|
this->msg.reset(new ReceivedDataStructType(msg));
|
|
}
|
|
|
|
public:
|
|
std::auto_ptr<ReceivedDataStructType> msg;
|
|
|
|
typedef uavcan::MethodBinder<SubscriptionCollector*,
|
|
void (SubscriptionCollector::*)(const ReceivedDataStructType&)> Binder;
|
|
|
|
Binder bind() { return Binder(this, &SubscriptionCollector::handler); }
|
|
};
|
|
|
|
|
|
template <typename DataType>
|
|
struct SubscriberWithCollector
|
|
{
|
|
typedef SubscriptionCollector<DataType> Collector;
|
|
typedef uavcan::Subscriber<DataType, typename Collector::Binder> Subscriber;
|
|
|
|
Collector collector;
|
|
Subscriber subscriber;
|
|
|
|
SubscriberWithCollector(uavcan::INode& node)
|
|
: subscriber(node)
|
|
{ }
|
|
|
|
int start() { return subscriber.start(collector.bind()); }
|
|
};
|
|
|
|
|
|
template <typename DataType>
|
|
class ServiceCallResultCollector : uavcan::Noncopyable
|
|
{
|
|
typedef uavcan::ServiceCallResult<DataType> ResultType;
|
|
|
|
void handler(const ResultType& result)
|
|
{
|
|
this->result.reset(new ResultType(result));
|
|
}
|
|
|
|
public:
|
|
std::auto_ptr<ResultType> result;
|
|
|
|
typedef uavcan::MethodBinder<ServiceCallResultCollector*,
|
|
void (ServiceCallResultCollector::*)(const ResultType&)> Binder;
|
|
|
|
Binder bind() { return Binder(this, &ServiceCallResultCollector::handler); }
|
|
};
|
|
|
|
|
|
template <typename DataType>
|
|
struct ServiceClientWithCollector
|
|
{
|
|
typedef ServiceCallResultCollector<DataType> Collector;
|
|
typedef uavcan::ServiceClient<DataType, typename Collector::Binder> ServiceClient;
|
|
|
|
Collector collector;
|
|
ServiceClient client;
|
|
|
|
ServiceClientWithCollector(uavcan::INode& node)
|
|
: client(node)
|
|
{ }
|
|
|
|
int call(uavcan::NodeID node_id, const typename DataType::Request& request)
|
|
{
|
|
client.setCallback(collector.bind());
|
|
return client.call(node_id, request);
|
|
}
|
|
};
|
|
|
|
|
|
struct BackgroundSpinner : uavcan::TimerBase
|
|
{
|
|
uavcan::INode& spinning_node;
|
|
|
|
BackgroundSpinner(uavcan::INode& spinning_node, uavcan::INode& running_node)
|
|
: uavcan::TimerBase(running_node)
|
|
, spinning_node(spinning_node)
|
|
{ }
|
|
|
|
virtual void handleTimerEvent(const uavcan::TimerEvent&)
|
|
{
|
|
spinning_node.spin(uavcan::MonotonicDuration::fromMSec(1));
|
|
}
|
|
};
|