<commit_before>#include "dynet/tensor-eigen.h"
#include "dynet/nodes-dropout.h"

#include "dynet/nodes-impl-macros.h"

using namespace std;

namespace dynet {

// ************* Dropout *************

#ifndef __CUDACC__

string Dropout::as_string(const vector<string>& arg_names) const {
  ostringstream s;
  s << "dropout(" << arg_names[0] << ",p=" << p << ')';
  return s.str();
}

Dim Dropout::dim_forward(const vector<Dim>& xs) const {
  DYNET_ARG_CHECK(xs.size() == 1, "Failed input count check in Dropout")
  return xs[0];
}

size_t Dropout::aux_storage_size() const {
  return dim.size() * sizeof(float);
}

#endif

template<class MyDevice>
void Dropout::forward_dev_impl(const MyDevice & dev, const vector<const Tensor*>& xs, Tensor& fx) const {
  Tensor m(fx.d, (float*)aux_mem, fx.device, DeviceMempool::FXS);
  TensorTools::randomize_bernoulli(m, (1.f-p), 1.f / (1.f-p));
  tvec(fx).device(*dev.edevice) = tvec(*xs[0]) * tvec(m);
}

template<class MyDevice>
void Dropout::backward_dev_impl(const MyDevice & dev,
                             const vector<const Tensor*>& xs,
                             const Tensor& fx,
                             const Tensor& dEdf,
                             unsigned i,
                             Tensor& dEdxi) const {
  Tensor m(fx.d, (float*)aux_mem, fx.device, DeviceMempool::FXS);
  tvec(dEdxi).device(*dev.edevice) += tvec(dEdf) * tvec(m);
}
DYNET_NODE_INST_DEV_IMPL(Dropout)

// ************* DropoutDim *************

#ifndef __CUDACC__

string DropoutDim::as_string(const vector<string>& arg_names) const {
  ostringstream s;
  s << "dropout_dim(" << arg_names[0] << ",p=" << p << ')';
  return s.str();
}

Dim DropoutDim::dim_forward(const vector<Dim>& xs) const {
  DYNET_ARG_CHECK(xs.size() == 1, "Failed input count check in DropoutDim")
  DYNET_ARG_CHECK(xs[0].nd < 4, "DropoutDim only supports tensor up to order 3 + batch dimension, got tensor of order"<<xs[0].nd)
  DYNET_ARG_CHECK(xs[0].nd > dimension, "In DropoutDim : tried to drop along dimension "<<dimension<<" on tensor of order"<<xs[0].nd)
  return xs[0];
}

size_t DropoutDim::aux_storage_size() const {
  return (dim.size() / dim[dimension]) * sizeof(float);
}

#endif

template<class MyDevice>
void DropoutDim::forward_dev_impl(const MyDevice & dev, const vector<const Tensor*>& xs, Tensor& fx) const {
  Dim mask_dim(dim);
  mask_dim.d[dimension]=1;
  Tensor m(mask_dim, (float*)aux_mem, fx.device, DeviceMempool::FXS);
  TensorTools::randomize_bernoulli(m, (1.f-p), 1.f / (1.f-p));
  Eigen::array<ptrdiff_t, 4> bcast = {1, 1, 1, 1}; bcast[dimension] = xs[0]->d[dimension];
  tb<3>(fx).device(*dev.edevice) = tb<3>(*xs[0]) * tb<3>(m).broadcast(bcast);
}

template<class MyDevice>
void DropoutDim::backward_dev_impl(const MyDevice & dev,
                             const vector<const Tensor*>& xs,
                             const Tensor& fx,
                             const Tensor& dEdf,
                             unsigned i,
                             Tensor& dEdxi) const {
  Dim mask_dim(dim);
  mask_dim.d[dimension]=1;
  Tensor m(mask_dim, (float*)aux_mem, fx.device, DeviceMempool::FXS);
  Eigen::array<ptrdiff_t, 4> bcast = {1, 1, 1, 1}; bcast[dimension] = dEdf.d[dimension];
  tb<3>(dEdxi).device(*dev.edevice) += tb<3>(dEdf) * tb<3>(m).broadcast(bcast);
}
DYNET_NODE_INST_DEV_IMPL(DropoutDim)

// ************* DropoutBatch *************

#ifndef __CUDACC__

string DropoutBatch::as_string(const vector<string>& arg_names) const {
  ostringstream s;
  s << "dropout_batch(" << arg_names[0] << ",p=" << p << ')';
  return s.str();
}

Dim DropoutBatch::dim_forward(const vector<Dim>& xs) const {
  DYNET_ARG_CHECK(xs.size() == 1, "Failed input count check in DropoutBatch")
  return xs[0];
}

size_t DropoutBatch::aux_storage_size() const {
  return dim.batch_elems() * sizeof(float);
}

#endif

template<class MyDevice>
void DropoutBatch::forward_dev_impl(const MyDevice & dev, const vector<const Tensor*>& xs, Tensor& fx) const {
  Dim mask_dim({1},xs[0]->d.batch_elems());
  Tensor m(mask_dim, (float*)aux_mem, fx.device, DeviceMempool::FXS);
  TensorTools::randomize_bernoulli(m, (1.f-p), 1.f / (1.f-p));
  Eigen::array<ptrdiff_t, 2> bcast = {xs[0]->d.batch_size(), 1};
  tbvec(fx).device(*dev.edevice) = tbvec(*xs[0]) * tbvec(m).broadcast(bcast);
}

template<class MyDevice>
void DropoutBatch::backward_dev_impl(const MyDevice & dev,
                             const vector<const Tensor*>& xs,
                             const Tensor& fx,
                             const Tensor& dEdf,
                             unsigned i,
                             Tensor& dEdxi) const {
  Dim mask_dim({1},xs[0]->d.batch_elems());
  Tensor m(mask_dim, (float*)aux_mem, fx.device, DeviceMempool::FXS);
  Eigen::array<ptrdiff_t, 2> bcast = {xs[0]->d.batch_size(), 1};
  tbvec(dEdxi).device(*dev.edevice) += tbvec(dEdf) * tbvec(m).broadcast(bcast);
}
DYNET_NODE_INST_DEV_IMPL(DropoutBatch)

// ************* BlockDropout *************

#ifndef __CUDACC__

string BlockDropout::as_string(const vector<string>& arg_names) const {
  ostringstream s;
  s << "block_dropout(" << arg_names[0] << ",dropout_probability=" << dropout_probability << ')';
  return s.str();
}

Dim BlockDropout::dim_forward(const vector<Dim>& xs) const {
  DYNET_ARG_CHECK(xs.size() == 1, "Failed input count check in BlockDropout")
  return xs[0];
}

size_t BlockDropout::aux_storage_size() const {
  // we just need to remember whether this entire block is turned on (1.0) or off (0.0)
  return 1 * sizeof(float);
}

#endif

template<class MyDevice>
void BlockDropout::forward_dev_impl(const MyDevice & dev, const vector<const Tensor*>& xs, Tensor& fx) const {
  bernoulli_distribution distribution(1.0 - dropout_probability);
  float block_multiplier = distribution(*rndeng)? 1.0 : 0.0;
  block_multiplier =
    dropout_probability == 1.0? 0.0 : block_multiplier / (1.0 - dropout_probability);
  if (dropout_probability > 1.0 || dropout_probability < 0.0)
    DYNET_INVALID_ARG("Dropout probability must be in the range [0, 1]");
  *(static_cast<float*>(aux_mem)) = block_multiplier;
  tvec(fx).device(*dev.edevice) = tvec(*xs[0]) * block_multiplier;
}

template<class MyDevice>
void BlockDropout::backward_dev_impl(const MyDevice & dev,
                             const vector<const Tensor*>& xs,
                             const Tensor& fx,
                             const Tensor& dEdf,
                             unsigned i,
                             Tensor& dEdxi) const {
  float block_multiplier = *(static_cast<float*>(aux_mem));
  tvec(dEdxi).device(*dev.edevice) += tvec(dEdf) * block_multiplier;
}
DYNET_NODE_INST_DEV_IMPL(BlockDropout)

}
<commit_msg>Fix block dropout on GPU<commit_after>#include "dynet/tensor-eigen.h"
#include "dynet/nodes-dropout.h"

#include "dynet/nodes-impl-macros.h"

using namespace std;

namespace dynet {

// ************* Dropout *************

#ifndef __CUDACC__

string Dropout::as_string(const vector<string>& arg_names) const {
  ostringstream s;
  s << "dropout(" << arg_names[0] << ",p=" << p << ')';
  return s.str();
}

Dim Dropout::dim_forward(const vector<Dim>& xs) const {
  DYNET_ARG_CHECK(xs.size() == 1, "Failed input count check in Dropout")
  return xs[0];
}

size_t Dropout::aux_storage_size() const {
  return dim.size() * sizeof(float);
}

#endif

template<class MyDevice>
void Dropout::forward_dev_impl(const MyDevice & dev, const vector<const Tensor*>& xs, Tensor& fx) const {
  Tensor m(fx.d, (float*)aux_mem, fx.device, DeviceMempool::FXS);
  TensorTools::randomize_bernoulli(m, (1.f-p), 1.f / (1.f-p));
  tvec(fx).device(*dev.edevice) = tvec(*xs[0]) * tvec(m);
}

template<class MyDevice>
void Dropout::backward_dev_impl(const MyDevice & dev,
                             const vector<const Tensor*>& xs,
                             const Tensor& fx,
                             const Tensor& dEdf,
                             unsigned i,
                             Tensor& dEdxi) const {
  Tensor m(fx.d, (float*)aux_mem, fx.device, DeviceMempool::FXS);
  tvec(dEdxi).device(*dev.edevice) += tvec(dEdf) * tvec(m);
}
DYNET_NODE_INST_DEV_IMPL(Dropout)

// ************* DropoutDim *************

#ifndef __CUDACC__

string DropoutDim::as_string(const vector<string>& arg_names) const {
  ostringstream s;
  s << "dropout_dim(" << arg_names[0] << ",p=" << p << ')';
  return s.str();
}

Dim DropoutDim::dim_forward(const vector<Dim>& xs) const {
  DYNET_ARG_CHECK(xs.size() == 1, "Failed input count check in DropoutDim")
  DYNET_ARG_CHECK(xs[0].nd < 4, "DropoutDim only supports tensor up to order 3 + batch dimension, got tensor of order"<<xs[0].nd)
  DYNET_ARG_CHECK(xs[0].nd > dimension, "In DropoutDim : tried to drop along dimension "<<dimension<<" on tensor of order"<<xs[0].nd)
  return xs[0];
}

size_t DropoutDim::aux_storage_size() const {
  return (dim.size() / dim[dimension]) * sizeof(float);
}

#endif

template<class MyDevice>
void DropoutDim::forward_dev_impl(const MyDevice & dev, const vector<const Tensor*>& xs, Tensor& fx) const {
  Dim mask_dim(dim);
  mask_dim.d[dimension]=1;
  Tensor m(mask_dim, (float*)aux_mem, fx.device, DeviceMempool::FXS);
  TensorTools::randomize_bernoulli(m, (1.f-p), 1.f / (1.f-p));
  Eigen::array<ptrdiff_t, 4> bcast = {1, 1, 1, 1}; bcast[dimension] = xs[0]->d[dimension];
  tb<3>(fx).device(*dev.edevice) = tb<3>(*xs[0]) * tb<3>(m).broadcast(bcast);
}

template<class MyDevice>
void DropoutDim::backward_dev_impl(const MyDevice & dev,
                             const vector<const Tensor*>& xs,
                             const Tensor& fx,
                             const Tensor& dEdf,
                             unsigned i,
                             Tensor& dEdxi) const {
  Dim mask_dim(dim);
  mask_dim.d[dimension]=1;
  Tensor m(mask_dim, (float*)aux_mem, fx.device, DeviceMempool::FXS);
  Eigen::array<ptrdiff_t, 4> bcast = {1, 1, 1, 1}; bcast[dimension] = dEdf.d[dimension];
  tb<3>(dEdxi).device(*dev.edevice) += tb<3>(dEdf) * tb<3>(m).broadcast(bcast);
}
DYNET_NODE_INST_DEV_IMPL(DropoutDim)

// ************* DropoutBatch *************

#ifndef __CUDACC__

string DropoutBatch::as_string(const vector<string>& arg_names) const {
  ostringstream s;
  s << "dropout_batch(" << arg_names[0] << ",p=" << p << ')';
  return s.str();
}

Dim DropoutBatch::dim_forward(const vector<Dim>& xs) const {
  DYNET_ARG_CHECK(xs.size() == 1, "Failed input count check in DropoutBatch")
  return xs[0];
}

size_t DropoutBatch::aux_storage_size() const {
  return dim.batch_elems() * sizeof(float);
}

#endif

template<class MyDevice>
void DropoutBatch::forward_dev_impl(const MyDevice & dev, const vector<const Tensor*>& xs, Tensor& fx) const {
  Dim mask_dim({1},xs[0]->d.batch_elems());
  Tensor m(mask_dim, (float*)aux_mem, fx.device, DeviceMempool::FXS);
  TensorTools::randomize_bernoulli(m, (1.f-p), 1.f / (1.f-p));
  Eigen::array<ptrdiff_t, 2> bcast = {xs[0]->d.batch_size(), 1};
  tbvec(fx).device(*dev.edevice) = tbvec(*xs[0]) * tbvec(m).broadcast(bcast);
}

template<class MyDevice>
void DropoutBatch::backward_dev_impl(const MyDevice & dev,
                             const vector<const Tensor*>& xs,
                             const Tensor& fx,
                             const Tensor& dEdf,
                             unsigned i,
                             Tensor& dEdxi) const {
  Dim mask_dim({1},xs[0]->d.batch_elems());
  Tensor m(mask_dim, (float*)aux_mem, fx.device, DeviceMempool::FXS);
  Eigen::array<ptrdiff_t, 2> bcast = {xs[0]->d.batch_size(), 1};
  tbvec(dEdxi).device(*dev.edevice) += tbvec(dEdf) * tbvec(m).broadcast(bcast);
}
DYNET_NODE_INST_DEV_IMPL(DropoutBatch)

// ************* BlockDropout *************

#ifndef __CUDACC__

string BlockDropout::as_string(const vector<string>& arg_names) const {
  ostringstream s;
  s << "block_dropout(" << arg_names[0] << ",dropout_probability=" << dropout_probability << ')';
  return s.str();
}

Dim BlockDropout::dim_forward(const vector<Dim>& xs) const {
  DYNET_ARG_CHECK(xs.size() == 1, "Failed input count check in BlockDropout")
  return xs[0];
}

size_t BlockDropout::aux_storage_size() const {
  // we just need to remember whether this entire block is turned on (1.0) or off (0.0)
  return 1 * sizeof(float);
}

#endif

template<class MyDevice>
void BlockDropout::forward_dev_impl(const MyDevice & dev, const vector<const Tensor*>& xs, Tensor& fx) const {
  float p = dropout_probability;
  Tensor m(Dim({1}), (float*)aux_mem, fx.device, DeviceMempool::FXS);
  if (p > 1.0 || p < 0.0) {
    DYNET_INVALID_ARG("Dropout probability must be in the range [0, 1]");
  } else {
    if (p == 1.0)
      TensorTools::zero(m);
    else
      TensorTools::randomize_bernoulli(m, (1.f-p), 1.f / (1.f-p));
  }
  float block_multiplier = as_scalar(m);
  tvec(fx).device(*dev.edevice) = tvec(*xs[0]) * block_multiplier;
}

template<class MyDevice>
void BlockDropout::backward_dev_impl(const MyDevice & dev,
                             const vector<const Tensor*>& xs,
                             const Tensor& fx,
                             const Tensor& dEdf,
                             unsigned i,
                             Tensor& dEdxi) const {
  Tensor m(Dim({1}), (float*)aux_mem, fx.device, DeviceMempool::FXS);
  float block_multiplier = as_scalar(m);
  tvec(dEdxi).device(*dev.edevice) += tvec(dEdf) * block_multiplier;
}
DYNET_NODE_INST_DEV_IMPL(BlockDropout)

}
<|endoftext|>


<commit_before>/* This file is part of Ingen.
 * Copyright (C) 2007 Dave Robillard <http://drobilla.net>
 *
 * Ingen is free software; you can redistribute it and/or modify it under the
 * terms of the GNU General Public License as published by the Free Software
 * Foundation; either version 2 of the License, or (at your option) any later
 * version.
 *
 * Ingen is distributed in the hope that it will be useful, but WITHOUT ANY
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
 */

#include <sys/mman.h>
#include <iostream>
#include "QueuedEventSource.hpp"
#include "QueuedEvent.hpp"
#include "PostProcessor.hpp"
#include "ThreadManager.hpp"
#include "ProcessContext.hpp"

using namespace std;

namespace Ingen {

QueuedEventSource::QueuedEventSource(size_t queued_size, size_t stamped_size)
	: _front(0)
	, _back(0)
	, _prepared_back(0)
	, _size(queued_size+1)
	, _blocking_semaphore(0)
	, _full_semaphore(0)
	, _stamped_queue(stamped_size)
{
	_events = (QueuedEvent**)calloc(_size, sizeof(QueuedEvent*));

	mlock(_events, _size * sizeof(QueuedEvent*));

	Thread::set_context(THREAD_PRE_PROCESS);
	assert(context() == THREAD_PRE_PROCESS);

	set_name("QueuedEventSource");
}

QueuedEventSource::~QueuedEventSource()
{
	Thread::stop();

	free(_events);
}

/** Push an unprepared event onto the queue.
 */
void
QueuedEventSource::push_queued(QueuedEvent* const ev)
{
	assert(!ev->is_prepared());

	unsigned back = _back.get();
	bool full = (((_front.get() - back + _size) % _size) == 1);
	while (full) {
		whip();
		cerr << "WARNING: Event queue full.  Waiting..." << endl;
		_full_semaphore.wait();
		back = _back.get();
		full = (((_front.get() - back + _size) % _size) == 1);
	}

	assert(_events[back] == NULL);
	_events[back] = ev;
	_back = (back + 1) % _size;
	whip();
}

/** Process all events for a cycle.
 *
 * Executed events will be pushed to @a dest.
 */
void
QueuedEventSource::process(PostProcessor& dest, ProcessContext& context)
{
	assert(ThreadManager::current_thread_id() == THREAD_PROCESS);

	Event* ev = NULL;

	/* Limit the maximum number of queued events to process per cycle.  This
	 * makes the process callback (more) realtime-safe by preventing being
	 * choked by events coming in faster than they can be processed.
	 * FIXME: test this and figure out a good value */
	const unsigned int MAX_QUEUED_EVENTS = context.nframes() / 100;

	unsigned int num_events_processed = 0;

	/* FIXME: Merge these next two loops into one */

	while ((ev = pop_earliest_queued_before(context.end()))) {
		ev->execute(context);
		dest.push(ev);
		if (++num_events_processed > MAX_QUEUED_EVENTS)
			break;
	}

	while ((ev = pop_earliest_stamped_before(context.end()))) {
		ev->execute(context);
		dest.push(ev);
		++num_events_processed;
	}

	if (num_events_processed > 0)
		while (_full_semaphore.has_waiter())
			_full_semaphore.post();
}

/** Pops the prepared event at the front of the prepare queue, if it exists.
 *
 * This method will only pop events that have been prepared, and are
 * stamped before the time passed.  In other words, it may return NULL
 * even if there are events pending in the queue.  The events returned are
 * actually QueuedEvents, but after this they are "normal" events and the
 * engine deals with them just like a realtime in-band event.  The engine will
 * not use the timestamps of the returned events in any way, since it is free
 * to execute these non-time-stamped events whenever it wants (at whatever rate
 * it wants).
 */
Event*
QueuedEventSource::pop_earliest_queued_before(const SampleCount time)
{
	assert(ThreadManager::current_thread_id() == THREAD_PROCESS);

	const unsigned front = _front.get();
	QueuedEvent* const front_event = _events[front];

	// Pop
	if (front_event && front_event->is_prepared() && front_event->time() < time) {
		_events[front] = NULL;
		_front = (front + 1) % _size;
		return front_event;
	} else {
		return NULL;
	}
}

// Private //

/** Pre-process a single event */
void
QueuedEventSource::_whipped()
{
	const unsigned prepared_back = _prepared_back.get();
	QueuedEvent* const ev = _events[prepared_back];
	if (!ev)
		return;

	assert(!ev->is_prepared());
	ev->pre_process();
	assert(ev->is_prepared());

	_prepared_back = (prepared_back + 1) % _size;

	// If event was blocking, wait for event to being run through the
	// process thread before preparing the next event
	if (ev->is_blocking())
		_blocking_semaphore.wait();
}

} // namespace Ingen

<commit_msg>Better handling of large patch loading.<commit_after>/* This file is part of Ingen.
 * Copyright (C) 2007 Dave Robillard <http://drobilla.net>
 *
 * Ingen is free software; you can redistribute it and/or modify it under the
 * terms of the GNU General Public License as published by the Free Software
 * Foundation; either version 2 of the License, or (at your option) any later
 * version.
 *
 * Ingen is distributed in the hope that it will be useful, but WITHOUT ANY
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
 */

#include <sys/mman.h>
#include <iostream>
#include "QueuedEventSource.hpp"
#include "QueuedEvent.hpp"
#include "PostProcessor.hpp"
#include "ThreadManager.hpp"
#include "ProcessContext.hpp"

using namespace std;

namespace Ingen {

QueuedEventSource::QueuedEventSource(size_t queued_size, size_t stamped_size)
	: _front(0)
	, _back(0)
	, _prepared_back(0)
	, _size(queued_size+1)
	, _blocking_semaphore(0)
	, _full_semaphore(0)
	, _stamped_queue(stamped_size)
{
	_events = (QueuedEvent**)calloc(_size, sizeof(QueuedEvent*));

	mlock(_events, _size * sizeof(QueuedEvent*));

	Thread::set_context(THREAD_PRE_PROCESS);
	assert(context() == THREAD_PRE_PROCESS);

	set_name("QueuedEventSource");
}

QueuedEventSource::~QueuedEventSource()
{
	Thread::stop();

	free(_events);
}

/** Push an unprepared event onto the queue.
 */
void
QueuedEventSource::push_queued(QueuedEvent* const ev)
{
	assert(!ev->is_prepared());

	unsigned back = _back.get();
	bool full = (((_front.get() - back + _size) % _size) == 1);
	while (full) {
		whip();
		cerr << "WARNING: Event queue full.  Waiting..." << endl;
		_full_semaphore.wait();
		back = _back.get();
		full = (((_front.get() - back + _size) % _size) == 1);
	}

	assert(_events[back] == NULL);
	_events[back] = ev;
	_back = (back + 1) % _size;
	whip();
}

/** Process all events for a cycle.
 *
 * Executed events will be pushed to @a dest.
 */
void
QueuedEventSource::process(PostProcessor& dest, ProcessContext& context)
{
	assert(ThreadManager::current_thread_id() == THREAD_PROCESS);

	Event* ev = NULL;

	/* Limit the maximum number of queued events to process per cycle.  This
	 * makes the process callback (more) realtime-safe by preventing being
	 * choked by events coming in faster than they can be processed.
	 * FIXME: test this and figure out a good value */
	const unsigned int MAX_QUEUED_EVENTS = context.nframes() / 64;

	unsigned int num_events_processed = 0;

	/* FIXME: Merge these next two loops into one */

	while ((ev = pop_earliest_queued_before(context.end()))) {
		ev->execute(context);
		dest.push(ev);
		if (++num_events_processed > MAX_QUEUED_EVENTS)
			break;
	}

	while ((ev = pop_earliest_stamped_before(context.end()))) {
		ev->execute(context);
		dest.push(ev);
		++num_events_processed;
	}

	if (_full_semaphore.has_waiter()) {
		const bool full = (((_front.get() - _back.get() + _size) % _size) == 1);
		if (!full)
			_full_semaphore.post();
	}
}

/** Pops the prepared event at the front of the prepare queue, if it exists.
 *
 * This method will only pop events that have been prepared, and are
 * stamped before the time passed.  In other words, it may return NULL
 * even if there are events pending in the queue.  The events returned are
 * actually QueuedEvents, but after this they are "normal" events and the
 * engine deals with them just like a realtime in-band event.  The engine will
 * not use the timestamps of the returned events in any way, since it is free
 * to execute these non-time-stamped events whenever it wants (at whatever rate
 * it wants).
 */
Event*
QueuedEventSource::pop_earliest_queued_before(const SampleCount time)
{
	assert(ThreadManager::current_thread_id() == THREAD_PROCESS);

	const unsigned front = _front.get();
	QueuedEvent* const front_event = _events[front];

	// Pop
	if (front_event && front_event->is_prepared() && front_event->time() < time) {
		_events[front] = NULL;
		_front = (front + 1) % _size;
		return front_event;
	} else {
		return NULL;
	}
}

// Private //

/** Pre-process a single event */
void
QueuedEventSource::_whipped()
{
	const unsigned prepared_back = _prepared_back.get();
	QueuedEvent* const ev = _events[prepared_back];
	if (!ev)
		return;

	assert(!ev->is_prepared());
	ev->pre_process();
	assert(ev->is_prepared());

	_prepared_back = (prepared_back + 1) % _size;

	// If event was blocking, wait for event to being run through the
	// process thread before preparing the next event
	if (ev->is_blocking())
		_blocking_semaphore.wait();
}

} // namespace Ingen

<|endoftext|>


<commit_before>#ifndef ENTT_ENTITY_RUNTIME_VIEW_HPP
#define ENTT_ENTITY_RUNTIME_VIEW_HPP

#include <iterator>
#include <cassert>
#include <vector>
#include <utility>
#include <algorithm>
#include <type_traits>
#include "../config/config.h"
#include "sparse_set.hpp"
#include "entity.hpp"
#include "fwd.hpp"

namespace entt {

/**
 * @brief Runtime view.
 *
 * Runtime views iterate over those entities that have at least all the given
 * components in their bags. During initialization, a runtime view looks at the
 * number of entities available for each component and picks up a reference to
 * the smallest set of candidate entities in order to get a performance boost
 * when iterate.

 * Order of elements during iterations are highly dependent on the order of the
 * underlying data structures. See sparse_set and its specializations for more
 * details.
 *
 * @b Important
 *
 * Iterators aren't invalidated if:
 *
 * * New instances of the given components are created and assigned to entities.
 * * The entity currently pointed is modified (as an example, if one of the
 *   given components is removed from the entity to which the iterator points).
 * * The entity currently pointed is destroyed.
 *
 * In all the other cases, modifying the pools of the given components in any
 * way invalidates all the iterators and using them results in undefined
 * behavior.
 *
 * @note
 * Views share references to the underlying data structures of the registry that
 * generated them. Therefore any change to the entities and to the components
 * made by means of the registry are immediately reflected by the views, unless
 * a pool was missing when the view was built (in this case, the view won't
 * have a valid reference and won't be updated accordingly).
 *
 * @warning
 * Lifetime of a view must overcome the one of the registry that generated it.
 * In any other case, attempting to use a view results in undefined behavior.
 *
 * @tparam Entity A valid entity type (see entt_traits for more details).
 */
template<typename Entity>
class basic_runtime_view {
    /*! @brief A registry is allowed to create views. */
    friend class basic_registry<Entity>;

    using underlying_iterator_type = typename sparse_set<Entity>::iterator_type;
    using traits_type = entt_traits<std::underlying_type_t<Entity>>;

    class iterator {
        friend class basic_runtime_view<Entity>;

        iterator(underlying_iterator_type first, underlying_iterator_type last, const sparse_set<Entity> * const *others, const sparse_set<Entity> * const *length) ENTT_NOEXCEPT
            : begin{first},
              end{last},
              from{others},
              to{length}
        {
            if(begin != end && !valid()) {
                ++(*this);
            }
        }

        bool valid() const ENTT_NOEXCEPT {
            const auto entt = *begin;
            const auto sz = size_type(to_integer(entt) & traits_type::entity_mask);

            return std::all_of(from, to, [entt](const auto *view) {
                return view->has(entt);
            });
        }

    public:
        using difference_type = typename underlying_iterator_type::difference_type;
        using value_type = typename underlying_iterator_type::value_type;
        using pointer = typename underlying_iterator_type::pointer;
        using reference = typename underlying_iterator_type::reference;
        using iterator_category = std::forward_iterator_tag;

        iterator() ENTT_NOEXCEPT = default;

        iterator & operator++() ENTT_NOEXCEPT {
            return (++begin != end && !valid()) ? ++(*this) : *this;
        }

        iterator operator++(int) ENTT_NOEXCEPT {
            iterator orig = *this;
            return ++(*this), orig;
        }

        bool operator==(const iterator &other) const ENTT_NOEXCEPT {
            return other.begin == begin;
        }

        bool operator!=(const iterator &other) const ENTT_NOEXCEPT {
            return !(*this == other);
        }

        pointer operator->() const ENTT_NOEXCEPT {
            return begin.operator->();
        }

        reference operator*() const ENTT_NOEXCEPT {
            return *operator->();
        }

    private:
        underlying_iterator_type begin;
        underlying_iterator_type end;
        const sparse_set<Entity> * const *from;
        const sparse_set<Entity> * const *to;
    };

    basic_runtime_view(std::vector<const sparse_set<Entity> *> others) ENTT_NOEXCEPT
        : pools{std::move(others)}
    {
        const auto it = std::min_element(pools.begin(), pools.end(), [](const auto *lhs, const auto *rhs) {
            return (!lhs && rhs) || (lhs && rhs && lhs->size() < rhs->size());
        });

        // brings the best candidate (if any) on front of the vector
        std::rotate(pools.begin(), it, pools.end());
    }

    bool valid() const ENTT_NOEXCEPT {
        return !pools.empty() && pools.front();
    }

public:
    /*! @brief Underlying entity identifier. */
    using entity_type = typename sparse_set<Entity>::entity_type;
    /*! @brief Unsigned integer type. */
    using size_type = typename sparse_set<Entity>::size_type;
    /*! @brief Input iterator type. */
    using iterator_type = iterator;

    /**
     * @brief Estimates the number of entities that have the given components.
     * @return Estimated number of entities that have the given components.
     */
    size_type size() const ENTT_NOEXCEPT {
        return valid() ? pools.front()->size() : size_type{};
    }

    /**
     * @brief Checks if the view is definitely empty.
     * @return True if the view is definitely empty, false otherwise.
     */
    bool empty() const ENTT_NOEXCEPT {
        return !valid() || pools.front()->empty();
    }

    /**
     * @brief Returns an iterator to the first entity that has the given
     * components.
     *
     * The returned iterator points to the first entity that has the given
     * components. If the view is empty, the returned iterator will be equal to
     * `end()`.
     *
     * @note
     * Input iterators stay true to the order imposed to the underlying data
     * structures.
     *
     * @return An iterator to the first entity that has the given components.
     */
    iterator_type begin() const ENTT_NOEXCEPT {
        iterator_type it{};

        if(valid()) {
            const auto &pool = *pools.front();
            const auto * const *data = pools.data();
            it = { pool.begin(), pool.end(), data + 1, data + pools.size() };
        }

        return it;
    }

    /**
     * @brief Returns an iterator that is past the last entity that has the
     * given components.
     *
     * The returned iterator points to the entity following the last entity that
     * has the given components. Attempting to dereference the returned iterator
     * results in undefined behavior.
     *
     * @note
     * Input iterators stay true to the order imposed to the underlying data
     * structures.
     *
     * @return An iterator to the entity following the last entity that has the
     * given components.
     */
    iterator_type end() const ENTT_NOEXCEPT {
        iterator_type it{};

        if(valid()) {
            const auto &pool = *pools.front();
            it = { pool.end(), pool.end(), nullptr, nullptr };
        }

        return it;
    }

    /**
     * @brief Checks if a view contains an entity.
     * @param entt A valid entity identifier.
     * @return True if the view contains the given entity, false otherwise.
     */
    bool contains(const entity_type entt) const ENTT_NOEXCEPT {
        return valid() && std::all_of(pools.cbegin(), pools.cend(), [entt](const auto *view) {
            return view->find(entt) != view->end();
        });
    }

    /**
     * @brief Iterates entities and applies the given function object to them.
     *
     * The function object is invoked for each entity. It is provided only with
     * the entity itself. To get the components, users can use the registry with
     * which the view was built.

     * The signature of the function should be equivalent to the following:
     *
     * @code{.cpp}
     * void(const entity_type);
     * @endcode
     *
     * @tparam Func Type of the function object to invoke.
     * @param func A valid function object.
     */
    template<typename Func>
    void each(Func func) const {
        std::for_each(begin(), end(), func);
    }

private:
    std::vector<const sparse_set<Entity> *> pools;
};

}

#endif // ENTT_ENTITY_RUNTIME_VIEW_HPP
<commit_msg>cleanup<commit_after>#ifndef ENTT_ENTITY_RUNTIME_VIEW_HPP
#define ENTT_ENTITY_RUNTIME_VIEW_HPP

#include <iterator>
#include <cassert>
#include <vector>
#include <utility>
#include <algorithm>
#include <type_traits>
#include "../config/config.h"
#include "sparse_set.hpp"
#include "entity.hpp"
#include "fwd.hpp"

namespace entt {

/**
 * @brief Runtime view.
 *
 * Runtime views iterate over those entities that have at least all the given
 * components in their bags. During initialization, a runtime view looks at the
 * number of entities available for each component and picks up a reference to
 * the smallest set of candidate entities in order to get a performance boost
 * when iterate.

 * Order of elements during iterations are highly dependent on the order of the
 * underlying data structures. See sparse_set and its specializations for more
 * details.
 *
 * @b Important
 *
 * Iterators aren't invalidated if:
 *
 * * New instances of the given components are created and assigned to entities.
 * * The entity currently pointed is modified (as an example, if one of the
 *   given components is removed from the entity to which the iterator points).
 * * The entity currently pointed is destroyed.
 *
 * In all the other cases, modifying the pools of the given components in any
 * way invalidates all the iterators and using them results in undefined
 * behavior.
 *
 * @note
 * Views share references to the underlying data structures of the registry that
 * generated them. Therefore any change to the entities and to the components
 * made by means of the registry are immediately reflected by the views, unless
 * a pool was missing when the view was built (in this case, the view won't
 * have a valid reference and won't be updated accordingly).
 *
 * @warning
 * Lifetime of a view must overcome the one of the registry that generated it.
 * In any other case, attempting to use a view results in undefined behavior.
 *
 * @tparam Entity A valid entity type (see entt_traits for more details).
 */
template<typename Entity>
class basic_runtime_view {
    /*! @brief A registry is allowed to create views. */
    friend class basic_registry<Entity>;

    using underlying_iterator_type = typename sparse_set<Entity>::iterator_type;
    using traits_type = entt_traits<std::underlying_type_t<Entity>>;

    class iterator {
        friend class basic_runtime_view<Entity>;

        iterator(underlying_iterator_type first, underlying_iterator_type last, const sparse_set<Entity> * const *others, const sparse_set<Entity> * const *length) ENTT_NOEXCEPT
            : begin{first},
              end{last},
              from{others},
              to{length}
        {
            if(begin != end && !valid()) {
                ++(*this);
            }
        }

        bool valid() const ENTT_NOEXCEPT {
            return std::all_of(from, to, [entt = *begin](const auto *view) {
                return view->has(entt);
            });
        }

    public:
        using difference_type = typename underlying_iterator_type::difference_type;
        using value_type = typename underlying_iterator_type::value_type;
        using pointer = typename underlying_iterator_type::pointer;
        using reference = typename underlying_iterator_type::reference;
        using iterator_category = std::forward_iterator_tag;

        iterator() ENTT_NOEXCEPT = default;

        iterator & operator++() ENTT_NOEXCEPT {
            return (++begin != end && !valid()) ? ++(*this) : *this;
        }

        iterator operator++(int) ENTT_NOEXCEPT {
            iterator orig = *this;
            return ++(*this), orig;
        }

        bool operator==(const iterator &other) const ENTT_NOEXCEPT {
            return other.begin == begin;
        }

        bool operator!=(const iterator &other) const ENTT_NOEXCEPT {
            return !(*this == other);
        }

        pointer operator->() const ENTT_NOEXCEPT {
            return begin.operator->();
        }

        reference operator*() const ENTT_NOEXCEPT {
            return *operator->();
        }

    private:
        underlying_iterator_type begin;
        underlying_iterator_type end;
        const sparse_set<Entity> * const *from;
        const sparse_set<Entity> * const *to;
    };

    basic_runtime_view(std::vector<const sparse_set<Entity> *> others) ENTT_NOEXCEPT
        : pools{std::move(others)}
    {
        const auto it = std::min_element(pools.begin(), pools.end(), [](const auto *lhs, const auto *rhs) {
            return (!lhs && rhs) || (lhs && rhs && lhs->size() < rhs->size());
        });

        // brings the best candidate (if any) on front of the vector
        std::rotate(pools.begin(), it, pools.end());
    }

    bool valid() const ENTT_NOEXCEPT {
        return !pools.empty() && pools.front();
    }

public:
    /*! @brief Underlying entity identifier. */
    using entity_type = typename sparse_set<Entity>::entity_type;
    /*! @brief Unsigned integer type. */
    using size_type = typename sparse_set<Entity>::size_type;
    /*! @brief Input iterator type. */
    using iterator_type = iterator;

    /**
     * @brief Estimates the number of entities that have the given components.
     * @return Estimated number of entities that have the given components.
     */
    size_type size() const ENTT_NOEXCEPT {
        return valid() ? pools.front()->size() : size_type{};
    }

    /**
     * @brief Checks if the view is definitely empty.
     * @return True if the view is definitely empty, false otherwise.
     */
    bool empty() const ENTT_NOEXCEPT {
        return !valid() || pools.front()->empty();
    }

    /**
     * @brief Returns an iterator to the first entity that has the given
     * components.
     *
     * The returned iterator points to the first entity that has the given
     * components. If the view is empty, the returned iterator will be equal to
     * `end()`.
     *
     * @note
     * Input iterators stay true to the order imposed to the underlying data
     * structures.
     *
     * @return An iterator to the first entity that has the given components.
     */
    iterator_type begin() const ENTT_NOEXCEPT {
        iterator_type it{};

        if(valid()) {
            const auto &pool = *pools.front();
            const auto * const *data = pools.data();
            it = { pool.begin(), pool.end(), data + 1, data + pools.size() };
        }

        return it;
    }

    /**
     * @brief Returns an iterator that is past the last entity that has the
     * given components.
     *
     * The returned iterator points to the entity following the last entity that
     * has the given components. Attempting to dereference the returned iterator
     * results in undefined behavior.
     *
     * @note
     * Input iterators stay true to the order imposed to the underlying data
     * structures.
     *
     * @return An iterator to the entity following the last entity that has the
     * given components.
     */
    iterator_type end() const ENTT_NOEXCEPT {
        iterator_type it{};

        if(valid()) {
            const auto &pool = *pools.front();
            it = { pool.end(), pool.end(), nullptr, nullptr };
        }

        return it;
    }

    /**
     * @brief Checks if a view contains an entity.
     * @param entt A valid entity identifier.
     * @return True if the view contains the given entity, false otherwise.
     */
    bool contains(const entity_type entt) const ENTT_NOEXCEPT {
        return valid() && std::all_of(pools.cbegin(), pools.cend(), [entt](const auto *view) {
            return view->find(entt) != view->end();
        });
    }

    /**
     * @brief Iterates entities and applies the given function object to them.
     *
     * The function object is invoked for each entity. It is provided only with
     * the entity itself. To get the components, users can use the registry with
     * which the view was built.

     * The signature of the function should be equivalent to the following:
     *
     * @code{.cpp}
     * void(const entity_type);
     * @endcode
     *
     * @tparam Func Type of the function object to invoke.
     * @param func A valid function object.
     */
    template<typename Func>
    void each(Func func) const {
        std::for_each(begin(), end(), func);
    }

private:
    std::vector<const sparse_set<Entity> *> pools;
};

}

#endif // ENTT_ENTITY_RUNTIME_VIEW_HPP
<|endoftext|>


<commit_before>#ifndef ENTT_ENTITY_RUNTIME_VIEW_HPP
#define ENTT_ENTITY_RUNTIME_VIEW_HPP

#include <iterator>
#include <vector>
#include <utility>
#include <algorithm>
#include <type_traits>
#include "../config/config.h"
#include "sparse_set.hpp"
#include "fwd.hpp"

namespace entt {

/**
 * @brief Runtime view.
 *
 * Runtime views iterate over those entities that have at least all the given
 * components in their bags. During initialization, a runtime view looks at the
 * number of entities available for each component and picks up a reference to
 * the smallest set of candidate entities in order to get a performance boost
 * when iterate.

 * Order of elements during iterations are highly dependent on the order of the
 * underlying data structures. See sparse_set and its specializations for more
 * details.
 *
 * @b Important
 *
 * Iterators aren't invalidated if:
 *
 * * New instances of the given components are created and assigned to entities.
 * * The entity currently pointed is modified (as an example, if one of the
 *   given components is removed from the entity to which the iterator points).
 * * The entity currently pointed is destroyed.
 *
 * In all the other cases, modifying the pools of the given components in any
 * way invalidates all the iterators and using them results in undefined
 * behavior.
 *
 * @note
 * Views share references to the underlying data structures of the registry that
 * generated them. Therefore any change to the entities and to the components
 * made by means of the registry are immediately reflected by the views, unless
 * a pool was missing when the view was built (in this case, the view won't
 * have a valid reference and won't be updated accordingly).
 *
 * @warning
 * Lifetime of a view must not overcome that of the registry that generated it.
 * In any other case, attempting to use a view results in undefined behavior.
 *
 * @tparam Entity A valid entity type (see entt_traits for more details).
 */
template<typename Entity>
class basic_runtime_view {
    /*! @brief A registry is allowed to create views. */
    friend class basic_registry<Entity>;

    using underlying_iterator = typename sparse_set<Entity>::iterator;

    class view_iterator final {
        friend class basic_runtime_view<Entity>;

        view_iterator(const std::vector<const sparse_set<Entity> *> &cpools, const std::vector<const sparse_set<Entity> *> &ignore, underlying_iterator curr) ENTT_NOEXCEPT
            : pools{&cpools},
              filter{&ignore},
              it{curr}
        {
            if(it != (*pools)[0]->end() && !valid()) {
                ++(*this);
            }
        }

        [[nodiscard]] bool valid() const {
            return std::all_of(pools->begin()++, pools->end(), [entt = *it](const auto *curr) { return curr->contains(entt); })
                    && std::none_of(filter->cbegin(), filter->cend(), [entt = *it](const auto *curr) { return curr && curr->contains(entt); });
        }

    public:
        using difference_type = typename underlying_iterator::difference_type;
        using value_type = typename underlying_iterator::value_type;
        using pointer = typename underlying_iterator::pointer;
        using reference = typename underlying_iterator::reference;
        using iterator_category = std::bidirectional_iterator_tag;

        view_iterator() ENTT_NOEXCEPT = default;

        view_iterator & operator++() {
            while(++it != (*pools)[0]->end() && !valid());
            return *this;
        }

        view_iterator operator++(int) {
            view_iterator orig = *this;
            return ++(*this), orig;
        }

        view_iterator & operator--() ENTT_NOEXCEPT {
            while(--it != (*pools)[0]->begin() && !valid());
            return *this;
        }

        view_iterator operator--(int) ENTT_NOEXCEPT {
            view_iterator orig = *this;
            return operator--(), orig;
        }

        [[nodiscard]] bool operator==(const view_iterator &other) const ENTT_NOEXCEPT {
            return other.it == it;
        }

        [[nodiscard]] bool operator!=(const view_iterator &other) const ENTT_NOEXCEPT {
            return !(*this == other);
        }

        [[nodiscard]] pointer operator->() const {
            return it.operator->();
        }

        [[nodiscard]] reference operator*() const {
            return *operator->();
        }

    private:
        const std::vector<const sparse_set<Entity> *> *pools;
        const std::vector<const sparse_set<Entity> *> *filter;
        underlying_iterator it;
    };

    basic_runtime_view(std::vector<const sparse_set<Entity> *> cpools, std::vector<const sparse_set<Entity> *> exclude) ENTT_NOEXCEPT
        : pools{std::move(cpools)},
          filter{std::move(exclude)}
    {
        const auto it = std::min_element(pools.begin(), pools.end(), [](const auto *lhs, const auto *rhs) {
            return (!lhs && rhs) || (lhs && rhs && lhs->size() < rhs->size());
        });

        // brings the best candidate (if any) on front of the vector
        std::rotate(pools.begin(), it, pools.end());
    }

    [[nodiscard]] bool valid() const {
        return !pools.empty() && pools.front();
    }

public:
    /*! @brief Underlying entity identifier. */
    using entity_type = Entity;
    /*! @brief Unsigned integer type. */
    using size_type = std::size_t;
    /*! @brief Bidirectional iterator type. */
    using iterator = view_iterator;

    /**
     * @brief Estimates the number of entities that have the given components.
     * @return Estimated number of entities that have the given components.
     */
    [[nodiscard]] size_type size() const {
        return valid() ? pools.front()->size() : size_type{};
    }

    /**
     * @brief Checks if the view is definitely empty.
     * @return True if the view is definitely empty, false otherwise.
     */
    [[nodiscard]] bool empty() const {
        return !valid() || pools.front()->empty();
    }

    /**
     * @brief Returns an iterator to the first entity that has the given
     * components.
     *
     * The returned iterator points to the first entity that has the given
     * components. If the view is empty, the returned iterator will be equal to
     * `end()`.
     *
     * @note
     * Iterators stay true to the order imposed to the underlying data
     * structures.
     *
     * @return An iterator to the first entity that has the given components.
     */
    [[nodiscard]] iterator begin() const {
        return valid() ? iterator{pools, filter, pools[0]->begin()} : iterator{};
    }

    /**
     * @brief Returns an iterator that is past the last entity that has the
     * given components.
     *
     * The returned iterator points to the entity following the last entity that
     * has the given components. Attempting to dereference the returned iterator
     * results in undefined behavior.
     *
     * @note
     * Iterators stay true to the order imposed to the underlying data
     * structures.
     *
     * @return An iterator to the entity following the last entity that has the
     * given components.
     */
    [[nodiscard]] iterator end() const {
        return valid() ? iterator{pools, filter, pools[0]->end()} : iterator{};
    }

    /**
     * @brief Checks if a view contains an entity.
     * @param entt A valid entity identifier.
     * @return True if the view contains the given entity, false otherwise.
     */
    [[nodiscard]] bool contains(const entity_type entt) const {
        return valid() && std::all_of(pools.cbegin(), pools.cend(), [entt](const auto *curr) { return curr->contains(entt); })
                && std::none_of(filter.cbegin(), filter.cend(), [entt](const auto *curr) { return curr && curr->contains(entt); });
    }

    /**
     * @brief Iterates entities and applies the given function object to them.
     *
     * The function object is invoked for each entity. It is provided only with
     * the entity itself. To get the components, users can use the registry with
     * which the view was built.

     * The signature of the function should be equivalent to the following:
     *
     * @code{.cpp}
     * void(const entity_type);
     * @endcode
     *
     * @tparam Func Type of the function object to invoke.
     * @param func A valid function object.
     */
    template<typename Func>
    void each(Func func) const {
        for(const auto entity: *this) {
            func(entity);
        }
    }

private:
    std::vector<const sparse_set<Entity> *> pools;
    std::vector<const sparse_set<Entity> *> filter;
};

}

#endif
<commit_msg>runtime view: suppress warnings for shadow variables<commit_after>#ifndef ENTT_ENTITY_RUNTIME_VIEW_HPP
#define ENTT_ENTITY_RUNTIME_VIEW_HPP

#include <iterator>
#include <vector>
#include <utility>
#include <algorithm>
#include <type_traits>
#include "../config/config.h"
#include "sparse_set.hpp"
#include "fwd.hpp"

namespace entt {

/**
 * @brief Runtime view.
 *
 * Runtime views iterate over those entities that have at least all the given
 * components in their bags. During initialization, a runtime view looks at the
 * number of entities available for each component and picks up a reference to
 * the smallest set of candidate entities in order to get a performance boost
 * when iterate.

 * Order of elements during iterations are highly dependent on the order of the
 * underlying data structures. See sparse_set and its specializations for more
 * details.
 *
 * @b Important
 *
 * Iterators aren't invalidated if:
 *
 * * New instances of the given components are created and assigned to entities.
 * * The entity currently pointed is modified (as an example, if one of the
 *   given components is removed from the entity to which the iterator points).
 * * The entity currently pointed is destroyed.
 *
 * In all the other cases, modifying the pools of the given components in any
 * way invalidates all the iterators and using them results in undefined
 * behavior.
 *
 * @note
 * Views share references to the underlying data structures of the registry that
 * generated them. Therefore any change to the entities and to the components
 * made by means of the registry are immediately reflected by the views, unless
 * a pool was missing when the view was built (in this case, the view won't
 * have a valid reference and won't be updated accordingly).
 *
 * @warning
 * Lifetime of a view must not overcome that of the registry that generated it.
 * In any other case, attempting to use a view results in undefined behavior.
 *
 * @tparam Entity A valid entity type (see entt_traits for more details).
 */
template<typename Entity>
class basic_runtime_view {
    /*! @brief A registry is allowed to create views. */
    friend class basic_registry<Entity>;

    using underlying_iterator = typename sparse_set<Entity>::iterator;

    class view_iterator final {
        friend class basic_runtime_view<Entity>;

        view_iterator(const std::vector<const sparse_set<Entity> *> &cpools, const std::vector<const sparse_set<Entity> *> &ignore, underlying_iterator curr) ENTT_NOEXCEPT
            : pools{&cpools},
              filter{&ignore},
              it{curr}
        {
            if(it != (*pools)[0]->end() && !valid()) {
                ++(*this);
            }
        }

        [[nodiscard]] bool valid() const {
            return std::all_of(pools->begin()++, pools->end(), [entt = *it](const auto *curr) { return curr->contains(entt); })
                    && std::none_of(filter->cbegin(), filter->cend(), [entt = *it](const auto *curr) { return curr && curr->contains(entt); });
        }

    public:
        using difference_type = typename underlying_iterator::difference_type;
        using value_type = typename underlying_iterator::value_type;
        using pointer = typename underlying_iterator::pointer;
        using reference = typename underlying_iterator::reference;
        using iterator_category = std::bidirectional_iterator_tag;

        view_iterator() ENTT_NOEXCEPT = default;

        view_iterator & operator++() {
            while(++it != (*pools)[0]->end() && !valid());
            return *this;
        }

        view_iterator operator++(int) {
            view_iterator orig = *this;
            return ++(*this), orig;
        }

        view_iterator & operator--() ENTT_NOEXCEPT {
            while(--it != (*pools)[0]->begin() && !valid());
            return *this;
        }

        view_iterator operator--(int) ENTT_NOEXCEPT {
            view_iterator orig = *this;
            return operator--(), orig;
        }

        [[nodiscard]] bool operator==(const view_iterator &other) const ENTT_NOEXCEPT {
            return other.it == it;
        }

        [[nodiscard]] bool operator!=(const view_iterator &other) const ENTT_NOEXCEPT {
            return !(*this == other);
        }

        [[nodiscard]] pointer operator->() const {
            return it.operator->();
        }

        [[nodiscard]] reference operator*() const {
            return *operator->();
        }

    private:
        const std::vector<const sparse_set<Entity> *> *pools;
        const std::vector<const sparse_set<Entity> *> *filter;
        underlying_iterator it;
    };

    basic_runtime_view(std::vector<const sparse_set<Entity> *> cpools, std::vector<const sparse_set<Entity> *> epools) ENTT_NOEXCEPT
        : pools{std::move(cpools)},
          filter{std::move(epools)}
    {
        const auto it = std::min_element(pools.begin(), pools.end(), [](const auto *lhs, const auto *rhs) {
            return (!lhs && rhs) || (lhs && rhs && lhs->size() < rhs->size());
        });

        // brings the best candidate (if any) on front of the vector
        std::rotate(pools.begin(), it, pools.end());
    }

    [[nodiscard]] bool valid() const {
        return !pools.empty() && pools.front();
    }

public:
    /*! @brief Underlying entity identifier. */
    using entity_type = Entity;
    /*! @brief Unsigned integer type. */
    using size_type = std::size_t;
    /*! @brief Bidirectional iterator type. */
    using iterator = view_iterator;

    /**
     * @brief Estimates the number of entities that have the given components.
     * @return Estimated number of entities that have the given components.
     */
    [[nodiscard]] size_type size() const {
        return valid() ? pools.front()->size() : size_type{};
    }

    /**
     * @brief Checks if the view is definitely empty.
     * @return True if the view is definitely empty, false otherwise.
     */
    [[nodiscard]] bool empty() const {
        return !valid() || pools.front()->empty();
    }

    /**
     * @brief Returns an iterator to the first entity that has the given
     * components.
     *
     * The returned iterator points to the first entity that has the given
     * components. If the view is empty, the returned iterator will be equal to
     * `end()`.
     *
     * @note
     * Iterators stay true to the order imposed to the underlying data
     * structures.
     *
     * @return An iterator to the first entity that has the given components.
     */
    [[nodiscard]] iterator begin() const {
        return valid() ? iterator{pools, filter, pools[0]->begin()} : iterator{};
    }

    /**
     * @brief Returns an iterator that is past the last entity that has the
     * given components.
     *
     * The returned iterator points to the entity following the last entity that
     * has the given components. Attempting to dereference the returned iterator
     * results in undefined behavior.
     *
     * @note
     * Iterators stay true to the order imposed to the underlying data
     * structures.
     *
     * @return An iterator to the entity following the last entity that has the
     * given components.
     */
    [[nodiscard]] iterator end() const {
        return valid() ? iterator{pools, filter, pools[0]->end()} : iterator{};
    }

    /**
     * @brief Checks if a view contains an entity.
     * @param entt A valid entity identifier.
     * @return True if the view contains the given entity, false otherwise.
     */
    [[nodiscard]] bool contains(const entity_type entt) const {
        return valid() && std::all_of(pools.cbegin(), pools.cend(), [entt](const auto *curr) { return curr->contains(entt); })
                && std::none_of(filter.cbegin(), filter.cend(), [entt](const auto *curr) { return curr && curr->contains(entt); });
    }

    /**
     * @brief Iterates entities and applies the given function object to them.
     *
     * The function object is invoked for each entity. It is provided only with
     * the entity itself. To get the components, users can use the registry with
     * which the view was built.

     * The signature of the function should be equivalent to the following:
     *
     * @code{.cpp}
     * void(const entity_type);
     * @endcode
     *
     * @tparam Func Type of the function object to invoke.
     * @param func A valid function object.
     */
    template<typename Func>
    void each(Func func) const {
        for(const auto entity: *this) {
            func(entity);
        }
    }

private:
    std::vector<const sparse_set<Entity> *> pools;
    std::vector<const sparse_set<Entity> *> filter;
};

}

#endif
<|endoftext|>


<commit_before>#include "updater.h"

#include <QtCore/QCoreApplication>
#include <QtCore/QFileInfo>
#include <QtCore/QProcess>
#include <QtXml/QDomDocument>

using namespace updatesChecker;

Updater::Updater(QObject *parent)
	: QObject(parent)
	, mUpdaterProcess(nullptr)
{
}

void Updater::checkForNewVersion()
{
	executeUpdater("--checkupdates");
}

void Updater::start()
{
	executeUpdater("--updater");
}

void Updater::executeUpdater(QString const &mode)
{
	mUpdaterProcess = new QProcess(this);
	mUpdaterProcess->setWorkingDirectory(QCoreApplication::applicationDirPath());
	connect(mUpdaterProcess, SIGNAL(finished(int)), this, SLOT(readAnswer()));

	if (mode == "--updater") {
		mUpdaterProcess->startDetached(QCoreApplication::applicationDirPath() + "/maintenance", {mode});
	} else {
		mUpdaterProcess->start(QCoreApplication::applicationDirPath() + "/maintenance", {mode});
	}
}

void Updater::readAnswer()
{
	QString const output = mUpdaterProcess->readAllStandardOutput();
	// Checking that output is a valid XML
	QDomDocument parser;
	parser.setContent(output);
	if (!output.isEmpty() && !parser.isNull()) {
		emit newVersionAvailable();
	} else {
		emit noNewVersionAvailable();
	}
}
<commit_msg>Added updater output logging<commit_after>#include "updater.h"

#include <QtCore/QCoreApplication>
#include <QtCore/QFileInfo>
#include <QtCore/QProcess>
#include <QtXml/QDomDocument>

#include <qrkernel/logging.h>

using namespace updatesChecker;

Updater::Updater(QObject *parent)
	: QObject(parent)
	, mUpdaterProcess(nullptr)
{
}

void Updater::checkForNewVersion()
{
	executeUpdater("--checkupdates");
}

void Updater::start()
{
	executeUpdater("--updater");
}

void Updater::executeUpdater(QString const &mode)
{
	mUpdaterProcess = new QProcess(this);
	mUpdaterProcess->setWorkingDirectory(QCoreApplication::applicationDirPath());
	connect(mUpdaterProcess, SIGNAL(finished(int)), this, SLOT(readAnswer()));

	if (mode == "--updater") {
		mUpdaterProcess->startDetached(QCoreApplication::applicationDirPath() + "/maintenance", {mode});
	} else {
		mUpdaterProcess->start(QCoreApplication::applicationDirPath() + "/maintenance", {mode});
	}
}

void Updater::readAnswer()
{
	QString const output = mUpdaterProcess->readAllStandardOutput();
	// Checking that output is a valid XML
	QDomDocument parser;
	parser.setContent(output);
	if (!output.isEmpty() && !parser.isNull()) {
		QLOG_INFO() << "Updater output:" << output;
		emit newVersionAvailable();
	} else {
		emit noNewVersionAvailable();
	}
}
<|endoftext|>


<commit_before>/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

#include <ode/odeconfig.h>
#include "config.h"
#include <ode/memory.h>
#include <ode/error.h>

static dAllocFunction *allocfn = 0;
static dReallocFunction *reallocfn = 0;
static dFreeFunction *freefn = 0;

void dSetAllocHandler (dAllocFunction *fn)
{
  allocfn = fn;
}

void dSetReallocHandler (dReallocFunction *fn)
{
  reallocfn = fn;
}

void dSetFreeHandler (dFreeFunction *fn)
{
  freefn = fn;
}

dAllocFunction *dGetAllocHandler()
{
  return allocfn;
}

dReallocFunction *dGetReallocHandler()
{
  return reallocfn;
}

dFreeFunction *dGetFreeHandler()
{
  return freefn;
}

void * dAlloc (size_t size)
{
  if (allocfn) return allocfn (size); else return malloc (size);
}

void * dRealloc (void *ptr, size_t oldsize, size_t newsize)
{
  if (reallocfn) return reallocfn (ptr,oldsize,newsize);
  else return realloc (ptr,newsize);
}

void dFree (void *ptr, size_t size)
{
  if (!ptr) return;
  if (freefn) freefn (ptr,size); else free (ptr);
}
<commit_msg>Added guard for AC_FUNC_MALLOC and AC_FUNC_REALLOC in memory.cpp, the macros break cross compilation Linux-to-Windows by defining malloc and realloc as rpl_malloc and rpl_realloc respectively<commit_after>/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

#include <ode/odeconfig.h>
#include "config.h"
#include <ode/memory.h>
#include <ode/error.h>

static dAllocFunction *allocfn = 0;
static dReallocFunction *reallocfn = 0;
static dFreeFunction *freefn = 0;

#ifdef __MINGW32__
/*
   this is a guard against AC_FUNC_MALLOC and AC_FUNC_REALLOC
   which break cross compilation, no issues in native MSYS.
*/
#undef malloc
#undef realloc
#endif

void dSetAllocHandler (dAllocFunction *fn)
{
  allocfn = fn;
}

void dSetReallocHandler (dReallocFunction *fn)
{
  reallocfn = fn;
}

void dSetFreeHandler (dFreeFunction *fn)
{
  freefn = fn;
}

dAllocFunction *dGetAllocHandler()
{
  return allocfn;
}

dReallocFunction *dGetReallocHandler()
{
  return reallocfn;
}

dFreeFunction *dGetFreeHandler()
{
  return freefn;
}

void * dAlloc (size_t size)
{
  if (allocfn) return allocfn (size); else return malloc (size);
}

void * dRealloc (void *ptr, size_t oldsize, size_t newsize)
{
  if (reallocfn) return reallocfn (ptr,oldsize,newsize);
  else return realloc (ptr,newsize);
}

void dFree (void *ptr, size_t size)
{
  if (!ptr) return;
  if (freefn) freefn (ptr,size); else free (ptr);
}
<|endoftext|>


<commit_before>// the trajectory action server for two wheel robot

// communication includes
    // subscribe to topic "swarm_robot_poses" with message swarm_robot_poses.msg
    // subscribe to topic "two_wheel_poses" with message two_wheel_poses.msg
    // publish to topic "two_wheel_poses_cmd" with message two_wheel_poses.msg
    // host a action server "two_wheel_traj_action" with message swarm_robot_traj.action

// there are different wheel trajectories of moving to a target position
// Method 1:
    // first rotating the robot to the direction of the target position
    // then moving the robot in straight line to the target position
    // pros:
        // simple and straightforward wheel position increment calculation
        // easy to debug the potential problems
    // cons:
        // ugly and inefficient to take two movements to moving to the target
// Method 2:
    // the path between start position and target position is circular
    // the center of the circular path is on the common axis of the two wheels
    // pros:
        // taking only one movements to move to the target, much more elegant
        // two wheel position are linearly interpolated because of this circular path
    // cons:
        // a bit difficult to calculate the increment of the wheel positions
        // difficult to debug the algorithm for the wheel positions

// for the convenience of wheel position calculation and debug, method 1 is implemented here

// one significant problem of the controller of two_wheel_robot is that
    // when one wheel is commanded rotating and the other is remained current position
    // the static wheel will naturally rotating at the other direction because of inertia
    // how to solve or avoid this problem? and make position control more precise

#include <ros/ros.h>
#include <math.h>
#include <actionlib/server/simple_action_server.h>
#include <swarm_robot_action/swarm_robot_trajAction.h>
#include <swarm_robot_msgs/two_wheel_poses.h>

// interpolation parameters, change setup here
const double dt = 0.01;  // interpolating resolution
// minimal distance with goal position to interpolate
// distance small than ds_min will be neglected, cube size of two_wheel_robot is 0.0254
// this value should be set very low
const double ds_min = 0.001;
// half distance of two wheels
// not a good way to get this value here
const double half_wheel_dist = 0.0177;

class TwoWheelTrajActionServer {
public:
    TwoWheelTrajActionServer(ros::NodeHandle* nodehandle);

private:
    void swarmRobotPosesCb(const swarm_robot_msgs::swarm_robot_poses& message_holder);
    void twoWheelPosesCb(const swarm_robot_msgs::two_wheel_poses& message_holder);
    void executeCb(const
        actionlib::SimpleActionServer<swarm_robot_action::swarm_robot_trajAction>::GoalConstPtr& goal);

    ros::NodeHandle nh_;  // a node handle is needed

    // declare a subscribers to topic "swarm_robot_poses" and "two_wheel_poses"
    ros::Subscriber swarm_robot_poses_subscriber;
    ros::Subscriber two_wheel_poses_subscriber;
    // declare a publisher to topic "two_wheel_poses_cmd"
    ros::Publisher two_wheel_poses_cmd_publisher;
    // declare an action server
    actionlib::SimpleActionServer<swarm_robot_action::swarm_robot_trajAction> as_;

    // messages
    swarm_robot_msgs::swarm_robot_poses robot_poses_msg;  // current robot poses
    swarm_robot_msgs::two_wheel_poses wheel_poses_msg;  // current wheel poses
    swarm_robot_msgs::two_wheel_poses wheel_poses_cmd_msg;  // wheel poses command
    // for trajectory interpolation
    swarm_robot_msgs::two_wheel_poses wheel_poses_self_rotating;
    swarm_robot_msgs::two_wheel_poses wheel_poses_line_moving;
    // action messages
    swarm_robot_action::swarm_robot_trajActionGoal goal_;
    swarm_robot_action::swarm_robot_trajActionResult result_;
    swarm_robot_action::swarm_robot_trajActionFeedback feedback_;

    // other variables
    std::string robot_model_name;  // from parameter server
    int robot_quantity;  // from parameter server
    bool b_robot_poses_cb_started;
    bool b_wheel_poses_cb_started;
};

// most initialization work will be done here
TwoWheelTrajActionServer::TwoWheelTrajActionServer(ros::NodeHandle* nodehandle):
    nh_(*nodehandle),  // dereference the pointer and pass the value
    as_(nh_, "two_wheel_traj_action", boost::bind(&TwoWheelTrajActionServer::executeCb, this, _1), false)
{
    ROS_INFO("in constructor of TwoWheelTrajActionServer...");

    // initialize the subscribers
    b_robot_poses_cb_started = false;
    swarm_robot_poses_subscriber = nh_.subscribe("swarm_robot_poses", 1, swarmRobotPosesCb);
    b_poses_callback_started = false;
    two_wheel_poses_subscriber = nh_.subscribe("two_wheel_poses", 1, twoWheelPosesCb);
    // initialize the publisher
    two_wheel_poses_cmd_publisher = nh_.advertise<swarm_robot_msgs::two_wheel_poses>("two_wheel_poses_cmd", 1);

    // get initialization message of robot swarm from parameter server
    bool get_name, get_quantity;
    get_name = nh_.getParam("/robot_model_name", robot_model_name);
    get_quantity = nh_.getParam("/robot_quantity", robot_quantity);
    if (!(get_name && get_quantity))
        return 0;  // return if fail to get parameter

    // make sure topics "swarm_robot_poses" and "two_wheel_poses" are active
    while (!(b_robot_poses_cb_started && b_wheel_poses_cb_started)) {
        ros::Duration(0.5).sleep();
        ros::spinOnce();
    }
    std::cout << "topic message from swarm_robot_poses and two_wheel_poses is ready" << std::endl;

    as_.start();  // start the "two_wheel_traj_action"
}

// callback for message from topic "swarm_robot_poses"
void TwoWheelTrajActionServer::swarmRobotPosesCb(const swarm_robot_msgs::swarm_robot_poses& message_holder) {
    if (!b_robot_poses_cb_started)  // first time to be invoked
        b_robot_poses_cb_started = true;
    robot_poses_msg = message_holder;
}

// callback for message from topic "two_wheel_poses"
void TwoWheelTrajActionServer::twoWheelPosesCb(const swarm_robot_msgs::two_wheel_poses& message_holder) {
    if (!b_wheel_poses_cb_started)  // first time to be invoked
        b_wheel_poses_cb_started = true;
    wheel_poses_msg = message_holder;
}

// most practical work will be done here
void TwoWheelTrajActionServer::executeCb(const actionlib::
    SimpleActionServer<swarm_robot_action::swarm_robot_trajAction>::GoalConstPtr& goal)
{
    ROS_INFO("in executeCb...");

    // copy the goal message, avoid using "->" too much
    double goal_x[] = goal -> x;
    double goal_y[] = goal -> y;
    double goal_time = goal -> time;

// messages
swarm_robot_msgs::swarm_robot_poses robot_poses_msg;  // current robot poses
swarm_robot_msgs::two_wheel_poses wheel_poses_msg;  // current wheel poses
swarm_robot_msgs::two_wheel_poses wheel_poses_cmd_msg;  // wheel poses command

swarm_robot_msgs::two_wheel_poses wheel_poses_self_rotating;
swarm_robot_msgs::two_wheel_poses wheel_poses_line_moving;

    // the time for this two movements are linearly distributed according to the wheel path

    double x_start;
    double y_start;
    double x_end;
    double y_end;
    double distance;
    double angle_start;
    double angle_end;
    double angle_rotate;
    // calculate the cmd message for each robot
    for (int i=0; i<robot_quantity; i++) {
        x_start = robot_poses_msg.x[i];
        y_start = robot_poses_msg.y[i];
        x_end = goal_x[i];
        y_end = goal_y[i];
        distance = sqrt(pow((x_end - x_start), 2) + pow((y_end - y_start), 2));
        // check if target position is too close
        if (distance < ds_min) {
            // copy the current wheel position
            wheel_poses_self_rotating.left_wheel_pos[i] = wheel_poses_msg.left_wheel_pos[i];
            wheel_poses_line_moving.left_wheel_pos[i] = wheel_poses_msg.left_wheel_pos[i];
            wheel_poses_self_rotating.right_wheel_pos[i] = wheel_poses_msg.right_wheel_pos[i];
            wheel_poses_line_moving.right_wheel_pos[i] = wheel_poses_msg.right_wheel_pos[i];
        }
        else {
            // two stage movement calculation
            // stage 1, self rotating calculation
            angle_start = robot_poses_msg.angle[i];
            angle_end = atan2(y_end - y_start, x_end - x_start);
            angle_rotate = angle_end - angle_start;  // rotate from angle_start to angle_end
            // angel_end and angle_start both belong to range of (-M_PI, M_PI)

        }

    }

// if the goal position is too close, neglect it
// rotation angle is between -M_PI and M_PI

// remember to ros::spinOnce at place that is necessary

}

int main(int argc, char **argv) {
    ros::init(argc, argv, "two_wheel_traj_interpolator_as");
    ros::NodeHandle nh;

    ROS_INFO("instantiating an object of class TwoWheelTrajActionServer...");
    TwoWheelTrajActionServer as_object(&nh);

    ROS_INFO("going into spin...");

    while (ros::ok()) {
        ros::spinOnce();
        ros::Duration(0.1).sleep();
    }
    return 0;
}

<commit_msg>update two wheel trajectory interpolator<commit_after>// the trajectory action server for two wheel robot

// communication includes
    // subscribe to topic "swarm_robot_poses" with message swarm_robot_poses.msg
    // subscribe to topic "two_wheel_poses" with message two_wheel_poses.msg
    // publish to topic "two_wheel_poses_cmd" with message two_wheel_poses.msg
    // host a action server "two_wheel_traj_action" with message swarm_robot_traj.action

// there are different wheel trajectories of moving to a target position
// Method 1:
    // first rotating the robot to the direction of the target position
    // then moving the robot in straight line to the target position
    // pros:
        // simple and straightforward wheel position increment calculation
        // easy to debug the potential problems
    // cons:
        // ugly and inefficient to take two movements to moving to the target
// Method 2:
    // the path between start position and target position is circular
    // the center of the circular path is on the common axis of the two wheels
    // pros:
        // taking only one movements to move to the target, much more elegant
        // two wheel position are linearly interpolated because of this circular path
    // cons:
        // a bit difficult to calculate the increment of the wheel positions
        // difficult to debug the algorithm for the wheel positions

// for the convenience of wheel position calculation and debug, method 1 is implemented here

// one significant problem of the controller of two_wheel_robot is that
    // when one wheel is commanded rotating and the other is remained current position
    // the static wheel will naturally rotating at the other direction because of inertia
    // how to solve or avoid this problem? and make position control more precise

#include <ros/ros.h>
#include <math.h>
#include <actionlib/server/simple_action_server.h>
#include <swarm_robot_action/swarm_robot_trajAction.h>
#include <swarm_robot_msgs/two_wheel_poses.h>

// interpolation parameters, change setup here
const double dt = 0.01;  // interpolating resolution on time
// minimal distance with goal position to interpolate
// distance small than ds_min will be neglected, cube size of two_wheel_robot is 0.0254
// this value should be set very low
const double ds_min = 0.001;
// half distance of two wheels
// not a good way to get this value here
const double half_wheel_dist = 0.0177;

class TwoWheelTrajActionServer {
public:
    TwoWheelTrajActionServer(ros::NodeHandle* nodehandle);

private:
    void swarmRobotPosesCb(const swarm_robot_msgs::swarm_robot_poses& message_holder);
    void twoWheelPosesCb(const swarm_robot_msgs::two_wheel_poses& message_holder);
    void executeCb(const
        actionlib::SimpleActionServer<swarm_robot_action::swarm_robot_trajAction>::GoalConstPtr& goal);

    ros::NodeHandle nh_;  // a node handle is needed

    // declare a subscribers to topic "swarm_robot_poses" and "two_wheel_poses"
    ros::Subscriber swarm_robot_poses_subscriber;
    ros::Subscriber two_wheel_poses_subscriber;
    // declare a publisher to topic "two_wheel_poses_cmd"
    ros::Publisher two_wheel_poses_cmd_publisher;
    // declare an action server
    actionlib::SimpleActionServer<swarm_robot_action::swarm_robot_trajAction> as_;

    // messages
    swarm_robot_msgs::swarm_robot_poses robot_poses_msg;  // current robot poses
    swarm_robot_msgs::two_wheel_poses wheel_poses_msg;  // current wheel poses
    swarm_robot_msgs::two_wheel_poses wheel_poses_cmd_msg;  // wheel poses command
    // for trajectory interpolation
    swarm_robot_msgs::two_wheel_poses wheel_poses_self_rotating;
    swarm_robot_msgs::two_wheel_poses wheel_poses_line_moving;
    // action messages
    swarm_robot_action::swarm_robot_trajActionGoal goal_;
    swarm_robot_action::swarm_robot_trajActionResult result_;
    swarm_robot_action::swarm_robot_trajActionFeedback feedback_;

    // other variables
    std::string robot_model_name;  // from parameter server
    int robot_quantity;  // from parameter server
    bool b_robot_poses_cb_started;
    bool b_wheel_poses_cb_started;
};

// most initialization work will be done here
TwoWheelTrajActionServer::TwoWheelTrajActionServer(ros::NodeHandle* nodehandle):
    nh_(*nodehandle),  // dereference the pointer and pass the value
    as_(nh_, "two_wheel_traj_action", boost::bind(&TwoWheelTrajActionServer::executeCb, this, _1), false)
{
    ROS_INFO("in constructor of TwoWheelTrajActionServer...");

    // initialize the subscribers
    b_robot_poses_cb_started = false;
    swarm_robot_poses_subscriber = nh_.subscribe("swarm_robot_poses", 1, swarmRobotPosesCb);
    b_poses_callback_started = false;
    two_wheel_poses_subscriber = nh_.subscribe("two_wheel_poses", 1, twoWheelPosesCb);
    // initialize the publisher
    two_wheel_poses_cmd_publisher = nh_.advertise<swarm_robot_msgs::two_wheel_poses>("two_wheel_poses_cmd", 1);

    // get initialization message of robot swarm from parameter server
    bool get_name, get_quantity;
    get_name = nh_.getParam("/robot_model_name", robot_model_name);
    get_quantity = nh_.getParam("/robot_quantity", robot_quantity);
    if (!(get_name && get_quantity))
        return 0;  // return if fail to get parameter

    // make sure topics "swarm_robot_poses" and "two_wheel_poses" are active
    while (!(b_robot_poses_cb_started && b_wheel_poses_cb_started)) {
        ros::Duration(0.5).sleep();
        ros::spinOnce();
    }
    std::cout << "topic message from swarm_robot_poses and two_wheel_poses is ready" << std::endl;

    as_.start();  // start the "two_wheel_traj_action"
}

// callback for message from topic "swarm_robot_poses"
void TwoWheelTrajActionServer::swarmRobotPosesCb(const swarm_robot_msgs::swarm_robot_poses& message_holder) {
    if (!b_robot_poses_cb_started)  // first time to be invoked
        b_robot_poses_cb_started = true;
    robot_poses_msg = message_holder;
}

// callback for message from topic "two_wheel_poses"
void TwoWheelTrajActionServer::twoWheelPosesCb(const swarm_robot_msgs::two_wheel_poses& message_holder) {
    if (!b_wheel_poses_cb_started)  // first time to be invoked
        b_wheel_poses_cb_started = true;
    wheel_poses_msg = message_holder;
}

// most practical work will be done here
void TwoWheelTrajActionServer::executeCb(const actionlib::
    SimpleActionServer<swarm_robot_action::swarm_robot_trajAction>::GoalConstPtr& goal)
{
    ROS_INFO("in executeCb...");

    // copy the goal message, avoid using "->" too much
    double goal_x[] = goal -> x;
    double goal_y[] = goal -> y;
    double goal_time = goal -> time;

    // variables needed in wheel trajectory calculation
    double x_start;
    double y_start;
    double x_end;
    double y_end;
    double distance;
    double angle_start;  // current angle of the robot
    double angle_end;  // target angle of the robot
    double angle_rotate;  // rotated angle of the robot
    double wheel_incre_self_rotating;  // the wheel postion increment in self rotating stage
    double wheel_incre_line_moving;  // the wheel position increment in line moving stage
        // both increment are signed after been given values
    // calculate the cmd message for each robot
    for (int i=0; i<robot_quantity; i++) {
        x_start = robot_poses_msg.x[i];
        y_start = robot_poses_msg.y[i];
        x_end = goal_x[i];
        y_end = goal_y[i];
        distance = sqrt(pow((x_end - x_start), 2) + pow((y_end - y_start), 2));
        // check if target position is too close
        if (distance < ds_min) {
            // copy the current wheel position
            wheel_poses_self_rotating.left_wheel_pos[i] = wheel_poses_msg.left_wheel_pos[i];
            wheel_poses_self_rotating.right_wheel_pos[i] = wheel_poses_msg.right_wheel_pos[i];
            wheel_poses_line_moving.left_wheel_pos[i] = wheel_poses_msg.left_wheel_pos[i];
            wheel_poses_line_moving.right_wheel_pos[i] = wheel_poses_msg.right_wheel_pos[i];
        }
        else {
            // two stage movement calculation

            // stage 1, self rotating calculation
            angle_start = robot_poses_msg.angle[i];
            angle_end = atan2(y_end - y_start, x_end - x_start);
            angle_rotate = angle_end - angle_start;  // rotate from angle_start to angle_end
            // angel_end and angle_start both belong to range of (-M_PI, M_PI)
            // change angle_rotate into the same range (-M_PI, M_PI)
            if (angle_rotate > M_PI)
                angle_rotate = angle_rotate - 2 * M_PI;
            if (angle_rotate < -M_PI)
                angle_rotate = angle_rotate + 2 * M_PI;
            bool robot_heading = true;  // whether the robot is heading or backing to the target
            // it's not necessary to rotate the heading direction to the target every time
            // because the two wheel robot can also moving backward
            // change angle_rotate into the range (-M_PI/2, M_PI/2), use robot_heading to mark it
            if (angle_rotate > M_PI/2) {
                robot_heading = false;
                angle_rotate = angle_rotate - M_PI;
            }
            if (angle_rotate < -M_PI/2) {
                robot_heading = false;
                angle_rotate = angle_rotate + M_PI;
            }
            // prepare wheel poses message base on current wheel positions
            wheel_incre_self_rotating = angle_rotate * half_wheel_dist;
            // left wheel move backward when rotating angle is positive
            wheel_poses_self_rotating.left_wheel_pos[i] =
                wheel_poses_msg.left_wheel_pos[i] - wheel_incre_self_rotating;
            // right wheel move forward when rotating angle is positive
            wheel_poses_self_rotating.right_wheel_pos[i] =
                wheel_poses_msg.right_wheel_pos[i] + wheel_incre_self_rotating;

            // stage 2, line moving calculation
            // prepare wheel poses message bese on self rotated wheel positions
            if (robot_heading)
                wheel_incre_line_moving = distance;  // moving forward
            else
                wheel_incre_line_moving = -distance;  // moving backward
            // both left and right wheel rotating at same direction
            wheel_poses_line_moving.left_wheel_pos[i] =
                wheel_poses_self_rotating.left_wheel_pos[i] + wheel_incre_line_moving;
            wheel_poses_line_moving.right_wheel_pos[i] =
                wheel_poses_self_rotating.right_wheel_pos[i] + wheel_incre_line_moving;
        }
    }

    // trajectory interpolation
    // time for the two movements are linearly distributed wrt the wheel rotation
    double time_self_rotating;
    double time_line_moving;
    time_self_rotating = goal_time * (abs(wheel_incre_self_rotating)) /
        (abs(wheel_incre_self_rotating) + abs(wheel_incre_line_moving));
    time_line_moving = goal_time - time_self_rotating;

// messages
swarm_robot_msgs::swarm_robot_poses robot_poses_msg;  // current robot poses
swarm_robot_msgs::two_wheel_poses wheel_poses_msg;  // current wheel poses
swarm_robot_msgs::two_wheel_poses wheel_poses_cmd_msg;  // wheel poses command

swarm_robot_msgs::two_wheel_poses wheel_poses_self_rotating;
swarm_robot_msgs::two_wheel_poses wheel_poses_line_moving;

// remember to ros::spinOnce at place that is necessary

}

int main(int argc, char **argv) {
    ros::init(argc, argv, "two_wheel_traj_interpolator_as");
    ros::NodeHandle nh;

    ROS_INFO("instantiating an object of class TwoWheelTrajActionServer...");
    TwoWheelTrajActionServer as_object(&nh);

    ROS_INFO("going into spin...");

    while (ros::ok()) {
        ros::spinOnce();
        ros::Duration(0.1).sleep();
    }
    return 0;
}

<|endoftext|>


<commit_before>// Genetic5.cpp : Defines the entry point for the console application.
//

#pragma warning(disable:4786)		// disable debug warning

#include <iostream>					// for cout etc.
#include <vector>					// for vector class
#include <string>					// for string class
#include <algorithm>				// for sort algorithm
#include <time.h>					// for random seed
#include <math.h>					// for abs()

#define GA_POPSIZE        2048        // ga population size
#define GA_MAXITER        16384        // maximum iterations
#define GA_ELITRATE        0.10f        // elitism rate
#define GA_MUTATIONRATE    0.25f        // mutation rate
#define GA_MUTATION        RAND_MAX * GA_MUTATIONRATE
#define GA_TARGET        std::string("Hello world!")

using namespace std;                // polluting global namespace, but hey...

struct ga_struct {
    string str;                        // the string
    unsigned int fitness;            // its fitness
};

typedef vector<ga_struct> ga_vector;// for brevity

void init_population(ga_vector &population,
                     ga_vector &buffer) {
    int tsize = GA_TARGET.size();

    for (int i = 0; i < GA_POPSIZE; i++) {
        ga_struct citizen;

        citizen.fitness = 0;
        citizen.str.erase();

        for (int j = 0; j < tsize; j++)
            citizen.str += (rand() % 90) + 32;

        population.push_back(citizen);
    }

    buffer.resize(GA_POPSIZE);
}

int heuristic1(string elem, string target) {
    unsigned int fitness = 0;
    for (int j = 0; j < target.size(); j++) {
        fitness += abs(int(elem[j] - target[j]));
    }
    return fitness;
}

int heuristic2(string elem, string target) {
    unsigned int fitness = 0;
    for (int j = 0; j < target.size(); j++) {
        fitness += elem[j] == target[j] ? 0 : 1;
    }
    return fitness;
}

bool strContains(string s, char c) {
    for (int i = 0; i < s.length(); i++) {
        if (s[i] == c) return true;
    }
    return false;
}

int generalHeuristic3(string elem, string target, unsigned int containsWeight, unsigned int eqWeight) {
    unsigned int fitness = 0;
    for (int j = 0; j < target.size(); j++) {
        bool contains = strContains(target, elem[j]);
        bool eq = elem[j] == target[j];

        fitness += (containsWeight + eqWeight) - containsWeight * contains - eqWeight * eq;
    }
    return fitness;
}

int heuristic3(string elem, string target) {
    return generalHeuristic3(elem, target, 1, 2);
}

void calc_fitness(ga_vector &population) {
    string target = GA_TARGET;
    for (int i = 0; i < GA_POPSIZE; i++) {
        population[i].fitness = heuristic3(population[i].str, target);
    }
}

bool fitness_sort(ga_struct x, ga_struct y) {
    return (x.fitness < y.fitness);
}

inline void sort_by_fitness(ga_vector &population) {
    sort(population.begin(), population.end(), fitness_sort);
}

void elitism(ga_vector &population,
             ga_vector &buffer, int esize) {
    for (int i = 0; i < esize; i++) {
        buffer[i].str = population[i].str;
        buffer[i].fitness = population[i].fitness;
    }
}

void mutate(ga_struct &member) {
    int tsize = GA_TARGET.size();
    int ipos = rand() % tsize;
    int delta = (rand() % 90) + 32;

    member.str[ipos] = ((member.str[ipos] + delta) % 122);
}

void mate(ga_vector &population, ga_vector &buffer) {
    int esize = GA_POPSIZE * GA_ELITRATE;
    int tsize = GA_TARGET.size(), spos, i1, i2;

    elitism(population, buffer, esize);

    // Mate the rest
    for (int i = esize; i < GA_POPSIZE; i++) {
        i1 = rand() % (GA_POPSIZE / 2);
        i2 = rand() % (GA_POPSIZE / 2);
        spos = rand() % tsize;

        buffer[i].str = population[i1].str.substr(0, spos) +
                        population[i2].str.substr(spos, tsize - spos);

        if (rand() < GA_MUTATION) mutate(buffer[i]);
    }
}

inline void swap(ga_vector *&population,
                 ga_vector *&buffer) {
    ga_vector *temp = population;
    population = buffer;
    buffer = temp;
}

double average(ga_vector &vect) {
    double sum = 0;
    int size = vect.size();

    for (int i = 0; i < vect.size(); i++)
        sum += vect[i].fitness;

    return sum / size;
}

double variance(ga_vector &vect) {
    double sumOfDiffs = 0;
    double avg = average(vect);
    int size = vect.size();

    for (int i = 0; i < vect.size(); i++) {
        double diff = (double) vect[i].fitness - avg;
        sumOfDiffs += diff * diff;
    }

    return sumOfDiffs / size;
}

double standardDeviation(ga_vector &vect) {
    return sqrt(variance(vect));
}

void print_best(ga_vector &gav, unsigned int iteration) {
    cout << "Gen " << iteration << endl;
    cout << "Best: " << gav[0].str << " (" << gav[0].fitness << ")" << endl;
    cout << "Average Fitness: " << average(gav) << endl;
    cout << "Standard Deviation: " << standardDeviation(gav) << endl << endl;
}

int main() {
    srand(unsigned(time(NULL)));

    ga_vector pop_alpha, pop_beta;
    ga_vector *population, *buffer;

    init_population(pop_alpha, pop_beta);
    population = &pop_alpha;
    buffer = &pop_beta;

    for (int i = 0; i < GA_MAXITER; i++) {
        calc_fitness(*population);        // calculate fitness
        sort_by_fitness(*population);    // sort them
        print_best(*population, i);        // print the best one

        if ((*population)[0].fitness == 0) break;

        mate(*population, *buffer);        // mate the population together
        swap(population, buffer);        // swap buffers
    }

    return 0;
}
<commit_msg>Lab1: Get rid of warnings<commit_after>// Genetic5.cpp : Defines the entry point for the console application.
//

#pragma warning(disable:4786)		// disable debug warning

#include <iostream>					// for cout etc.
#include <vector>					// for vector class
#include <string>					// for string class
#include <algorithm>				// for sort algorithm
#include <time.h>					// for random seed
#include <math.h>					// for abs()

#define GA_POPSIZE        2048        // ga population size
#define GA_MAXITER        16384        // maximum iterations
#define GA_ELITRATE        0.10f        // elitism rate
#define GA_MUTATIONRATE    0.25f        // mutation rate
#define GA_MUTATION        RAND_MAX * GA_MUTATIONRATE
#define GA_TARGET        std::string("Hello world!")

using namespace std;                // polluting global namespace, but hey...

struct ga_struct {
    string str;                        // the string
    unsigned int fitness;            // its fitness
};

typedef vector<ga_struct> ga_vector;// for brevity

void init_population(ga_vector &population,
                     ga_vector &buffer) {
    int tsize = (int) GA_TARGET.size();

    for (int i = 0; i < GA_POPSIZE; i++) {
        ga_struct citizen;

        citizen.fitness = 0;
        citizen.str.erase();

        for (int j = 0; j < tsize; j++)
            citizen.str += (rand() % 90) + 32;

        population.push_back(citizen);
    }

    buffer.resize(GA_POPSIZE);
}

unsigned int heuristic1(string elem, string target) {
    unsigned int fitness = 0;
    for (int j = 0; j < target.size(); j++) {
        fitness += abs(int(elem[j] - target[j]));
    }
    return fitness;
}

unsigned int heuristic2(string elem, string target) {
    unsigned int fitness = 0;
    for (int j = 0; j < target.size(); j++) {
        fitness += elem[j] == target[j] ? 0 : 1;
    }
    return fitness;
}

bool strContains(string s, char c) {
    for (int i = 0; i < s.length(); i++) {
        if (s[i] == c) return true;
    }
    return false;
}

unsigned int generalHeuristic3(string elem, string target, unsigned int containsWeight, unsigned int eqWeight) {
    unsigned int fitness = 0;
    for (int j = 0; j < target.size(); j++) {
        bool contains = strContains(target, elem[j]);
        bool eq = elem[j] == target[j];

        fitness += (containsWeight + eqWeight) - containsWeight * contains - eqWeight * eq;
    }
    return fitness;
}

unsigned int heuristic3(string elem, string target) {
    return generalHeuristic3(elem, target, 1, 2);
}

void calc_fitness(ga_vector &population) {
    string target = GA_TARGET;
    for (int i = 0; i < GA_POPSIZE; i++) {
        population[i].fitness = heuristic3(population[i].str, target);
    }
}

bool fitness_sort(ga_struct x, ga_struct y) {
    return (x.fitness < y.fitness);
}

inline void sort_by_fitness(ga_vector &population) {
    sort(population.begin(), population.end(), fitness_sort);
}

void elitism(ga_vector &population,
             ga_vector &buffer, int esize) {
    for (int i = 0; i < esize; i++) {
        buffer[i].str = population[i].str;
        buffer[i].fitness = population[i].fitness;
    }
}

void mutate(ga_struct &member) {
    int tsize = (int) GA_TARGET.size();
    int ipos = rand() % tsize;
    int delta = (rand() % 90) + 32;

    member.str[ipos] = (char) ((member.str[ipos] + delta) % 122);
}

void mate(ga_vector &population, ga_vector &buffer) {
    int esize = (int) (GA_POPSIZE * GA_ELITRATE);
    size_t tsize = GA_TARGET.size();

    elitism(population, buffer, esize);

    // Mate the rest
    for (int i = esize; i < GA_POPSIZE; i++) {
        int i1 = rand() % (GA_POPSIZE / 2);
        int i2 = rand() % (GA_POPSIZE / 2);
        size_t spos = rand() % tsize;

        buffer[i].str = population[i1].str.substr(0, spos) +
                        population[i2].str.substr(spos, tsize - spos);

        if (rand() < GA_MUTATION) mutate(buffer[i]);
    }
}

inline void swap(ga_vector *&population,
                 ga_vector *&buffer) {
    ga_vector *temp = population;
    population = buffer;
    buffer = temp;
}

double average(ga_vector &vect) {
    double sum = 0;
    size_t size = vect.size();

    for (int i = 0; i < vect.size(); i++)
        sum += vect[i].fitness;

    return sum / size;
}

double variance(ga_vector &vect) {
    double sumOfDiffs = 0;
    double avg = average(vect);
    size_t size = vect.size();

    for (int i = 0; i < vect.size(); i++) {
        double diff = (double) vect[i].fitness - avg;
        sumOfDiffs += diff * diff;
    }

    return sumOfDiffs / size;
}

double standardDeviation(ga_vector &vect) {
    return sqrt(variance(vect));
}

void print_best(ga_vector &gav, unsigned int iteration) {
    cout << "Gen " << iteration << endl;
    cout << "Best: " << gav[0].str << " (" << gav[0].fitness << ")" << endl;
    cout << "Average Fitness: " << average(gav) << endl;
    cout << "Standard Deviation: " << standardDeviation(gav) << endl << endl;
}

int main() {
    srand(unsigned(time(NULL)));

    ga_vector pop_alpha, pop_beta;
    ga_vector *population, *buffer;

    init_population(pop_alpha, pop_beta);
    population = &pop_alpha;
    buffer = &pop_beta;

    for (unsigned int i = 0; i < GA_MAXITER; i++) {
        calc_fitness(*population);        // calculate fitness
        sort_by_fitness(*population);    // sort them
        print_best(*population, i);        // print the best one

        if ((*population)[0].fitness == 0) break;

        mate(*population, *buffer);        // mate the population together
        swap(population, buffer);        // swap buffers
    }

    return 0;
}
<|endoftext|>