void stateGoingToCube::processData(){
//Wait to get a still image
static int cubeFound;
static double distance;
static double angle;
static int color;
startProcessData();
if((getTimeMicroseconds()-startTimeStateMicroseconds)/1000 <= mywaitTimeMS){
stop();
cubeFound=foundCube();
distance = myImageProcessor->getNearestCubeDist()+GO_TO_CUBE_OVERSHOOT_DISTANCE;
angle = myImageProcessor->getNearestCubeAngle();
color = myImageProcessor->getNearestCubeColor();
}
else if(cubeFound){
goToPoint(distance,angle);
if (finishedGoingToPoint){
nextState = new stateCollectingCube(this);//(this,color); //->pass camera color
//PASS NO COLOR PARAMETER TO GET DATA FROM THE COLOR SENSOR
}
}
else{
nextState=new stateLookingForBlocks(this);
}
finishProcessData();
}
void
Timers::restartTimer (TimerKey const mt_nonnull timer_key)
{
Timer * const timer = timer_key;
TimerChain * const chain = timer->chain;
mutex.lock ();
assert (timer->active);
expiration_tree.remove (chain);
chain->timer_list.remove (timer);
timer->due_time = getTimeMicroseconds() + chain->interval_microseconds;
if (timer->due_time < chain->interval_microseconds) {
logW_ (_func, "Expiration time overflow");
timer->due_time = (Time) -1;
}
chain->timer_list.append (timer);
chain->nearest_time = chain->timer_list.getFirst()->due_time;
expiration_tree.add (chain);
expiration_tree_leftmost = expiration_tree.getLeftmost();
mutex.unlock ();
}
void libMaryInit ()
{
{
static bool initialized = false;
if (initialized) {
return;
}
initialized = true;
}
// Setting numeric locale for snprintf() to behave uniformly in all cases.
// Specifically, we need dot ('.') to be used as a decimal separator.
if (setlocale (LC_NUMERIC, "C") == NULL)
fprintf (stderr, "WARNING: Could not set LC_NUMERIC locale to \"C\"\n");
#ifndef LIBMARY_PLATFORM_WIN32
// GStreamer calls setlocale(LC_ALL, ""), which is lame. We fight this with setenv().
if (setenv ("LC_NUMERIC", "C", 1 /* overwrite */) == -1)
perror ("WARNING: Could not set LC_NUMERIC environment variable to \"C\"");
#endif
#ifdef LIBMARY_MT_SAFE
#ifdef LIBMARY__OLD_GTHREAD_API
if (!g_thread_get_initialized ())
g_thread_init (NULL);
#endif
#endif
initStatCounters ();
_libMary_stat = new (std::nothrow) Stat;
assert (_libMary_stat);
libMary_threadLocalInit ();
libMary_platformInit ();
// log*() logging is now available.
if (!updateTime ())
logE_ (_func, exc->toString());
#ifdef LIBMARY_ENABLE_MWRITEV
libMary_mwritevInit ();
#endif
randomSetSeed ((Uint32) getTimeMicroseconds());
}
Time
Timers::getSleepTime_microseconds ()
{
Time const cur_time = getTimeMicroseconds ();
MutexLock l (&mutex);
TimerChain * const chain = expiration_tree_leftmost;
if (chain == NULL) {
logD (timers, _func, ": null chain");
return (Time) -1;
}
if (chain->nearest_time <= cur_time) {
logD (timers, _func, ": now");
return 0;
}
logD (timers, _func, ": nearest: ", chain->nearest_time, ", cur: ", cur_time, ", delta: ", chain->nearest_time - cur_time);
return chain->nearest_time - cur_time;
}
/// Returns the time in s since the last call to reset or since
/// the Clock was created.
btScalar btClock::getTimeSeconds()
{
static const btScalar microseconds_to_seconds = btScalar(0.000001);
return btScalar(getTimeMicroseconds()) * microseconds_to_seconds;
}
double b3Clock::getTimeInSeconds()
{
return double(getTimeMicroseconds()/1.e6);
}
mt_throws Result
PollPollGroup::poll (Uint64 const timeout_microsec)
{
Time const start_microsec = getTimeMicroseconds ();
Result ret_res = Result::Success;
SelectedList selected_list;
for (;;) {
selected_list.clear ();
Count cur_num_pollables = 1;
struct pollfd pollfds [num_pollables];
pollfds [0].fd = trigger_pipe [0];
pollfds [0].events = POLLIN;
{
mutex.lock ();
PollableList::iter iter (pollable_list);
while (!pollable_list.iter_done (iter)) {
PollableEntry * const pollable_entry = pollable_list.iter_next (iter);
selected_list.append (pollable_entry);
pollfds [cur_num_pollables].fd = pollable_entry->fd;
pollfds [cur_num_pollables].events =
#ifdef __linux__
POLLRDHUP;
#else
0;
#endif
if (pollable_entry->need_input)
pollfds [cur_num_pollables].events |= POLLIN;
if (pollable_entry->need_output)
pollfds [cur_num_pollables].events |= POLLOUT;
pollable_entry->ref ();
++cur_num_pollables;
}
assert (cur_num_pollables == num_pollables + 1);
mutex.unlock ();
}
Time elapsed_microsec;
int nfds;
{
Time cur_microsec = getTimeMicroseconds ();
if (cur_microsec < start_microsec)
cur_microsec = start_microsec;
elapsed_microsec = cur_microsec - start_microsec;
int timeout;
if (!got_deferred_tasks) {
if (timeout_microsec != (Uint64) -1) {
if (timeout_microsec > elapsed_microsec) {
Uint64 const tmp_timeout = (timeout_microsec - elapsed_microsec) / 1000;
timeout = (int) tmp_timeout;
if ((Uint64) timeout != tmp_timeout || timeout < 0)
timeout = Int_Max - 1;
if (timeout == 0)
timeout = 1;
} else {
timeout = 0;
}
} else {
timeout = -1;
}
} else {
timeout = 0;
}
mutex.lock ();
if (triggered || timeout == 0) {
block_trigger_pipe = true;
timeout = 0;
} else {
block_trigger_pipe = false;
}
mutex.unlock ();
nfds = ::poll (pollfds, cur_num_pollables, timeout);
if (nfds == -1) {
if (errno == EINTR) {
SelectedList::iter iter (selected_list);
mutex.lock ();
while (!selected_list.iter_done (iter)) {
PollableEntry * const pollable_entry = selected_list.iter_next (iter);
pollable_entry->unref ();
}
mutex.unlock ();
// Re-initializing pollfds.
continue;
}
exc_throw (PosixException, errno);
exc_push (InternalException, InternalException::BackendError);
logE_ (_func, "poll() failed: ", errnoString (errno));
ret_res = Result::Failure;
goto _poll_interrupted;
} else
if (nfds < 0) {
logE_ (_func, "unexpected return value from poll(): ", nfds);
exc_throw (InternalException, InternalException::BackendMalfunction);
ret_res = Result::Failure;
goto _poll_interrupted;
}
}
mutex.lock ();
block_trigger_pipe = true;
bool const was_triggered = triggered;
triggered = false;
mutex.unlock ();
got_deferred_tasks = false;
if (frontend)
frontend.call (frontend->pollIterationBegin);
bool trigger_pipe_ready = false;
{
if (pollfds [0].revents & (POLLIN | POLLERR | POLLHUP
#ifdef __linux__
| POLLRDHUP
#endif
))
trigger_pipe_ready = true;
mutex.lock ();
SelectedList::iter iter (selected_list);
Count i = 1;
while (!selected_list.iter_done (iter)) {
PollableEntry * const pollable_entry = selected_list.iter_next (iter);
if (nfds > 0 &&
pollable_entry->valid)
{
Uint32 event_flags = 0;
if (pollfds [i].revents & POLLNVAL) {
logW_ (_func, "POLLNVAL for pollable_entry "
"0x", fmt_hex, (UintPtr) pollable_entry, ", "
"fd ", pollable_entry->fd, " (", pollfds [i].fd, ")");
++i;
continue;
}
if (pollfds [i].revents & POLLIN) {
pollable_entry->need_input = false;
event_flags |= PollGroup::Input;
}
if (pollfds [i].revents & POLLOUT) {
pollable_entry->need_output = false;
event_flags |= PollGroup::Output;
}
if (pollfds [i].revents & POLLHUP
#ifdef __linux__
|| pollfds [i].revents & POLLRDHUP
#endif
)
{
event_flags |= PollGroup::Hup;
}
if (pollfds [i].revents & POLLERR) {
event_flags |= PollGroup::Error;
}
if (event_flags) {
mutex.unlock ();
pollable_entry->pollable.call (pollable_entry->pollable->processEvents, /*(*/ event_flags /*)*/);
mutex.lock ();
}
}
pollable_entry->unref ();
++i;
}
assert (i == cur_num_pollables);
mutex.unlock ();
}
if (frontend) {
bool extra_iteration_needed = false;
frontend.call_ret (&extra_iteration_needed, frontend->pollIterationEnd);
if (extra_iteration_needed)
got_deferred_tasks = true;
}
if (trigger_pipe_ready) {
if (!commonTriggerPipeRead (trigger_pipe [0])) {
logF_ (_func, "commonTriggerPipeRead() failed: ", exc->toString());
return Result::Failure;
}
break;
}
if (was_triggered)
break;
if (elapsed_microsec >= timeout_microsec) {
// Timeout expired.
break;
}
}
return ret_res;
_poll_interrupted:
SelectedList::iter iter (selected_list);
mutex.lock ();
while (!selected_list.iter_done (iter)) {
PollableEntry * const pollable_entry = selected_list.iter_next (iter);
pollable_entry->unref ();
}
mutex.unlock ();
return ret_res;
}
float Time::getTimeSecFloat(const std::string &msg) {
return getTimeMicroseconds(msg) / static_cast<float>(1e6);
}
unsigned int Time::getTimeSec(const std::string &msg) {
return (unsigned int)(getTimeMicroseconds(msg) / 1e6);
}
Timers::TimerKey
Timers::addTimer_microseconds (CbDesc<TimerCallback> const &cb,
Time const time_microseconds,
bool const periodical,
bool const auto_delete,
bool const delete_after_tick)
{
logD (timers, _func, "time_microseconds: ", time_microseconds);
Timer * const timer = new (std::nothrow) Timer (this, cb);
assert (timer);
timer->periodical = periodical;
timer->delete_after_tick = delete_after_tick;
timer->due_time = getTimeMicroseconds() + time_microseconds;
if (timer->due_time < time_microseconds) {
logW_ (_func, "Expiration time overflow");
timer->due_time = (Time) -1;
}
if (auto_delete && cb.coderef_container) {
timer->del_sbn = cb.coderef_container->addDeletionCallback (
CbDesc<Object::DeletionCallback> (subscriberDeletionCallback,
timer,
getCoderefContainer()));
}
logD (timers, _func, "getTimeMicroseconds(): ", getTimeMicroseconds(), ", due_time: ", timer->due_time);
mutex.lock ();
bool first_timer = false;
TimerChain *chain = interval_tree.lookup (time_microseconds);
if (!chain) {
chain = new (std::nothrow) TimerChain;
assert (chain);
chain->interval_microseconds = time_microseconds;
chain->nearest_time = timer->due_time;
if (expiration_tree.isEmpty()
|| expiration_tree_leftmost->nearest_time > chain->nearest_time)
{
first_timer = true;
}
interval_tree.add (chain);
expiration_tree.add (chain);
expiration_tree_leftmost = expiration_tree.getLeftmost();
}
timer->chain = chain;
chain->timer_list.append (timer);
mutex.unlock ();
if (first_timer) {
logD (timers, _func, "calling first_added_cb()");
first_added_cb.call_ ();
}
return timer;
}
void
Timers::processTimers ()
{
Time const cur_time = getTimeMicroseconds ();
mutex.lock ();
TimerChain *chain = expiration_tree_leftmost;
if (chain == NULL) {
mutex.unlock ();
return;
}
while (!chain->timer_list.isEmpty () && chain->nearest_time <= cur_time) {
Time const cur_nearest_time = chain->nearest_time;
logD (timers, _func, "cur_nearest_time: ", cur_nearest_time, ", cur_time: ", cur_time);
Timer * const timer = chain->timer_list.getFirst ();
assert (timer->active);
chain->timer_list.remove (timer);
bool delete_timer = false;
if (timer->periodical) {
timer->due_time += chain->interval_microseconds;
if (timer->due_time < chain->interval_microseconds) {
logW_ (_func, "Expiration time overflow");
timer->due_time = (Time) -1;
}
chain->timer_list.append (timer);
} else {
timer->active = false;
if (timer->delete_after_tick) {
if (timer->del_sbn) {
// TODO We create CodeRef twice: here and in call_unlocks_mutex_().
// Should do this only once for efficiency.
CodeRef const code_ref = timer->timer_cb.getWeakCodeRef();
// If 'code_ref' is null, then doDeleteTimer() will be called
// by subscriberDeletionCallback(), which is likely just about
// to be called.
if (code_ref) {
code_ref->removeDeletionCallback (timer->del_sbn);
delete_timer = true;
}
} else {
delete_timer = true;
}
}
}
bool delete_chain;
if (chain->timer_list.isEmpty ()) {
expiration_tree.remove (chain);
expiration_tree_leftmost = expiration_tree.getLeftmost();
interval_tree.remove (chain);
delete_chain = true;
} else {
if (cur_nearest_time != chain->timer_list.getFirst ()->due_time) {
expiration_tree.remove (chain);
chain->nearest_time = chain->timer_list.getFirst ()->due_time;
expiration_tree.add (chain);
expiration_tree_leftmost = expiration_tree.getLeftmost();
}
delete_chain = false;
}
timer->timer_cb.call_unlocks_mutex_ (mutex);
// 'timer' might have been deleted by the user and should not be used
// directly anymore.
//
// We can't delete the timer ourselves here for a similar reason: its
// lifetime is controlled by the user, so we can't tie it to callback's
// weak_obj.
if (delete_timer)
delete timer;
if (delete_chain)
delete chain;
mutex.lock ();
// 'chain' might have been deleted due to user's actions in 'timer_cb' callback.
chain = expiration_tree_leftmost;
if (chain == NULL) {
mutex.unlock ();
return;
}
}
mutex.unlock ();
}
unsigned long int btClock::getTimeMilliseconds()
{
return getTimeMicroseconds() * 1000;
}
