Skip to content

Commit

Permalink
Merge pull request #2 from Intermodalics/fix/activity_sleep_oro_wait_…
Browse files Browse the repository at this point in the history 
…policies_toolchain-2.9

Fix Activity default wait period policy and behavior of ORO_WAIT_REL in rtt 2.9
  • Loading branch information
@meyerj
meyerj authored Jan 16, 2018
2 parents afe081a + 00caecb commit 09f5543
Show file tree
Hide file tree
Showing 5 changed files with 125 additions and 38 deletions.
32 changes: 17 additions & 15 deletions rtt/Activity.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,19 +55,19 @@ namespace RTT

Activity::Activity(RunnableInterface* _r, const std::string& name )
: ActivityInterface(_r), os::Thread(ORO_SCHED_OTHER, RTT::os::LowestPriority, 0.0, 0, name ),
update_period(0.0), mtimeout(false), mstopRequested(false), mabswaitpolicy(false)
update_period(0.0), mtimeout(false), mstopRequested(false), mwaitpolicy(ORO_WAIT_ABS)
{
}

Activity::Activity(int priority, RunnableInterface* r, const std::string& name )
: ActivityInterface(r), os::Thread(ORO_SCHED_RT, priority, 0.0, 0, name ),
update_period(0.0), mtimeout(false), mstopRequested(false), mabswaitpolicy(false)
update_period(0.0), mtimeout(false), mstopRequested(false), mwaitpolicy(ORO_WAIT_ABS)
{
}

Activity::Activity(int priority, Seconds period, RunnableInterface* r, const std::string& name )
: ActivityInterface(r), os::Thread(ORO_SCHED_RT, priority, period, 0, name ),
update_period(period), mtimeout(false), mstopRequested(false), mabswaitpolicy(false)
update_period(period), mtimeout(false), mstopRequested(false), mwaitpolicy(ORO_WAIT_ABS)
{
// We pass the requested period to the constructor to not confuse users with log messages.
// Then we clear it immediately again in order to force the Thread implementation to
Expand All @@ -77,13 +77,13 @@ namespace RTT

Activity::Activity(int scheduler, int priority, RunnableInterface* r, const std::string& name )
: ActivityInterface(r), os::Thread(scheduler, priority, 0.0, 0, name ),
update_period(0.0), mtimeout(false), mstopRequested(false), mabswaitpolicy(false)
update_period(0.0), mtimeout(false), mstopRequested(false), mwaitpolicy(ORO_WAIT_ABS)
{
}

Activity::Activity(int scheduler, int priority, Seconds period, RunnableInterface* r, const std::string& name )
: ActivityInterface(r), os::Thread(scheduler, priority, period, 0, name ),
update_period(period), mtimeout(false), mstopRequested(false), mabswaitpolicy(false)
update_period(period), mtimeout(false), mstopRequested(false), mwaitpolicy(ORO_WAIT_ABS)
{
// We pass the requested period to the constructor to not confuse users with log messages.
// Then we clear it immediately again in order to force the Thread implementation to
Expand All @@ -93,7 +93,7 @@ namespace RTT

Activity::Activity(int scheduler, int priority, Seconds period, unsigned cpu_affinity, RunnableInterface* r, const std::string& name )
: ActivityInterface(r), os::Thread(scheduler, priority, period, cpu_affinity, name ),
update_period(period), mtimeout(false), mstopRequested(false), mabswaitpolicy(false)
update_period(period), mtimeout(false), mstopRequested(false), mwaitpolicy(ORO_WAIT_ABS)
{
// We pass the requested period to the constructor to not confuse users with log messages.
// Then we clear it immediately again in order to force the Thread implementation to
Expand Down Expand Up @@ -213,14 +213,17 @@ namespace RTT
return;
} else {
// If periodic, sleep until wakeup time or a message comes in.
// when wakeup time passed, wait_until will return false and we recalculate wakeup + mtimeout
// when wakeup time passed, wait_until will return false and we recalculate wakeup + update_period
bool time_elapsed = ! msg_cond.wait_until(msg_lock,wakeup);

if (time_elapsed) {
nsecs now = os::TimeService::Instance()->getNSecs();

// calculate next wakeup point
nsecs nsperiod = Seconds_to_nsecs(update_period);
wakeup = wakeup + nsperiod;
// calculate next wakeup point, overruns causes skips:
nsecs now = os::TimeService::Instance()->getNSecs();

// detect overruns
if ( wakeup < now )
{
++overruns;
Expand All @@ -230,8 +233,10 @@ namespace RTT
else if (overruns != 0) {
--overruns;
}
if ( !mabswaitpolicy && wakeup < now ) {
wakeup = wakeup + ((now-wakeup)/nsperiod+1)*nsperiod; // assumes that (now-wakeup)/nsperiod rounds down !

// ORO_WAIT_REL: reset next wakeup time to now (before step) + period
if ( mwaitpolicy == ORO_WAIT_REL ) {
wakeup = now + nsperiod;
}
mtimeout = true;
}
Expand Down Expand Up @@ -342,10 +347,7 @@ namespace RTT

void Activity::setWaitPeriodPolicy(int p)
{
if ( p == ORO_WAIT_ABS)
mabswaitpolicy = true;
else
mabswaitpolicy = false;
mwaitpolicy = p;
}

}
2 changes: 1 addition & 1 deletion rtt/Activity.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -232,7 +232,7 @@ namespace RTT
*/
bool mtimeout;
bool mstopRequested;
bool mabswaitpolicy;
int mwaitpolicy;
};

}
Expand Down
4 changes: 2 additions & 2 deletions rtt/os/ecos/fosi.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,8 @@ extern "C"
#define ORO_SCHED_RT 0
#define ORO_SCHED_OTHER 0

#define ORO_WAIT_ABS 0 /** Not supported for the ecos target */
#define ORO_WAIT_REL 1 /** Not supported for the ecos target */
#define ORO_WAIT_ABS 0
#define ORO_WAIT_REL 1

typedef long long NANO_TIME;
typedef cyg_tick_count_t TICK_TIME;
Expand Down
4 changes: 2 additions & 2 deletions rtt/os/xenomai/fosi.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,8 +119,8 @@ extern "C" {
#define ORO_SCHED_RT 0 /** Hard real-time */
#define ORO_SCHED_OTHER 1 /** Soft real-time */

#define ORO_WAIT_ABS 0 /** Not supported for the xenomai target */
#define ORO_WAIT_REL 1 /** Not supported for the xenomai target */
#define ORO_WAIT_ABS 0
#define ORO_WAIT_REL 1

// hrt is in ticks
static inline TIME_SPEC ticks2timespec(TICK_TIME hrt)
Expand Down
121 changes: 103 additions & 18 deletions tests/tasks_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,9 @@
#include "tasks_test.hpp"

#include <cstdlib>
#include <cstring>
#include <iostream>
#include <vector>

#include <extras/PeriodicActivity.hpp>
#include <os/TimeService.hpp>
Expand All @@ -35,23 +37,49 @@ using namespace boost;
using namespace RTT;
using namespace RTT::detail;

#define BOOST_CHECK_EQUAL_MESSAGE(M, v1, v2) BOOST_CHECK_MESSAGE( v1==v2, M)
#define BOOST_CHECK_EQUAL_MESSAGE(v1, v2, M) BOOST_CHECK_MESSAGE( v1==v2, M)
#define BOOST_REQUIRE_EQUAL_MESSAGE(v1, v2, M) BOOST_REQUIRE_MESSAGE( v1==v2, M)

struct TestOverrun
: public RunnableInterface
{
nsecs sleep_time;
bool fini;
bool initialize() { fini = false; return true; }
std::vector<nsecs> wakeup_time; // record wakeup times
std::vector<nsecs> additional_sleep_time; // sleep ... additional nsecs in cycle i
unsigned int cycle;

TestOverrun()
: sleep_time(Seconds_to_nsecs(0.2)), fini(false), wakeup_time(3), additional_sleep_time(), cycle(0)
{
}

bool initialize() {
sleep_time = Seconds_to_nsecs(0.2);
fini = false;
wakeup_time.assign(wakeup_time.size(), 0);
cycle = 0;
return true;
}

void step() {
//requires that getPeriod() << 1
usleep(200*1000);
if (cycle < wakeup_time.size())
{
wakeup_time[cycle] = os::TimeService::Instance()->getNSecs();
}
if (cycle < additional_sleep_time.size() && additional_sleep_time[cycle] > 0) {
usleep(additional_sleep_time[cycle]/1000);
}
++cycle;

//requires that getPeriod() << sleep_time
usleep(sleep_time/1000);
// Tried to implement it like this for Xenomai, but the
// underlying rt_task_sleep function always returns immediately
// and returns zero (success). A plain usleep still works.
#if 0
TIME_SPEC timevl;
timevl = ticks2timespec( nano2ticks(200*1000*1000) );
timevl = ticks2timespec( nano2ticks(sleep_time) );
rtos_nanosleep( &timevl, 0);
#endif
}
Expand Down Expand Up @@ -270,15 +298,13 @@ BOOST_AUTO_TEST_CASE( testFailInit )

}

#if !defined( OROCOS_TARGET_WIN32 ) && !defined(OROCOS_TARGET_LXRT)
BOOST_AUTO_TEST_CASE( testOverrun )
BOOST_AUTO_TEST_CASE( testMaxOverrun )
{
bool r = false;
// create
boost::scoped_ptr<TestOverrun> run( new TestOverrun() );
boost::scoped_ptr<Activity> t( new Activity(25, 0.1, 0,"ORThread") );
//BOOST_CHECK_EQUAL(25,t->getPriority() );
BOOST_CHECK_EQUAL(0.1,t->getPeriod() );
BOOST_REQUIRE_EQUAL(0.1,t->getPeriod() );

t->thread()->setMaxOverrun(1);

t->run( run.get() );
Expand All @@ -294,16 +320,76 @@ BOOST_AUTO_TEST_CASE( testOverrun )
usleep(400*1000);
Logger::log().setLogLevel(ll);

r = !t->isRunning();
BOOST_REQUIRE_MESSAGE( !t->isRunning(), "Failed to detect step overrun in Thread");
BOOST_CHECK_MESSAGE( run->fini, "Failed to execute finalize in emergencyStop" );
}

static const nsecs WaitPeriodPolicyTolerance = Seconds_to_nsecs(0.01);

t->run(0);
BOOST_AUTO_TEST_CASE( testDefaultWaitPeriodPolicy )
{
// create
boost::scoped_ptr<TestOverrun> run( new TestOverrun() );
boost::scoped_ptr<Activity> t( new Activity(25, 0.3, 0,"ORThread") );
t->run( run.get() );
run->additional_sleep_time.resize(1);
run->additional_sleep_time[0] = Seconds_to_nsecs(0.2);
nsecs period = Seconds_to_nsecs(t->getPeriod()); (void) period;

BOOST_REQUIRE_MESSAGE( r, "Failed to detect step overrun in Thread");
// test default wait period policy (== ORO_WAIT_ABS)
BOOST_REQUIRE_MESSAGE( t->start(), "start thread");
// In Xenomai (2.5), the first usleep returns immediately.
// We can 'fix' this by adding a log() statement before usleep() .... crap
usleep(100*1000);
usleep(1000*1000);
BOOST_REQUIRE_MESSAGE( t->stop(), "stop thread");
BOOST_CHECK_SMALL( ((run->wakeup_time[1] - run->wakeup_time[0]) - Seconds_to_nsecs(0.4)), WaitPeriodPolicyTolerance ); // Second wakeup: 2 * sleep_time
BOOST_CHECK_SMALL( ((run->wakeup_time[2] - run->wakeup_time[1]) - Seconds_to_nsecs(0.2)), WaitPeriodPolicyTolerance ); // Third wakeup right after end of second step (ORO_WAIT_ABS)
}

BOOST_CHECK_MESSAGE( run->fini, "Failed to execute finalize in emergencyStop" );
BOOST_AUTO_TEST_CASE( testAbsoluteWaitPeriodPolicy )
{
// create
boost::scoped_ptr<TestOverrun> run( new TestOverrun() );
boost::scoped_ptr<Activity> t( new Activity(25, 0.3, 0,"ORThread") );
t->run( run.get() );
run->additional_sleep_time.resize(1);
run->additional_sleep_time[0] = Seconds_to_nsecs(0.2);
nsecs period = Seconds_to_nsecs(t->getPeriod()); (void) period;

// test absolute wait period policy (ORO_WAIT_ABS)
t->setWaitPeriodPolicy(ORO_WAIT_ABS);
BOOST_REQUIRE_MESSAGE( t->start(), "start thread");
// In Xenomai (2.5), the first usleep returns immediately.
// We can 'fix' this by adding a log() statement before usleep() .... crap
usleep(100*1000);
usleep(1000*1000);
BOOST_REQUIRE_MESSAGE( t->stop(), "stop thread");
BOOST_CHECK_SMALL( ((run->wakeup_time[1] - run->wakeup_time[0]) - Seconds_to_nsecs(0.4)), WaitPeriodPolicyTolerance ); // Second wakeup: 2 * sleep_time (overrun)
BOOST_CHECK_SMALL( ((run->wakeup_time[2] - run->wakeup_time[1]) - Seconds_to_nsecs(0.2)), WaitPeriodPolicyTolerance ); // Third wakeup right after end of second step (ORO_WAIT_ABS)
}

BOOST_AUTO_TEST_CASE( testRelativeWaitPeriodPolicy )
{
// create
boost::scoped_ptr<TestOverrun> run( new TestOverrun() );
boost::scoped_ptr<Activity> t( new Activity(25, 0.3, 0,"ORThread") );
t->run( run.get() );
run->additional_sleep_time.resize(1);
run->additional_sleep_time[0] = Seconds_to_nsecs(0.2);
nsecs period = Seconds_to_nsecs(t->getPeriod()); (void) period;

// test relative wait period policy (ORO_WAIT_REL)
t->setWaitPeriodPolicy(ORO_WAIT_REL);
BOOST_REQUIRE_MESSAGE( t->start(), "start thread");
// In Xenomai (2.5), the first usleep returns immediately.
// We can 'fix' this by adding a log() statement before usleep() .... crap
usleep(100*1000);
usleep(1000*1000);
BOOST_REQUIRE_MESSAGE( t->stop(), "stop thread");
BOOST_CHECK_SMALL( ((run->wakeup_time[1] - run->wakeup_time[0]) - Seconds_to_nsecs(0.4)), WaitPeriodPolicyTolerance ); // Second wakeup: 2 * sleep_time (overrun)
BOOST_CHECK_SMALL( ((run->wakeup_time[2] - run->wakeup_time[1]) - period), WaitPeriodPolicyTolerance ); // Third wakeup after period (ORO_WAIT_REL)
}
#endif

BOOST_AUTO_TEST_CASE( testThread )
{
Expand All @@ -320,9 +406,8 @@ BOOST_AUTO_TEST_CASE( testThread )
r = t->stop();
BOOST_CHECK_MESSAGE( r, "Failed to stop Thread" );
BOOST_CHECK_MESSAGE( run->stepped == true, "Step not executed" );
BOOST_CHECK_EQUAL_MESSAGE("Periodic Failure: period of step() too long !", run->overfail, 0);
BOOST_CHECK_EQUAL_MESSAGE("Periodic Failure: period of step() too short!", run->underfail, 0);
t->run(0);
BOOST_CHECK_EQUAL_MESSAGE(run->overfail, 0, "Periodic Failure: period of step() too long !");
BOOST_CHECK_EQUAL_MESSAGE(run->underfail, 0, "Periodic Failure: period of step() too short!");
}

#if defined( OROCOS_TARGET_GNULINUX )
Expand Down

0 comments on commit 09f5543

Please sign in to comment.