fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 12:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan_linux::Node::makeBlockingServiceClient()

Browse Source
This commit is contained in:
Pavel Kirienko
2014-04-29 13:59:11 +04:00
parent 5d272ca410
commit ea19fea568
2 changed files with 86 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp
+73 -64
View File
@@ -27,6 +27,70 @@ class DefaultLogSink : public uavcan::ILogSink
}
};
/**
* Wrapper over uavcan::ServiceClient<> for blocking calls.
* Calls spin() internally.
*/
template <typename DataType>
class BlockingServiceClient : public uavcan::ServiceClient<DataType>
{
typedef uavcan::ServiceClient<DataType> Super;
typename DataType::Response response_;
bool call_was_successful_;
void callback(const uavcan::ServiceCallResult<DataType>& res)
{
response_ = res.response;
call_was_successful_ = res.isSuccessful();
}
void setup()
{
Super::setCallback(std::bind(&BlockingServiceClient::callback, this, std::placeholders::_1));
call_was_successful_ = false;
response_ = typename DataType::Response();
}
public:
BlockingServiceClient(uavcan::INode& node)
: uavcan::ServiceClient<DataType>(node)
, call_was_successful_(false)
{
setup();
}
int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request)
{
const auto SpinDuration = uavcan::MonotonicDuration::fromMSec(2);
setup();
const int call_res = Super::call(server_node_id, request);
if (call_res >= 0)
{
while (Super::isPending())
{
const int spin_res = Super::getNode().spin(SpinDuration);
if (spin_res < 0)
{
return spin_res;
}
}
}
return call_res;
}
int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request,
uavcan::MonotonicDuration timeout)
{
Super::setRequestTimeout(timeout);
return blockingCall(server_node_id, request);
}
bool wasSuccessful() const { return call_was_successful_; }
const typename DataType::Response& getResponse() const { return response_; }
};
/**
* Contains all drivers needed for uavcan::Node.
*/
@@ -129,6 +193,15 @@ public:
return p;
}
template <typename DataType>
std::shared_ptr<BlockingServiceClient<DataType>>
makeBlockingServiceClient()
{
std::shared_ptr<BlockingServiceClient<DataType>> p(new BlockingServiceClient<DataType>(*this));
enforce(p->init(), "BlockingServiceClient init failure " + getDataTypeName<DataType>());
return p;
}
TimerPtr makeTimer(uavcan::MonotonicTime deadline, const typename uavcan::Timer::Callback& cb)
{
TimerPtr p(new uavcan::Timer(*this));
@@ -172,68 +245,4 @@ static inline NodePtr makeNode(const std::vector<std::string>& iface_names)
return makeNode(iface_names, SystemClock::detectPreferredClockAdjustmentMode());
}
/**
* Wrapper over uavcan::ServiceClient<> for blocking calls.
* Calls spin() internally.
*/
template <typename DataType>
class BlockingServiceClient : public uavcan::ServiceClient<DataType>
{
typedef uavcan::ServiceClient<DataType> Super;
typename DataType::Response response_;
bool call_was_successful_;
void callback(const uavcan::ServiceCallResult<DataType>& res)
{
response_ = res.response;
call_was_successful_ = res.isSuccessful();
}
void setup()
{
Super::setCallback(std::bind(&BlockingServiceClient::callback, this, std::placeholders::_1));
call_was_successful_ = false;
response_ = typename DataType::Response();
}
public:
BlockingServiceClient(uavcan::INode& node)
: uavcan::ServiceClient<DataType>(node)
, call_was_successful_(false)
{
setup();
}
int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request)
{
const auto SpinDuration = uavcan::MonotonicDuration::fromMSec(2);
setup();
const int call_res = Super::call(server_node_id, request);
if (call_res >= 0)
{
while (Super::isPending())
{
const int spin_res = Super::getNode().spin(SpinDuration);
if (spin_res < 0)
{
return spin_res;
}
}
}
return call_res;
}
int blockingCall(uavcan::NodeID server_node_id, const typename DataType::Request& request,
uavcan::MonotonicDuration timeout)
{
Super::setRequestTimeout(timeout);
return blockingCall(server_node_id, request);
}
bool wasSuccessful() const { return call_was_successful_; }
const typename DataType::Response& getResponse() const { return response_; }
};
}