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();
}
예제 #2
0
파일: timers.cpp 프로젝트: LXiong/OpenNVR
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 ();
}
예제 #3
0
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());
}
예제 #4
0
파일: timers.cpp 프로젝트: LXiong/OpenNVR
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;
}
예제 #5
0
/// 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;
}
예제 #6
0
double b3Clock::getTimeInSeconds()
{
	return double(getTimeMicroseconds()/1.e6);
}
예제 #7
0
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;
}
예제 #8
0
float Time::getTimeSecFloat(const std::string &msg) {
    return getTimeMicroseconds(msg) / static_cast<float>(1e6);
}
예제 #9
0
unsigned int Time::getTimeSec(const std::string &msg) {
    return (unsigned int)(getTimeMicroseconds(msg) / 1e6);
}
예제 #10
0
파일: timers.cpp 프로젝트: LXiong/OpenNVR
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;
}
예제 #11
0
파일: timers.cpp 프로젝트: LXiong/OpenNVR
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 ();
}
예제 #12
0
unsigned long int btClock::getTimeMilliseconds()
{
	return getTimeMicroseconds() * 1000;
}