iterator of neighbours time map bug fix

This commit is contained in:
David St-Onge 2018-07-30 00:09:29 -04:00
parent c5f0a3ddd4
commit 00fdc56642
1 changed files with 8 additions and 3 deletions

View File

@ -1468,15 +1468,20 @@ void roscontroller::time_sync_step()
double avgOffset = 0; double avgOffset = 0;
int offsetCount = 0; int offsetCount = 0;
map<int, buzz_utility::neighbor_time>::iterator it; map<int, buzz_utility::neighbor_time>::iterator it;
for (it = neighbours_time_map.begin(); it != neighbours_time_map.end(); ++it) for (it = neighbours_time_map.begin(); it != neighbours_time_map.end(); )
{ {
avgRate += (it->second).relative_rate; avgRate += (it->second).relative_rate;
// estimate current offset // estimate current offset
int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time; int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time;
avgOffset = avgOffset + offset; avgOffset = avgOffset + offset;
offsetCount++; offsetCount++;
if((it->second).age < BUZZRATE) (it->second).age++; if((it->second).age > BUZZRATE){
else neighbours_time_map.erase(it); neighbours_time_map.erase(it);
}
else{
(it->second).age++;
++it;
}
ROS_INFO("Size of nei time %i",neighbours_time_map.size()); ROS_INFO("Size of nei time %i",neighbours_time_map.size());
} }
avgRate = avgRate/(neighbours_time_map.size()+1); avgRate = avgRate/(neighbours_time_map.size()+1);