NodeID pickNextNodeToQuery(bool& out_at_least_one_request_needed) const
{
out_at_least_one_request_needed = false;
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)
{
out_at_least_one_request_needed = true;
if (entry.updated_since_last_attempt &&
!get_node_info_client_.hasPendingCallToServer(last_picked_node_))
{
UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_));
return NodeID(last_picked_node_);
}
}
}
return NodeID(); // No node could be found
}
void constrain_to_field(Node& n1) {
// keep snake in the middle of the left area
if (n1.type() == COMMIT || n1.display_type() == SNAKE_TAIL || n1.display_type() == HEAD) {
n1.velocity().x -= 0.0001*pow(n1.pos().x-graph.left_border/2,3);
}
// keep HEAD around y = graph.history_pos
if (n1.oid() == NodeID(REF, "HEAD"))
n1.velocity().y -= 0.0001*pow(n1.pos().y-graph.history_pos,3);
float border = 200;
float strength = 0.00002;
// keep blobs and trees in the middle area
if (n1.type() == BLOB || n1.type() == TREE) {
if (n1.pos().x < graph.left_border + border)
n1.velocity().x += strength*pow(graph.left_border + border - n1.pos().x,3);
if (n1.pos().x > graph.right_border - border)
n1.velocity().x += strength*pow(graph.right_border - border - n1.pos().x,3);
if (n1.pos().y < border)
n1.velocity().y += strength*pow(border - n1.pos().y,3);
if (n1.pos().y > graph.height-border)
n1.velocity().y += strength*pow(graph.height - border - n1.pos().y,3);
}
}
void ProofNode::PrintNodes(ostream& fout)
{
//print child connections if possible
if (_left)
{
fout << "\t"+NodeID()+" -> "+_left->NodeID()+" ;" << endl;
}
if (_right)
{
fout << "\t"+NodeID()+" -> "+_right->NodeID()+" ;" << endl;
}
//recurse to lower nodes
if (_left) this->_left->PrintNodes(fout);
if (_right) this->_right->PrintNodes(fout);
}
// Internally uses DFS for graph traversal (in fact it's topological sort)
void NodeTree::prepareListImpl()
{
// Initialize color map with white color
std::vector<ENodeColor> colorMap(_nodes.size(), ENodeColor::White);
_executeList.clear();
// For each invalid nodes (in terms of executable) mark them black
// so "true" graph traversal can skip them
for(NodeID nodeID = 0; nodeID < NodeID(_nodes.size()); ++nodeID)
{
if(!validateNode(nodeID))
continue;
if(colorMap[nodeID] != ENodeColor::White)
continue;
if(isNodeExecutable(nodeID))
continue;
if(!depthFirstSearch(nodeID, colorMap))
return;
}
// For each tagged and valid nodes that haven't been visited yet do ..
for(NodeID nodeID = 0; nodeID < NodeID(_nodes.size()); ++nodeID)
{
if(!validateNode(nodeID))
continue;
if(colorMap[nodeID] != ENodeColor::White)
continue;
if(!_nodes[nodeID].flag(ENodeFlags::Tagged))
continue;
if(!depthFirstSearch(nodeID, colorMap, &_executeList))
{
_executeList.clear();
return;
}
}
// Topological sort gives result in inverse order
std::reverse(std::begin(_executeList), std::end(_executeList));
}
void ObjectCM::_sendEmptyVersion( const MasterCMCommand& command,
const uint128_t& version,
const bool multicast )
{
NodePtr node = command.getNode();
ConnectionPtr connection = node->getConnection( multicast );
ObjectDataOCommand( Connections( 1, connection ), CMD_OBJECT_INSTANCE,
COMMANDTYPE_OBJECT, _object->getID(),
command.getInstanceID(), version, 0, 0, true, 0 )
<< NodeID() << _object->getInstanceID();
}
void NodeTree::notifyFinish()
{
for(NodeID nodeID = 0; nodeID < NodeID(_nodes.size()); ++nodeID)
{
if(!validateNode(nodeID))
continue;
_nodes[nodeID].finish();
// Finally, tag node that need to be auto-tagged
if(_nodes[nodeID].flag(ENodeFlags::AutoTag))
tagNode(nodeID);
}
}
Node &nearest_node(float x, float y) {
Node *best = 0;
float best_distance = 99999999;
Vec2f pos(x,y);
for(map<NodeID,Node>::iterator it = nodes.begin(); it != nodes.end(); it++) {
if (!it->second.visible()) continue;
if (it->second.oid() == NodeID(INDEX,"index")) continue;
if (it->second.oid().type == INDEX_ENTRY) continue;
float distance = it->second.pos().distance(pos);
if (distance < best_distance) {
best_distance = distance;
best = &(it->second);
}
}
return (*best);
}
NodeID pickNextNodeToQueryAndCleanupMap()
{
NodeID node_id;
do
{
node_id = pickNextNodeToQuery();
if (node_id.isUnicast())
{
if (needToQuery(node_id))
{
return node_id;
}
else
{
removeNode(node_id);
}
}
}
while (node_id.isUnicast());
return NodeID();
}
NodeID NodeTree::allocateNodeID()
{
NodeID id = InvalidNodeID;
// Check for recycled ids
if(_recycledIDs.empty())
{
// Need to create new one
id = NodeID(_nodes.size());
// Expand node array - createNode relies on it
_nodes.resize(_nodes.size() + 1);
}
else
{
// Reuse unused but still cached one
id = _recycledIDs.back();
_recycledIDs.pop_back();
}
return id;
}
//---------------------------------------------------------------------------
// identifier master node mapping
//---------------------------------------------------------------------------
NodeID ObjectStore::findMasterNodeID( const uint128_t& identifier )
{
LB_TS_NOT_THREAD( _commandThread );
// OPT: look up locally first?
Nodes nodes;
_localNode->getNodes( nodes );
// OPT: send to multiple nodes at once?
for( NodesIter i = nodes.begin(); i != nodes.end(); ++i )
{
NodePtr node = *i;
lunchbox::Request< NodeID > request =
_localNode->registerRequest< NodeID >();
LBLOG( LOG_OBJECTS ) << "Finding " << identifier << " on " << node
<< " req " << request.getID() << std::endl;
node->send( CMD_NODE_FIND_MASTER_NODE_ID ) << identifier << request;
try
{
const NodeID& masterNodeID = request.wait( Global::getTimeout( ));
if( masterNodeID != 0 )
{
LBLOG( LOG_OBJECTS ) << "Found " << identifier << " on "
<< masterNodeID << std::endl;
return masterNodeID;
}
}
catch( const lunchbox::FutureTimeout& )
{
_localNode->unregisterRequest( request.getID( ));
throw;
}
}
return NodeID();
}
NodeID pickNextNodeToQuery() const
{
// This essentially means that we pick first available node. Remember that the map is unordered.
const NodeMap::KVPair* const pair = node_map_.getByIndex(0);
return (pair == NULL) ? NodeID() : pair->key;
}
void ProofNode::PrintLabels(ostream& fout)
{
fout << NodeID() << " [label=" << NodeName() << "];" << endl;
if (_left) this->_left->PrintLabels(fout);
if (_right) this->_right->PrintLabels(fout);
}
/**
* Initialization is performed as follows (every step may fail and return an error):
* 1. Log is restored or initialized.
* 2. Current term is restored. If there was no current term stored and the log is empty, it will be initialized
* with zero.
* 3. VotedFor value is restored. If there was no VotedFor value stored, the log is empty, and the current term is
* zero, the value will be initialized with zero.
*/
int init()
{
/*
* Reading log
*/
int res = log_.init();
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", "Log init failed: %d", res);
return res;
}
const Entry* const last_entry = log_.getEntryAtIndex(log_.getLastIndex());
if (last_entry == UAVCAN_NULLPTR)
{
UAVCAN_ASSERT(0);
return -ErrLogic;
}
const bool log_is_empty = (log_.getLastIndex() == 0) && (last_entry->term == 0);
StorageMarshaller io(storage_);
/*
* Reading currentTerm
*/
if (storage_.get(getCurrentTermKey()).empty() && log_is_empty)
{
// First initialization
current_term_ = 0;
res = io.setAndGetBack(getCurrentTermKey(), current_term_);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Failed to init current term: %d", res);
return res;
}
if (current_term_ != 0)
{
return -ErrFailure;
}
}
else
{
// Restoring
res = io.get(getCurrentTermKey(), current_term_);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Failed to read current term: %d", res);
return res;
}
}
tracer_.onEvent(TraceRaftCurrentTermRestored, current_term_);
if (current_term_ < last_entry->term)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Persistent storage is damaged: current term is less than term of the last log entry (%u < %u)",
unsigned(current_term_), unsigned(last_entry->term));
return -ErrLogic;
}
/*
* Reading votedFor
*/
if (storage_.get(getVotedForKey()).empty() && log_is_empty && (current_term_ == 0))
{
// First initialization
voted_for_ = NodeID(0);
uint32_t stored_voted_for = 0;
res = io.setAndGetBack(getVotedForKey(), stored_voted_for);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Failed to init votedFor: %d", res);
return res;
}
if (stored_voted_for != 0)
{
return -ErrFailure;
}
}
else
{
// Restoring
uint32_t stored_voted_for = 0;
res = io.get(getVotedForKey(), stored_voted_for);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Failed to read votedFor: %d", res);
return res;
}
if (stored_voted_for > NodeID::Max)
{
return -ErrInvalidConfiguration;
}
voted_for_ = NodeID(uint8_t(stored_voted_for));
}
tracer_.onEvent(TraceRaftVotedForRestored, voted_for_.get());
return 0;
}
int resetVotedFor() { return setVotedFor(NodeID(0)); }
int DynamicNodeIDClient::start(const protocol::HardwareVersion& hardware_version,
const NodeID preferred_node_id,
const TransferPriority transfer_priority)
{
terminate();
// Allocation is not possible if node ID is already set
if (dnida_pub_.getNode().getNodeID().isUnicast())
{
return -ErrLogic;
}
// Unique ID initialization & validation
copy(hardware_version.unique_id.begin(), hardware_version.unique_id.end(), unique_id_);
bool unique_id_is_zero = true;
for (uint8_t i = 0; i < sizeof(unique_id_); i++)
{
if (unique_id_[i] != 0)
{
unique_id_is_zero = false;
break;
}
}
if (unique_id_is_zero)
{
return -ErrInvalidParam;
}
if (!preferred_node_id.isValid()) // Only broadcast and unicast are allowed
{
return -ErrInvalidParam;
}
// Initializing the fields
preferred_node_id_ = preferred_node_id;
allocated_node_id_ = NodeID();
allocator_node_id_ = NodeID();
UAVCAN_ASSERT(preferred_node_id_.isValid());
UAVCAN_ASSERT(!allocated_node_id_.isValid());
UAVCAN_ASSERT(!allocator_node_id_.isValid());
// Initializing node objects - Rule A
int res = dnida_pub_.init();
if (res < 0)
{
return res;
}
dnida_pub_.allowAnonymousTransfers();
dnida_pub_.setPriority(transfer_priority);
res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDClient::handleAllocation));
if (res < 0)
{
return res;
}
dnida_sub_.allowAnonymousTransfers();
startPeriodic(MonotonicDuration::fromMSec(protocol::dynamic_node_id::Allocation::DEFAULT_REQUEST_PERIOD_MS));
return 0;
}
