source: rrlib_thread/tLoopThread.cpp @ 15:d0e7b92ad22f

Last change on this file since 15:d0e7b92ad22f was 15:d0e7b92ad22f, checked in by Tobias Föhst <foehst@…>, 6 years ago

Added and updated license information

File size: 5.8 KB
Line 
1//
2// You received this file as part of RRLib
3// Robotics Research Library
4//
5// Copyright (C) Finroc GbR (finroc.org)
6//
7// This program is free software; you can redistribute it and/or modify
8// it under the terms of the GNU General Public License as published by
9// the Free Software Foundation; either version 2 of the License, or
10// (at your option) any later version.
11//
12// This program is distributed in the hope that it will be useful,
13// but WITHOUT ANY WARRANTY; without even the implied warranty of
14// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15// GNU General Public License for more details.
16//
17// You should have received a copy of the GNU General Public License along
18// with this program; if not, write to the Free Software Foundation, Inc.,
19// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
20//
21//----------------------------------------------------------------------
22/*!\file    rrlib/thread/tLoopThread.cpp
23 *
24 * \author  Max Reichardt
25 *
26 * \date    2012-07-05
27 *
28 */
29//----------------------------------------------------------------------
30#include "rrlib/thread/tLoopThread.h"
31
32//----------------------------------------------------------------------
33// External includes (system with <>, local with "")
34//----------------------------------------------------------------------
35
36//----------------------------------------------------------------------
37// Internal includes with ""
38//----------------------------------------------------------------------
39
40//----------------------------------------------------------------------
41// Debugging
42//----------------------------------------------------------------------
43#include <cassert>
44
45//----------------------------------------------------------------------
46// Namespace usage
47//----------------------------------------------------------------------
48
49//----------------------------------------------------------------------
50// Namespace declaration
51//----------------------------------------------------------------------
52namespace rrlib
53{
54namespace thread
55{
56
57//----------------------------------------------------------------------
58// Forward declarations / typedefs / enums
59//----------------------------------------------------------------------
60
61//----------------------------------------------------------------------
62// Const values
63//----------------------------------------------------------------------
64const bool tLoopThread::cDISPLAYWARNINGS;
65
66//----------------------------------------------------------------------
67// Implementation
68//----------------------------------------------------------------------
69
70tLoopThread::tLoopThread(rrlib::time::tDuration default_cycle_time, bool use_application_time, bool warn_on_cycle_time_exceed, bool pause_on_startup) :
71  pause_signal(pause_on_startup),
72  cycle_time(default_cycle_time),
73  use_application_time(use_application_time),
74  warn_on_cycle_time_exceed(warn_on_cycle_time_exceed),
75  last_cycle_time(),
76  last_cycle_start(rrlib::time::cNO_TIME),
77  last_wait(rrlib::time::tDuration::zero())
78{
79}
80
81void tLoopThread::ContinueThread()
82{
83  tLock l(*this);
84  pause_signal = false;
85  GetMonitor().Notify(l);
86}
87
88void tLoopThread::MainLoop()
89{
90  while (!IsStopSignalSet())
91  {
92    if (pause_signal.load(std::memory_order_relaxed))
93    {
94      last_cycle_start = rrlib::time::cNO_TIME;
95      tLock l(*this);
96      GetMonitor().Wait(l);
97      continue;
98    }
99
100    if (last_cycle_start != rrlib::time::cNO_TIME)
101    {
102      // copy atomics to local variables
103      rrlib::time::tDuration cycle_time = this->cycle_time.Load();
104      bool local_use_application_time = use_application_time.load(std::memory_order_relaxed);
105
106      // wait
107      rrlib::time::tTimestamp now = rrlib::time::Now();
108      rrlib::time::tDuration last_cycle_time_tmp = now - last_cycle_start;
109      last_cycle_time.Store(last_cycle_time_tmp);
110      rrlib::time::tDuration wait_for_x = cycle_time - last_cycle_time_tmp;
111      if (wait_for_x < rrlib::time::tDuration::zero() && warn_on_cycle_time_exceed && cDISPLAYWARNINGS)
112      {
113        //System.err.println("warning: Couldn't keep up cycle time (" + (-waitForX) + " ms too long)");
114        RRLIB_LOG_PRINT(WARNING, "Couldn't keep up cycle time (", rrlib::time::ToString(-wait_for_x), " too long)");
115      }
116      else if (wait_for_x > cycle_time)
117      {
118        RRLIB_LOG_PRINT(WARNING, "Clock inconsistency detected: Last cycle started \"after\" this cycle. This would mean we'd have to wait for ", rrlib::time::ToString(wait_for_x), " now.");
119        if (last_wait > rrlib::time::tDuration::zero())
120        {
121          RRLIB_LOG_PRINT(WARNING, "Waiting for ", rrlib::time::ToString(last_wait), ", as in last cycle, instead.");
122          Sleep(last_wait, local_use_application_time);
123        }
124        else
125        {
126          RRLIB_LOG_PRINT(WARNING, "Not waiting at all. As it appears, this thread has never waited yet.");
127        }
128      }
129      else if (wait_for_x > rrlib::time::tDuration::zero())
130      {
131        last_wait = wait_for_x;
132        Sleep(wait_for_x, local_use_application_time, last_cycle_start + cycle_time);
133      }
134      last_cycle_start += cycle_time;
135      if (wait_for_x < rrlib::time::tDuration::zero())
136      {
137        last_cycle_start = rrlib::time::Now();
138      }
139    }
140    else
141    {
142      last_cycle_start = rrlib::time::Now();
143    }
144
145    MainLoopCallback();
146  }
147}
148
149void tLoopThread::Run()
150{
151  // Start main loop
152  MainLoop();
153}
154
155void tLoopThread::SetUseApplicationTime(bool use_application_time)
156{
157  //assert(&CurrentThread() == this && "Please only call from this thread");
158  bool last_value = this->use_application_time.exchange(use_application_time);
159  if (last_value != use_application_time)
160  {
161    last_cycle_start = rrlib::time::cNO_TIME;
162  }
163}
164
165
166//----------------------------------------------------------------------
167// End of namespace declaration
168//----------------------------------------------------------------------
169}
170}
Note: See TracBrowser for help on using the repository browser.