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;
@@ -168,6 +176,10 @@ namespace RTT
         return true;
     }
 
+    void Activity::timedOut() {
+        mtimeout = true;
+    }
+
     void Activity::loop() {
         nsecs wakeup = 0;
         int overruns = 0;

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);
@@ -203,6 +205,11 @@ namespace RTT
          */
         virtual void step();
 
+        /**
+         * @see os::Thread::timedOut()
+         */
+        void timedOut() override;
+
         /**
          * @see base::RunnableInterface::loop()
          */

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,18 @@ 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 {
+                                int ret = rtos_sem_wait_timed(&(task->sem), task->aperiodicTriggerTimeout); // wait for trigger.
+                                bool timed_out = (ret < 0) && (errno == ETIMEDOUT);
+                                if (timed_out) {
+                                    task->timedOut();
+                                }
+                            }
+
                             task->configure();           // check for reconfigure
                             if (task->prepareForExit)    // check for exit
                             {
@@ -242,6 +253,7 @@ namespace RTT {
 #ifdef OROPKG_OS_THREAD_SCOPE
         ,d(NULL)
 #endif
+                    , aperiodicTriggerTimeout(0)
                     , stopTimeout(0)
         {
             this->setup(_priority, cpu_affinity, name);
@@ -430,15 +442,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);
@@ -511,6 +523,10 @@ namespace RTT {
         {
         }
 
+        void Thread::timedOut()
+        {
+        }
+
         void Thread::loop()
         {
             this->step();
@@ -650,9 +666,15 @@ 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;
+            rtos_sem_signal(&sem);
+            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
@@ -257,6 +267,8 @@ namespace RTT
              */
             virtual void step();
 
+            virtual void timedOut();
+
             /**
              * @see base::RunnableInterface::loop()
              */
@@ -351,6 +363,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);