feat: implement trigger timeout on aperiodic tasks

rtt/Activity.cpp

@@ -143,6 +143,14 @@ namespace RTT
         return update_period;
     }
 
+    bool Activity::setAperiodicTriggerTimeout(NANO_TIME timeout) {
+        if (timeout < 0.0)
+            return false;
+
+        Thread::setAperiodicTriggerTimeout(timeout);
+        return true;
+    }
+
     bool Activity::trigger() {
         if ( ! Thread::isActive() )
             return false;

rtt/Activity.hpp

@@ -185,6 +185,8 @@ namespace RTT
 
         virtual bool setPeriod(Seconds period);
 
+        virtual bool setAperiodicTriggerTimeout(NANO_TIME timeout) override;
+
         virtual unsigned getCpuAffinity() const;
 
         virtual bool setCpuAffinity(unsigned cpu);

rtt/base/ActivityInterface.hpp

@@ -161,6 +161,17 @@ namespace RTT
          */
         virtual bool setPeriod(Seconds s)  = 0;
 
+        /**
+         * Set a trigger timeout for aperiodic activities.
+         *
+         * When set, an aperiodic activity will be triggered after this long.
+         * Note that this is not the same as a periodic task: the period is not
+         * fixed (the timeout starts at the last time the thread started waiting)
+         * and on at least Linux, it is not using a monotonic clock.
+         *
+         * @return true if it could be updated, false otherwise.
+         */
+        virtual bool setAperiodicTriggerTimeout(NANO_TIME timeout)  = 0;
 
         /**
          * Get the cpu affinity of this activity

rtt/extras/FileDescriptorActivity.cpp

@@ -189,6 +189,12 @@ void FileDescriptorActivity::setTimeout_us(int timeout_us)
         log(Error) << "Ignoring invalid timeout (" << timeout_us << ")" << endlog();
     }
 }
+
+bool FileDescriptorActivity::setAperiodicTriggerTimeout(NANO_TIME timeout) {
+    setTimeout_us(timeout / 1000);
+    return true;
+}
+
 void FileDescriptorActivity::watch(int fd)
 { RTT::os::MutexLock lock(m_lock);
     if (fd < 0)

rtt/extras/FileDescriptorActivity.hpp

@@ -262,6 +262,11 @@ namespace RTT { namespace extras {
          */
         void setTimeout(int timeout);
 
+        /** Same as setTimeout, from the common Activity interface
+         *
+         */
+        bool setAperiodicTriggerTimeout(NANO_TIME timeout) override;
+
         /** Sets the timeout, in microseconds, for waiting on the IO. Set to 0
          * for blocking behaviour (no timeout).
 		 * @pre 0 <= timeout (otherwise an error is logged and \a timeout_us

rtt/extras/FileDescriptorSimulationActivity.hpp

@@ -51,7 +51,7 @@ namespace RTT { namespace extras {
         the component, and similarly for \a work(TimeOut) and \a hasTimeout().
         Currently \a hasError() always returns false - there is no way to
         simulate this with the current implementation.
-     
+
         \note Component OwnThread operations are executed by the MainThread,
         which is correct for a unit test case that is directly executing.
      */
@@ -122,6 +122,9 @@ namespace RTT { namespace extras {
         /// Does nothing
         void setTimeout_us(int timeout_us);
 
+        /// Does nothing
+        bool setAperiodicTriggerTimeout(NANO_TIME) override { return true; };
+
         /// Return 0
         int getTimeout() const;
 

rtt/extras/PeriodicActivity.cpp

@@ -165,6 +165,10 @@ namespace RTT {
         return false;
     }
 
+    bool PeriodicActivity::setAperiodicTriggerTimeout(NANO_TIME timeout) {
+        return false;
+    }
+
     unsigned PeriodicActivity::getCpuAffinity() const
     {
       return thread_->getCpuAffinity();

rtt/extras/PeriodicActivity.hpp

@@ -188,6 +188,8 @@ namespace RTT
 
         virtual bool setPeriod(Seconds s);
 
+        virtual bool setAperiodicTriggerTimeout(NANO_TIME timeout);
+
         virtual unsigned getCpuAffinity() const;
 
         virtual bool setCpuAffinity(unsigned cpu);

rtt/extras/SequentialActivity.cpp

@@ -65,6 +65,10 @@ namespace RTT {
         return false;
     }
 
+    bool SequentialActivity::setAperiodicTriggerTimeout(NANO_TIME timeout) {
+        return (timeout == 0);
+    }
+
     unsigned SequentialActivity::getCpuAffinity() const
     {
       return ~0;

rtt/extras/SequentialActivity.hpp

@@ -87,6 +87,8 @@ namespace RTT
 
         bool setPeriod(Seconds s);
 
+        bool setAperiodicTriggerTimeout(NANO_TIME timeout) override;
+
         unsigned getCpuAffinity() const;
 
         bool setCpuAffinity(unsigned cpu);

rtt/extras/SlaveActivity.cpp

@@ -77,6 +77,10 @@ namespace RTT {
         return true;
     }
 
+    bool SlaveActivity::setAperiodicTriggerTimeout(NANO_TIME timeout) {
+        return (timeout == 0);
+    }
+
     unsigned SlaveActivity::getCpuAffinity() const
     {
         if (mmaster)

rtt/extras/SlaveActivity.hpp

@@ -122,6 +122,8 @@ namespace RTT
 
         bool setPeriod(Seconds s);
 
+        bool setAperiodicTriggerTimeout(NANO_TIME timeout) override;
+
         unsigned getCpuAffinity() const;
 
         bool setCpuAffinity(unsigned cpu);

rtt/os/Thread.cpp

@@ -69,7 +69,7 @@ namespace RTT {
         void Thread::setStackSize(unsigned int ssize) { default_stack_size = ssize; }
 
         void Thread::setLockTimeoutNoPeriod(double timeout_in_s) { lock_timeout_no_period_in_s = timeout_in_s; }
-       
+
         void Thread::setLockTimeoutPeriodFactor(double factor) { lock_timeout_period_factor = factor; }
 
         void *thread_function(void* t)
@@ -112,7 +112,14 @@ namespace RTT {
                                 // drop out of periodic mode:
                                 rtos_task_set_period(task->getTask(), 0);
                             }
-                            rtos_sem_wait(&(task->sem)); // wait for command.
+
+                            if (!task->running || task->period != 0 || task->aperiodicTriggerTimeout == 0) {
+                                rtos_sem_wait(&(task->sem)); // wait for command.
+                            }
+                            else {
+                                rtos_sem_wait_timed(&(task->sem), task->aperiodicTriggerTimeout); // wait for trigger.
+                            }
+
                             task->configure();           // check for reconfigure
                             if (task->prepareForExit)    // check for exit
                             {
@@ -242,6 +249,7 @@ namespace RTT {
 #ifdef OROPKG_OS_THREAD_SCOPE
         ,d(NULL)
 #endif
+                    , aperiodicTriggerTimeout(0)
                     , stopTimeout(0)
         {
             this->setup(_priority, cpu_affinity, name);
@@ -430,15 +438,15 @@ namespace RTT {
                     // breakLoop was ok, wait for loop() to return.
                 }
                 // always take this lock, but after breakLoop was called !
-                MutexTimedLock lock(breaker, getStopTimeout()); 
+                MutexTimedLock lock(breaker, getStopTimeout());
                 if ( !lock.isSuccessful() ) {
                     log(Error) << "Failed to stop thread " << this->getName() << ": breakLoop() returned true, but loop() function did not return after " << getStopTimeout() <<" seconds."<<endlog();
                     running = true;
                     return false;
                 }
             } else {
                 //
-                MutexTimedLock lock(breaker, getStopTimeout() ); 
+                MutexTimedLock lock(breaker, getStopTimeout() );
                 if ( lock.isSuccessful() ) {
                     // drop out of periodic mode.
                     rtos_task_make_periodic(&rtos_task, 0);
@@ -650,9 +658,14 @@ namespace RTT {
 
         void Thread::setWaitPeriodPolicy(int p)
         {
-            rtos_task_set_wait_period_policy(&rtos_task, p);  
+            rtos_task_set_wait_period_policy(&rtos_task, p);
         }
 
+        bool Thread::setAperiodicTriggerTimeout(NANO_TIME timeout)
+        {
+            aperiodicTriggerTimeout = timeout;
+            return true;
+        }
     }
 }
 

rtt/os/Thread.hpp

@@ -139,7 +139,7 @@ namespace RTT
 
             /**
              * Sets the lock timeout for a thread which does not have a period
-             * The default is 1 second 
+             * The default is 1 second
              * @param timeout_in_s the timeout is seconds
              */
             static void setLockTimeoutNoPeriod(double timeout_in_s);
@@ -148,7 +148,7 @@ namespace RTT
              * Set the lock timeout for a thread which has a period
              * by a factor of the period
              * The default is factor 10
-             * @param factor Factor of the period 
+             * @param factor Factor of the period
              */
             static void setLockTimeoutPeriodFactor(double factor);
 
@@ -242,6 +242,16 @@ namespace RTT
 
             virtual void setWaitPeriodPolicy(int p);
 
+            /** Sets a maximum wait time between triggers in aperiodic mode
+             *
+             * This is *not* as precise as a periodic task. It is meant as a way
+             * to ensure that the tasks will be called regularly to check for e.g.
+             * input timeouts internally while in explicit trigger mode (e.g. port driven)
+             *
+             * The boolean return type is here to match the ActivityInterface definition
+             */
+            virtual bool setAperiodicTriggerTimeout(NANO_TIME timeout);
+
         protected:
             /**
              * Exit and destroy the thread
@@ -351,6 +361,11 @@ namespace RTT
              */
             NANO_TIME period;
 
+            /**
+             * When in aperiodic mode, wait for a trigger for at most this long
+             */
+            NANO_TIME aperiodicTriggerTimeout;
+
             /**
              * The timeout, in seconds, for stop()
              */

rtt/os/gnulinux/fosi.h

@@ -82,9 +82,9 @@ extern "C"
     static const NANO_TIME InfiniteNSecs = LLONG_MAX;
     static const double    InfiniteSeconds = DBL_MAX;
 
-#define ORO_WAIT_ABS 0 /** rtos_task_wait_period may wait less than the duration required to pad the period to 
+#define ORO_WAIT_ABS 0 /** rtos_task_wait_period may wait less than the duration required to pad the period to
                             catch-up with overrun timesteps (wait according to an absolute timeline) */
-#define ORO_WAIT_REL 1 /** rtos_task_wait_period will always pad the current timestep to the desired period, 
+#define ORO_WAIT_REL 1 /** rtos_task_wait_period will always pad the current timestep to the desired period,
                             regardless of previous overruns (wait according to a relative timeline) */
 
   typedef struct {
@@ -185,6 +185,23 @@ extern "C"
         return sem_wait(m);
     }
 
+    static inline int rtos_sem_wait_timed(rt_sem_t* m, NANO_TIME delay_ns)
+    {
+        const unsigned long long NSEC_PER_SEC = 1000000000ULL;
+
+        struct timespec ts;
+        clock_gettime(CLOCK_REALTIME, &ts);
+
+        long long delay_s = delay_ns / NSEC_PER_SEC;
+        long time_nsec = ts.tv_nsec + delay_ns % NSEC_PER_SEC;
+        delay_s += time_nsec / NSEC_PER_SEC;
+        time_nsec = time_nsec % NSEC_PER_SEC;
+
+        ts.tv_sec += delay_s;
+        ts.tv_nsec = time_nsec;
+        return sem_timedwait(m, &ts);
+    }
+
     static inline int rtos_sem_trywait(rt_sem_t* m )
     {
         return sem_trywait(m);

tests/tasks_test.cpp

@@ -100,8 +100,11 @@ struct TestPeriodic
     // non-LINUX: 0
     int cpu;
 
-    TestPeriodic()
+    Seconds expectedPeriod;
+
+    TestPeriodic(Seconds expectedPeriod)
             : overfail(0), underfail(0), succ(0), stepped(false), cpu(0)
+            , expectedPeriod(expectedPeriod)
     {
     }
 
@@ -115,11 +118,11 @@ struct TestPeriodic
             stepped = true;
         } else {
             TimeService::Seconds s = TimeService::Instance()->secondsSince( ts );
-            if ( s < this->getThread()->getPeriod() *0.9 ) { // if elapsed time is smaller than 10% of period, something went wrong
+            if ( s < expectedPeriod *0.9 ) { // if elapsed time is smaller than 10% of period, something went wrong
                 ++underfail;
                 //rtos_printf("UnderFailPeriod: %f \n", s);
             }
-            else if ( s > this->getThread()->getPeriod() *1.1 ) { // if elapsed time is smaller than 10% of period, something went wrong
+            else if ( s > expectedPeriod *1.1 ) { // if elapsed time is smaller than 10% of period, something went wrong
                 ++overfail;
                 //rtos_printf("OverFailPeriod: %f \n", s);
             }
@@ -395,17 +398,40 @@ BOOST_AUTO_TEST_CASE( testThread )
 {
   bool r = false;
   // create
-  boost::scoped_ptr<TestPeriodic> run( new TestPeriodic() );
+  boost::scoped_ptr<TestPeriodic> run( new TestPeriodic(0.1) );
 
   boost::scoped_ptr<ActivityInterface> t( new Activity(ORO_SCHED_RT, os::HighestPriority, 0.1, 0, "PThread") );
   t->run( run.get() );
 
   r = t->start();
   BOOST_CHECK_MESSAGE( r, "Failed to start Thread");
-  usleep(1000*100);
+  usleep(1000*1000);
+  r = t->stop();
+  BOOST_CHECK_MESSAGE( r, "Failed to stop Thread" );
+  BOOST_CHECK_MESSAGE( run->stepped == true, "Step not executed" );
+  BOOST_CHECK_MESSAGE(run->succ > 5, "Periodic Failure: less than 5 steps within 1s");
+  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!");
+}
+
+BOOST_AUTO_TEST_CASE( testAperiodicTriggerTimeout )
+{
+  bool r = false;
+  // create
+  boost::scoped_ptr<TestPeriodic> run( new TestPeriodic(0.1) );
+
+  boost::scoped_ptr<ActivityInterface> t( new Activity(ORO_SCHED_RT, os::HighestPriority, 0, 0, "PThread") );
+  t->setAperiodicTriggerTimeout(100 * 1000 * 1000);
+  //t->setAperiodicTriggerTimeout(0);
+  t->run( run.get() );
+
+  r = t->start();
+  BOOST_CHECK_MESSAGE( r, "Failed to start Thread");
+  usleep(1000*1000);
   r = t->stop();
   BOOST_CHECK_MESSAGE( r, "Failed to stop Thread" );
   BOOST_CHECK_MESSAGE( run->stepped == true, "Step not executed" );
+  BOOST_CHECK_MESSAGE(run->succ > 5, "Less than 5 steps within 1s");
   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!");
 }
@@ -447,7 +473,7 @@ BOOST_AUTO_TEST_CASE( testAffinity )
     int numCPU = sysconf( _SC_NPROCESSORS_ONLN );
     if (1 < numCPU)
     {
-        boost::scoped_ptr<TestPeriodic> run( new TestPeriodic() );
+        boost::scoped_ptr<TestPeriodic> run( new TestPeriodic(0.1) );
         boost::scoped_ptr<Activity> t( new Activity(ORO_SCHED_RT, os::HighestPriority, 0.1, ~0, 0, "PThread") );
         // returned affinity depends on the number of actual CPUs, and won't be "~0"
         unsigned    mask=0;