fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-12 06:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Initial implementation of NodeInfoRetriever; fixes pending

Browse Source
This commit is contained in:
Pavel Kirienko
2015-05-13 21:59:43 +03:00
parent bc00899e70
commit 842319a290
2 changed files with 479 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/include/uavcan/protocol/node_info_retriever.hpp
+340
View File
@@ -0,0 +1,340 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED
#define UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED
#include <uavcan/build_config.hpp>
#include <uavcan/debug.hpp>
#include <uavcan/util/map.hpp>
#include <uavcan/node/service_client.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
#include <uavcan/protocol/GetNodeInfo.hpp>
namespace uavcan
{
/**
* Classes that need to receive GetNodeInfo responses should implement this interface.
*/
class INodeInfoListener
{
public:
/**
* Called when a response to GetNodeInfo request is received. This happens shortly after the node restarts or
* becomes online for the first time.
* @param node_id Node ID of the node
* @param response Node info struct
*/
virtual void handleNodeInfoRetrieved(NodeID node_id, const protocol::GetNodeInfo::Response& node_info) = 0;
/**
* Called when the retriever decides that the node does not support the GetNodeInfo service.
* This method will never be called if the number of attempts is unlimited.
*/
virtual void handleNodeInfoUnavailable(NodeID node_id) = 0;
/**
* This call is routed directly from @ref NodeStatusMonitor.
* Default implementation does nothing.
* @param event Node status change event
*/
virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event)
{
(void)event;
}
/**
* This call is routed directly from @ref NodeStatusMonitor.
* Default implementation does nothing.
* @param msg Node status message
*/
virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
(void)msg;
}
virtual ~INodeInfoListener() { }
};
/**
* This class automatically retrieves a response to GetNodeInfo once a node appears online or restarts.
* It does a number of attempts in case if there's a communication failure before assuming that the node does not
* implement the GetNodeInfo service.
* Events from this class can be routed to many subscribers.
*/
class NodeInfoRetriever : protected NodeStatusMonitor
{
public:
enum { MaxNumRequestAttempts = 254 };
enum { DefaultNumRequestAttempts = 30 };
enum { UnlimitedRequestAttempts = 0 };
private:
typedef MethodBinder<NodeInfoRetriever*,
void (NodeInfoRetriever::*)(const ServiceCallResult<protocol::GetNodeInfo>&)>
GetNodeInfoResponseCallback;
struct Entry
{
uint32_t uptime_sec;
uint8_t num_attempts_made;
bool request_needed; ///< Always false for unknown nodes
bool updated_since_last_attempt; ///< Always false for unknown nodes
Entry()
: uptime_sec(0)
, num_attempts_made(0)
, request_needed(false)
, updated_since_last_attempt(false)
{
#if UAVCAN_DEBUG
StaticAssert<sizeof(Entry) <= 8>::check();
#endif
}
};
/*
* Callers are used with removeWhere() predicate. They don't actually remove anything.
*/
struct NodeInfoRetrievedHandlerCaller
{
const NodeID node_id;
const protocol::GetNodeInfo::Response& node_info;
NodeInfoRetrievedHandlerCaller(NodeID arg_node_id, const protocol::GetNodeInfo::Response& arg_node_info)
: node_id(arg_node_id)
, node_info(arg_node_info)
{ }
bool operator()(INodeInfoListener* key, bool)
{
UAVCAN_ASSERT(key != NULL);
key->handleNodeInfoRetrieved(node_id, node_info);
return false;
}
};
template <typename Event>
struct GenericHandlerCaller
{
void (INodeInfoListener::* const method)(Event);
Event event;
GenericHandlerCaller(void (INodeInfoListener::*arg_method)(Event), Event arg_event)
: method(arg_method)
, event(arg_event)
{ }
bool operator()(INodeInfoListener* key, bool)
{
UAVCAN_ASSERT(key != NULL);
(key->*method)(event);
return false;
}
};
/*
* State
*/
Entry entries_[NodeID::Max]; // [1, NodeID::Max]
Map<INodeInfoListener*, bool, 2> listeners_; // Only keys are used
ServiceClient<protocol::GetNodeInfo, GetNodeInfoResponseCallback> get_node_info_client_;
mutable uint8_t last_picked_node_;
uint8_t num_attempts_;
/*
* Methods
*/
const Entry& getEntry(NodeID node_id) const { return const_cast<NodeInfoRetriever*>(this)->getEntry(node_id); }
Entry& getEntry(NodeID node_id)
{
if (node_id.get() < 1 || node_id.get() > NodeID::Max)
{
handleFatalError("NodeInfoRetriever NodeID");
}
return entries_[node_id.get() - 1];
}
NodeID pickNextNodeToQuery() const
{
for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin
{
last_picked_node_++;
if (last_picked_node_ > NodeID::Max)
{
last_picked_node_ = 1;
}
UAVCAN_ASSERT((last_picked_node_ >= 1) &&
(last_picked_node_ <= NodeID::Max));
const Entry& entry = getEntry(last_picked_node_);
if (entry.request_needed && entry.updated_since_last_attempt)
{
UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_));
return NodeID(last_picked_node_);
}
}
return NodeID(); // No node could be found
}
virtual void handleTimerEvent(const TimerEvent& event) // 2 Hz
{
NodeStatusMonitor::handleTimerEvent(event);
if (!get_node_info_client_.isPending()) // If request is pending, this condition will fail every second time
{
const NodeID next = pickNextNodeToQuery(); // Typ. 1 Hz
if (next.isUnicast())
{
getEntry(next).updated_since_last_attempt = false;
const int res = get_node_info_client_.call(next, protocol::GetNodeInfo::Request());
if (res < 0)
{
get_node_info_client_.getNode().registerInternalFailure("NodeInfoRetriever GetNodeInfo call");
}
}
}
}
virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event)
{
const bool was_offline = !event.old_status.known ||
(event.old_status.status_code == protocol::NodeStatus::STATUS_OFFLINE);
const bool offline_now = !event.status.known ||
(event.status.status_code == protocol::NodeStatus::STATUS_OFFLINE);
if (was_offline || offline_now)
{
Entry& entry = getEntry(event.node_id);
entry.request_needed = !offline_now;
entry.num_attempts_made = 0;
UAVCAN_TRACE("NodeInfoRetriever", "Offline status change: node ID %d, request needed: %d",
int(event.node_id.get()), int(entry.request_needed));
}
listeners_.removeWhere(
GenericHandlerCaller<const NodeStatusChangeEvent&>(&INodeInfoListener::handleNodeStatusChange, event));
}
virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
Entry& entry = getEntry(msg.getSrcNodeID());
if (msg.uptime_sec < entry.uptime_sec)
{
entry.request_needed = true;
entry.num_attempts_made = 0;
}
entry.uptime_sec = msg.uptime_sec;
entry.updated_since_last_attempt = true;
listeners_.removeWhere(GenericHandlerCaller<const ReceivedDataStructure<protocol::NodeStatus>&>(
&INodeInfoListener::handleNodeStatusMessage, msg));
}
void handleGetNodeInfoResponse(const ServiceCallResult<protocol::GetNodeInfo>& result)
{
Entry& entry = getEntry(result.server_node_id);
if (result.isSuccessful())
{
/*
* Updating the uptime here allows to properly handle a corner case where the service response arrives
* after the device has restarted and published its new NodeStatus (although it's unlikely to happen).
*/
entry.uptime_sec = result.response.status.uptime_sec;
entry.request_needed = false;
listeners_.removeWhere(NodeInfoRetrievedHandlerCaller(result.server_node_id, result.response));
}
else
{
if (num_attempts_ != UnlimitedRequestAttempts)
{
entry.num_attempts_made++;
if (entry.num_attempts_made >= num_attempts_)
{
entry.request_needed = false;
listeners_.removeWhere(GenericHandlerCaller<NodeID>(&INodeInfoListener::handleNodeInfoUnavailable,
result.server_node_id));
}
}
}
}
public:
NodeInfoRetriever(INode& node)
: NodeStatusMonitor(node)
, listeners_(node.getAllocator())
, get_node_info_client_(node)
, last_picked_node_(1)
, num_attempts_(DefaultNumRequestAttempts)
{ }
/**
* Starts the retriever.
* Destroy the object to stop it.
* Returns negative error code.
*/
int start()
{
int res = NodeStatusMonitor::start();
if (res < 0)
{
return res;
}
res = get_node_info_client_.init();
if (res < 0)
{
return res;
}
get_node_info_client_.setCallback(GetNodeInfoResponseCallback(this,
&NodeInfoRetriever::handleGetNodeInfoResponse));
return 0;
}
/**
* Adds one listener to the set.
* May return -ErrMemory if there's no space to add the listener.
*/
int addListener(INodeInfoListener* listener)
{
UAVCAN_ASSERT(listener != NULL);
bool* value = listeners_.insert(listener, true);
return (value == NULL) ? -ErrMemory : 0;
}
/**
* Removes the listener.
* If the listener was not registered, nothing will be done.
*/
void removeListener(INodeInfoListener* listener)
{
UAVCAN_ASSERT(listener != NULL);
listeners_.remove(listener);
}
/**
* Number of attempts to retrieve GetNodeInfo response before giving up on the assumption that the service is
* not implemented.
* Zero is a special value that can be used to set unlimited number of attempts, @ref UnlimitedRequestAttempts.
*/
uint8_t getNumRequestAttempts() const { return num_attempts_; }
void setNumRequestAttempts(const uint8_t num)
{
num_attempts_ = min(static_cast<uint8_t>(MaxNumRequestAttempts), num);
}
};
}
#endif // Include guard
libuavcan/test/protocol/node_info_retriever.cpp
+139
View File
@@ -0,0 +1,139 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#if __GNUC__
// We need auto_ptr for compatibility reasons
# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif
#include <memory>
#include <gtest/gtest.h>
#include <uavcan/protocol/node_info_retriever.hpp>
#include <uavcan/protocol/node_status_provider.hpp>
#include "helpers.hpp"
static void publishNodeStatus(PairableCanDriver& can, uavcan::NodeID node_id, uavcan::uint8_t status_code,
uavcan::uint32_t uptime_sec, uavcan::TransferID tid)
{
uavcan::protocol::NodeStatus msg;
msg.status_code = status_code;
msg.uptime_sec = uptime_sec;
emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid);
}
struct NodeInfoListener : public uavcan::INodeInfoListener
{
std::auto_ptr<uavcan::protocol::GetNodeInfo::Response> last_node_info;
uavcan::NodeID last_node_id;
unsigned status_message_cnt;
unsigned status_change_cnt;
unsigned info_unavailable_cnt;
NodeInfoListener()
: status_message_cnt(0)
, status_change_cnt(0)
, info_unavailable_cnt(0)
{ }
virtual void handleNodeInfoRetrieved(uavcan::NodeID node_id,
const uavcan::protocol::GetNodeInfo::Response& node_info)
{
last_node_id = node_id;
std::cout << node_info << std::endl;
last_node_info.reset(new uavcan::protocol::GetNodeInfo::Response(node_info));
}
virtual void handleNodeInfoUnavailable(uavcan::NodeID node_id)
{
std::cout << "NODE INFO FOR " << int(node_id.get()) << " IS UNAVAILABLE" << std::endl;
last_node_id = node_id;
info_unavailable_cnt++;
}
virtual void handleNodeStatusChange(const uavcan::NodeStatusMonitor::NodeStatusChangeEvent& event)
{
(void)event;
status_change_cnt++;
}
virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure<uavcan::protocol::NodeStatus>& msg)
{
std::cout << msg << std::endl;
status_message_cnt++;
}
};
TEST(NodeInfoRetriever, Basic)
{
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalDiscoveryRequest> _reg3;
InterlinkedTestNodesWithSysClock nodes;
uavcan::NodeInfoRetriever retr(nodes.a);
std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl;
std::cout << "sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>): "
<< sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>) << std::endl;
std::auto_ptr<uavcan::NodeStatusProvider> provider(new uavcan::NodeStatusProvider(nodes.b));
NodeInfoListener listener;
/*
* Initialization
*/
ASSERT_LE(0, retr.start());
retr.removeListener(&listener); // Does nothing
retr.addListener(&listener);
uavcan::protocol::HardwareVersion hwver;
hwver.unique_id[0] = 123;
hwver.unique_id[4] = 213;
hwver.unique_id[8] = 45;
provider->setName("Ivan");
provider->setHardwareVersion(hwver);
ASSERT_LE(0, provider->startAndPublish());
/*
* Waiting for discovery
*/
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600));
ASSERT_EQ(2, listener.status_message_cnt);
ASSERT_EQ(1, listener.status_change_cnt);
ASSERT_EQ(0, listener.info_unavailable_cnt);
ASSERT_TRUE(listener.last_node_info.get());
ASSERT_EQ(uavcan::NodeID(2), listener.last_node_id);
ASSERT_EQ("Ivan", listener.last_node_info->name);
ASSERT_TRUE(hwver == listener.last_node_info->hardware_version);
provider.reset(); // Moving the provider out of the way; its entry will timeout meanwhile
/*
* Declaring a bunch of different nodes that don't support GetNodeInfo
*/
retr.setNumRequestAttempts(3);
uavcan::TransferID tid;
publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 0, 10, tid);
publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 0, 10, tid);
publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 10, tid);
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100));
tid.increment();
publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 0, 11, tid);
publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 0, 11, tid);
publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 11, tid);
// TODO finish the test when the logic is fixed
}