RTPreemptEC.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
18 #include "RTPreemptEC.h"
19 #include <stdlib.h>
20 #include <stdio.h>
21 #include <time.h>
22 #include <sched.h>
23 #include <sys/mman.h>
24 #include <string.h>
25 #include <rtm/ECFactory.h>
26 #include <rtm/Manager.h>
27 #include <coil/stringutil.h>
28 
29 #define MAX_SAFE_STACK (8*1024)
30 #define NSEC_PER_SEC 1000000000
31 
32 namespace OpenRTM
33 {
43  m_priority(49), m_policy(SCHED_FIFO), m_waitoffset(0)
44  {
45  rtclog.setName("RTPreemptEC");
47 
48  // Priority
49  getProperty(prop, "exec_cxt.periodic.priority", m_priority);
50  getProperty(prop, "exec_cxt.periodic.rtpreempt.priority", m_priority);
51  RTC_DEBUG(("Priority: %d", m_priority));
52 
53  // Policy
54  {
55  std::string policy;
56  getProperty(prop, "exec_cxt.periodic.rtpreempt.sched_policy", policy);
57  if (!policy.empty())
58  {
59  coil::normalize(policy);
60  if (policy == "rr") { m_policy = SCHED_RR; }
61  if (policy == "fifo") { m_policy = SCHED_FIFO; }
62  RTC_DEBUG(("Scheduling policy: %s", policy.c_str()));
63  }
64  else
65  {
66  RTC_DEBUG(("Scheduling policy: fifo"));
67  }
68  }
69 
70  // Wait offset
71  getProperty(prop, "exec_cxt.periodic.rtpreempt.wait_offset", m_waitoffset);
72  RTC_DEBUG(("Wait offset: %d [ns]", m_waitoffset));
73 
74  }
75 
84  {
85  }
86 
94  int RTPreemptEC::svc(void)
95  {
96  // schedulaer setting
97  struct sched_param param;
98  param.sched_priority = m_priority;
99  if(sched_setscheduler(0, m_policy, &param) == -1)
100  {
101  std::cerr << "sched_setscheduler failed" << std::endl;
102  return -1;
103  }
104 
105  // memory locking
106  if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
107  {
108  std::cerr << "mlockall failed" << std::endl;
109  return -1;
110  }
111 
112  // stack preallocation
113  unsigned char dummy[MAX_SAFE_STACK];
114  memset(&dummy, 0, MAX_SAFE_STACK);
115  struct timespec t0, t1, t;
116  do
117  {
118  clock_gettime(CLOCK_MONOTONIC ,&t0);
119  m_worker.mutex_.lock();
120  while (!m_worker.running_)
121  {
122  m_worker.cond_.wait();
123  }
124  if (m_worker.running_)
125  {
126  std::for_each(m_comps.begin(), m_comps.end(), invoke_worker());
127  }
129  clock_gettime(CLOCK_MONOTONIC ,&t1);
130 
131  if (!m_nowait)
132  {
133  if (t0.tv_nsec > t1.tv_nsec)
134  {
135  t.tv_nsec = m_period.usec() * 1000
136  - (NSEC_PER_SEC - t0.tv_nsec + t1.tv_nsec) + m_waitoffset;
137  t.tv_sec = m_period.sec()
138  - (t1.tv_sec - 1 - t0.tv_sec);
139  }
140  else
141  {
142  t.tv_nsec = m_period.usec() * 1000
143  - (t1.tv_nsec - t0.tv_nsec) + m_waitoffset;
144  t.tv_sec = m_period.sec()
145  - (t1.tv_sec - t0.tv_sec);
146  }
147 
148  if (t.tv_nsec < 0 || t.tv_sec < 0)
149  {
150  std::cerr << "faital error: deadline passed. " << std::endl;
151  std::cerr << "Wait time: ";
152  std::cerr << t.tv_sec << "[s], ";
153  std::cerr << t.tv_nsec << "[ns]" << std::endl;
154  std::cerr << "Next wait time force to: 0.0 [s]" << std::endl;
155  continue;
156  }
157  clock_nanosleep(CLOCK_MONOTONIC, !TIMER_ABSTIME, &t, NULL);
158  }
159  } while (m_svc);
160 
161  return 0;
162  }
163 
164 };
165 
166 
167 extern "C"
168 {
177  {
179  registerECFactory("RTPreemptEC",
180  RTC::ECCreate<OpenRTM::RTPreemptEC>,
181  RTC::ECDelete<OpenRTM::RTPreemptEC>);
182 
183  }
184 };
std::string normalize(std::string &str)
Erase the head/tail blank and replace upper case to lower case.
Definition: stringutil.cpp:308
RT-Component.
std::vector< Comp > m_comps
List of the participating component.
bool m_svc
The thread running flag of ExecutionContext.
Manager class.
Definition: Manager.h:80
void setName(const char *name)
Set suffix of date/time string of header.
static Manager & instance()
Get instance of the manager.
Definition: Manager.cpp:140
RTComponent manager class.
Worker m_worker
A condition variable for external triggered worker.
void getProperty(coil::Properties &prop, const char *key, T &value)
Thread execution function for ExecutionContext.
Definition: RTPreemptEC.h:201
bool m_nowait
Flag of ExecutionContext to run immediately (to run without waiting)
#define RTC_DEBUG(fmt)
Debug level log output macro.
Definition: SystemLogger.h:488
virtual ~RTPreemptEC()
Destructor.
Definition: RTPreemptEC.cpp:83
prop
Organization::get_organization_property ();.
Class represents a set of properties.
Definition: Properties.h:101
ExecutionContext Factory class.
#define MAX_SAFE_STACK
Definition: RTPreemptEC.cpp:29
RTPreemptEC()
Default Constructor.
Definition: RTPreemptEC.cpp:41
long int sec() const
Get value of second time scale.
Definition: TimeValue.h:110
void RTPreemptECInit(RTC::Manager *manager)
Initialization function to register to ECFactory.
long int usec() const
Get value of micro second time scale.
Definition: TimeValue.h:131
#define NSEC_PER_SEC
Definition: RTPreemptEC.cpp:30
virtual int svc(void)
Thread execution function for ExecutionContext.
Definition: RTPreemptEC.cpp:94
RTPreemptEC class.
coil::TimeValue m_period
Execution cycle of ExecutionContext.
Functor for_each(CorbaSequence &seq, Functor f)
Apply the functor to all CORBA sequence elements.
Definition: CORBA_SeqUtil.h:98


openrtm_aist
Author(s): Noriaki Ando
autogenerated on Mon Feb 28 2022 23:00:45