// Remove a outlier from sample before pushing it into the queue
void SamplesInput::Remove_Outlier_Avalon(std::queue<base::samples::RigidBodyState> &queue, base::samples::RigidBodyState &sample)
{
//TODO Be careful with the outlier (initial case) and the amplitude. Verify with sample 12645 (18:03:40.383727)
if ( (!queue.empty()) && (queue.back().angular_velocity[0] != 0) &&
((sample.angular_velocity[0]/queue.back().angular_velocity[0]) < 0.05 ||
(sample.angular_velocity[0]/queue.back().angular_velocity[0] > 20)) )
{
//std::cout << std::endl << "actual_RBS.position[0]_before: "<< actual_RBS.position[0] << std::endl;
sample.angular_velocity[0] = queue.back().angular_velocity[0];
//std::cout << "actual_RBS.posititon[0]_after: "<< actual_RBS.position[0] << std::endl<< std::endl;
}
//clean the values before insert in the queue
sample.position[0] = 0;
sample.velocity[0] = 0;
}
void Save(Data &&data)
{
std::unique_lock<std::mutex> lock(m_queueGuard);
m_queue.emplace();
m_queue.back().swap(data);
m_condition.notify_all();
}
transfer* reserveTransfer()
{
transfer* Transfer = new transfer;
if(ReadPixelBufferFree.empty())
{
glGenBuffers(1, &Transfer->Buffer);
glBindBuffer(GL_PIXEL_PACK_BUFFER, Transfer->Buffer);
glBufferData(GL_PIXEL_PACK_BUFFER, 640 * 480 * 4, NULL, GL_DYNAMIC_DRAW);
Transfer->Fence = nullptr;
}
else
{
Transfer = ReadPixelBufferFree.back();
if (Transfer->Fence)
{
glDeleteSync(Transfer->Fence);
Transfer->Fence = nullptr;
}
ReadPixelBufferFree.pop();
}
ReadPixelBufferLive.push(Transfer);
return Transfer;
}
void DispatchEvents() {
lock_guard guard(mutex_);
while (!g_dispatchQueue.empty()) {
DispatchQueueItem item = g_dispatchQueue.back();
g_dispatchQueue.pop();
item.e->Dispatch(item.params);
}
}
void Path::push(double x, double y) {
if(this->navpts.empty()) {
this->navpts.push(Vector2D(x, y));
this->current = &(navpts.back());
this->parent->moveTo(x, y);
return;
}
this->navpts.push(Vector2D(x, y));
}
void handle()
{
if (!sending && !to_emit.empty())
{
sending = true;
out = to_emit.back();
to_emit.pop();
out_pos = 0;
}
if (sending)
{
send();
return;
}
if (!cmd_ready)
{
read_cmd();
}
if (cmd_ready)
{
std::cerr << "\t\t\t\tcmd == " << cmd << std::endl;
int cmd_len = cmd_pos - 1;
if (strncmp(cmd, LIST, LIST_LEN) == 0)
{
std::cerr << "\tcmd == list" << std::endl;
sending = true;
out = list;
out_pos = 0;
cmd_len = LIST_LEN;
} else if (strncmp(cmd, SUB, SUB_LEN) == 0)
{
std::cerr << "\tcmd == sub" << std::endl;
int cnt = sub();
cmd_len = SUB_LEN + cnt;
} else if (strncmp(cmd, UNSUB, UNSUB_LEN) == 0)
{
std::cerr << "\tcmd == unsub" << std::endl;
int cnt = unsub();
cmd_len = UNSUB_LEN + cnt;
} else if (strncmp(cmd, EMIT, EMIT_LEN) == 0)
{
std::cerr << "\tcmd == emit" << std::endl;
int cnt = emit();
cmd_len = EMIT_LEN + cnt;
}
memmove(cmd, cmd + cmd_len + 1, cmd_pos - cmd_len - 1);
cmd_pos -= cmd_len + 1;
cmd_ready = false;
}
}
Path::Path(std::queue<Vertex*> pathNodes):
currentTarget(pathNodes.back()),
endNode(pathNodes.front())
{
currentTargetIndex = 0;
std::vector<Vertex*> nodes;
while(pathNodes.size() > 0)
{
nodes.push_back(pathNodes.front());
pathNodes.pop();
}
this->pathNodes = nodes;
}
TaskHandle enqueueTask(const Task::TaskFunction& function, const ProblemSpace& problemSpace) {
Task task(function, problemSpace);
std::unique_lock<std::mutex> lock(m_queueMutex);
m_tasks.emplace(std::move(task));
TaskHandle handle(m_tasks.back());
m_queueSemaphore.notify();
if (!m_running) {
for (unsigned int i = 0; i < std::thread::hardware_concurrency(); i++) {
m_threads.emplace_back(std::bind(&ThreadPool::threadFunction, this));
}
m_running = true;
}
return handle;
}
//
// blocking pop: return a TPtr if available
//
T pop()
{
boost::mutex::scoped_lock data_available_lock(data_available_mtx);
while(!data_available)
data_available_cond.wait(data_available_lock);
T p;
{
boost::mutex::scoped_lock lock(q_mutex);
assert(q_.size() > 0);
p = q_.back();
q_.pop();
data_available = q_.size() > 0;
}
return p;
};
void updateFps(unsigned long long& lastId, double& fps, unsigned int& fpsCounter,
std::queue<std::chrono::high_resolution_clock::time_point>& fpsQueue,
const unsigned long long id, const int numberGpus)
{
try
{
// If only 1 GPU -> update fps every frame.
// If > 1 GPU:
// We updated fps every (3*numberGpus) frames. This is due to the variability introduced by
// using > 1 GPU at the same time.
// However, we update every frame during the first few frames to have an initial estimator.
// In any of the previous cases, the fps value is estimated during the last several frames.
// In this way, a sudden fps drop will be quickly visually identified.
if (lastId != id)
{
lastId = id;
fpsQueue.emplace(std::chrono::high_resolution_clock::now());
bool updatePrintedFps = true;
if (fpsQueue.size() > 5)
{
const auto factor = (numberGpus > 1 ? 25u : 15u);
updatePrintedFps = (fpsCounter % factor == 0);
// updatePrintedFps = (numberGpus == 1 ? true : fpsCounter % (3*numberGpus) == 0);
fpsCounter++;
if (fpsQueue.size() > factor)
fpsQueue.pop();
}
if (updatePrintedFps)
{
const auto timeSec = (double)std::chrono::duration_cast<std::chrono::nanoseconds>(
fpsQueue.back()-fpsQueue.front()
).count() * 1e-9;
fps = (fpsQueue.size()-1) / (timeSec != 0. ? timeSec : 1.);
}
}
}
catch (const std::exception& e)
{
error(e.what(), __LINE__, __FUNCTION__, __FILE__);
}
}
virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
uavcan::CanIOFlags flags)
{
assert(this);
EXPECT_TRUE(writeable); // Shall never be called when not writeable
if (tx_failure)
{
return -1;
}
if (!writeable)
{
return 0;
}
tx.push(FrameWithTime(frame, tx_deadline));
tx.back().flags = flags;
if (flags & uavcan::CanIOFlagLoopback)
{
loopback.push(FrameWithTime(frame, iclock.getMonotonic()));
}
return 1;
}
T back()
{
std::unique_lock<std::mutex> lock(mutex_);
if (queue_.empty()) return NULL;
return queue_.back();
}
