fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

137
libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp
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_; }
};
}

26
libuavcan_drivers/linux/test/test_nodetool.cpp
View File

@ -135,14 +135,14 @@ void executeCommand(const uavcan_linux::NodePtr& node, const std::string& cmd,
{
if (cmd == "param")
{
uavcan_linux::BlockingServiceClient<uavcan::protocol::param::GetSet> get_set(*node);
auto client = node->makeBlockingServiceClient<uavcan::protocol::param::GetSet>();
printGetSetResponseHeader();
uavcan::protocol::param::GetSet::Request request;
if (args.empty())
{
while (true)
{
auto response = call(get_set, node_id, request);
auto response = call(*client, node_id, request);
if (response.name.empty())
{
break;
@ -155,25 +155,25 @@ void executeCommand(const uavcan_linux::NodePtr& node, const std::string& cmd,
{
request.name = args.at(0).c_str();
request.value.value_float.push_back(std::stof(args.at(1)));
printGetSetResponse(call(get_set, node_id, request));
printGetSetResponse(call(*client, node_id, request));
}
}
else if (cmd == "param_save" || cmd == "param_erase")
{
uavcan_linux::BlockingServiceClient<uavcan::protocol::param::SaveErase> save_erase(*node);
auto client = node->makeBlockingServiceClient<uavcan::protocol::param::SaveErase>();
uavcan::protocol::param::SaveErase::Request request;
request.opcode = (cmd == "param_save") ? request.OPCODE_SAVE : request.OPCODE_ERASE;
std::cout << call(save_erase, node_id, request) << std::endl;
std::cout << call(*client, node_id, request) << std::endl;
}
else if (cmd == "restart")
{
uavcan_linux::BlockingServiceClient<uavcan::protocol::RestartNode> restart(*node);
auto client = node->makeBlockingServiceClient<uavcan::protocol::RestartNode>();
uavcan::protocol::RestartNode::Request request;
request.magic_number = request.MAGIC_NUMBER;
(void)restart.blockingCall(node_id, request);
if (restart.wasSuccessful())
(void)client->blockingCall(node_id, request);
if (client->wasSuccessful())
{
std::cout << restart.getResponse() << std::endl;
std::cout << client->getResponse() << std::endl;
}
else
{
@ -182,13 +182,13 @@ void executeCommand(const uavcan_linux::NodePtr& node, const std::string& cmd,
}
else if (cmd == "info")
{
uavcan_linux::BlockingServiceClient<uavcan::protocol::GetNodeInfo> client(*node);
std::cout << call(client, node_id, uavcan::protocol::GetNodeInfo::Request()) << std::endl;
auto client = node->makeBlockingServiceClient<uavcan::protocol::GetNodeInfo>();
std::cout << call(*client, node_id, uavcan::protocol::GetNodeInfo::Request()) << std::endl;
}
else if (cmd == "tstat")
{
uavcan_linux::BlockingServiceClient<uavcan::protocol::GetTransportStats> client(*node);
std::cout << call(client, node_id, uavcan::protocol::GetTransportStats::Request()) << std::endl;
auto client = node->makeBlockingServiceClient<uavcan::protocol::GetTransportStats>();
std::cout << call(*client, node_id, uavcan::protocol::GetTransportStats::Request()) << std::endl;
}
else if (cmd == "hardpoint")
{