source: rrlib_thread/tLoopThread.cpp @ 2:ab6b230e7da2

Last change on this file since 2:ab6b230e7da2 was 2:ab6b230e7da2, checked in by Max Reichardt <mreichardt@…>, 7 years ago

Fixed issue with gcc 4.5.

File size: 5.6 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
8// modify it under the terms of the GNU General Public License
9// as published by the Free Software Foundation; either version 2
10// of the License, or (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
18// along with this program; if not, write to the Free Software
19// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, 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 (!stop_signal)
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      rrlib::time::tDuration cycle_time = this->cycle_time.Load();
103
104      // wait
105      rrlib::time::tTimestamp now = rrlib::time::Now();
106      rrlib::time::tDuration last_cycle_time_tmp = now - last_cycle_start;
107      last_cycle_time.Store(last_cycle_time_tmp);
108      rrlib::time::tDuration wait_for_x = cycle_time - last_cycle_time_tmp;
109      if (wait_for_x < rrlib::time::tDuration::zero() && warn_on_cycle_time_exceed && cDISPLAYWARNINGS)
110      {
111        //System.err.println("warning: Couldn't keep up cycle time (" + (-waitForX) + " ms too long)");
112        RRLIB_LOG_PRINT(rrlib::logging::eLL_WARNING, "Couldn't keep up cycle time (", rrlib::time::ToString(-wait_for_x), " too long)");
113      }
114      else if (wait_for_x > cycle_time)
115      {
116        RRLIB_LOG_PRINT(rrlib::logging::eLL_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.");
117        if (last_wait > rrlib::time::tDuration::zero())
118        {
119          RRLIB_LOG_PRINT(rrlib::logging::eLL_WARNING, "Waiting for ", rrlib::time::ToString(last_wait), ", as in last cycle, instead.");
120          Sleep(last_wait, use_application_time);
121        }
122        else
123        {
124          RRLIB_LOG_PRINT(rrlib::logging::eLL_WARNING, "Not waiting at all. As it appears, this thread has never waited yet.");
125        }
126      }
127      else if (wait_for_x > rrlib::time::tDuration::zero())
128      {
129        last_wait = wait_for_x;
130        Sleep(wait_for_x, use_application_time, last_cycle_start + cycle_time);
131      }
132      last_cycle_start += cycle_time;
133      if (wait_for_x < rrlib::time::tDuration::zero())
134      {
135        last_cycle_start = rrlib::time::Now();
136      }
137    }
138    else
139    {
140      last_cycle_start = rrlib::time::Now();
141    }
142
143    MainLoopCallback();
144  }
145}
146
147void tLoopThread::Run()
148{
149  try
150  {
151    //stopSignal = false; // this may lead to unintended behaviour
152
153    // Start main loop
154    MainLoop();
155  }
156  catch (const std::exception& e)
157  {
158    RRLIB_LOG_PRINT(rrlib::logging::eLL_DEBUG, "Uncaught Thread Exception - ", e);
159  }
160}
161
162//----------------------------------------------------------------------
163// End of namespace declaration
164//----------------------------------------------------------------------
165}
166}
Note: See TracBrowser for help on using the repository browser.