Merge pull request #970 from snozawa/update_sem_ec

Update semaphore and EcexutionContext.
fkanehiro · Apr 8, 2016 · 93ea359 · 93ea359
2 parents 357d8ae + ed0499e
commit 93ea359
Show file tree

Hide file tree

Showing 8 changed files with 62 additions and 33 deletions.
diff --git a/ec/hrpEC/hrpEC-common.cpp b/ec/hrpEC/hrpEC-common.cpp
@@ -4,9 +4,8 @@
 #include <rtm/RTObjectStateMachine.h>
 #endif
 
-#ifdef __QNX__
-using std::fprintf;
-#endif
+//#define ENABLE_DEBUG_PRINT (true)
+#define ENABLE_DEBUG_PRINT (false)
 
 namespace RTC
 {
@@ -36,13 +35,18 @@ namespace RTC
         set_signal_period(period_nsec/nsubstep);
         std::cout << "period = " << get_signal_period()*nsubstep/1e6
                   << "[ms], priority = " << m_priority << std::endl;
+        struct timeval debug_tv1, debug_tv2, debug_tv3, debug_tv4, debug_tv5;
+        int loop = 0;
+        int debug_count = 5000.0/(get_signal_period()*nsubstep/1e6); // Loop count for debug print. Once per 5000.0 [ms].
 
         if (!enterRT()){
             unlock_iob();
             close_iob();
             return 0;
         }
         do{
+            loop++;
+            if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT) gettimeofday(&debug_tv1, NULL);
             if (!waitForNextPeriod()){
                 unlock_iob();
                 close_iob();
@@ -51,14 +55,15 @@ namespace RTC
             struct timeval tv;
             gettimeofday(&tv, NULL);
             if (m_profile.count > 0){
-#define DELTA_SEC(start, end) end.tv_sec - start.tv_sec + (end.tv_usec - start.tv_usec)/1e6;
+#define DELTA_SEC(start, end) (end.tv_sec - start.tv_sec + (end.tv_usec - start.tv_usec)/1e6)
                 double dt = DELTA_SEC(m_tv, tv);
                 if (dt > m_profile.max_period) m_profile.max_period = dt;
                 if (dt < m_profile.min_period) m_profile.min_period = dt;
                 m_profile.avg_period = (m_profile.avg_period*m_profile.count + dt)/(m_profile.count+1);
             }
             m_profile.count++;
             m_tv = tv;
+            if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT) gettimeofday(&debug_tv2, NULL);
 
 #ifndef OPENRTM_VERSION_TRUNK
             invoke_worker iw;
@@ -86,6 +91,11 @@ namespace RTC
                 tbegin = tend;
             }
 #endif
+            if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT &&
+                rtc_names.size() == processes.size()) {
+              printRTCProcessingTime(processes);
+            }
+            if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT) gettimeofday(&debug_tv3, NULL);
 
             gettimeofday(&tv, NULL);
             double dt = DELTA_SEC(m_tv, tv);
@@ -116,7 +126,7 @@ namespace RTC
             if (dt > period_sec*nsubstep){
   	        m_profile.timeover++; 
 #ifdef NDEBUG
-                fprintf(stderr, "[%d.%6.6d] Timeover: processing time = %4.2f[ms]\n",
+                fprintf(stderr, "[hrpEC][%d.%6.6d] Timeover: processing time = %4.2f[ms]\n",
                         tv.tv_sec, tv.tv_usec, dt*1e3);
                 // Update rtc_names only when rtcs length change.
                 if (processes.size() != rtc_names.size()){
@@ -126,14 +136,20 @@ namespace RTC
                         rtc_names.push_back(std::string(rtc->get_component_profile()->instance_name));
                     }
                 }
-                for (unsigned int i=0; i< processes.size(); i++){
-                    fprintf(stderr, "%s(%4.2f), ", rtc_names[i].c_str(),processes[i]*1e3);
-                }
-                fprintf(stderr, "\n");
+                printRTCProcessingTime(processes);
 #endif
             }
 
 #ifndef OPENRTM_VERSION_TRUNK
+            if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT) {
+              gettimeofday(&debug_tv4, NULL);
+              fprintf(stderr, "[hrpEC] Processing time breakdown : waitForNextPeriod %f[ms], warker (onExecute) %f[ms], ExecutionProfile %f[ms], time from prev cicle %f[ms]\n",
+                      DELTA_SEC(debug_tv1, debug_tv2)*1e3,
+                      DELTA_SEC(debug_tv2, debug_tv3)*1e3,
+                      DELTA_SEC(debug_tv3, debug_tv4)*1e3,
+                      DELTA_SEC(debug_tv5, debug_tv1)*1e3);
+            }
+            if (loop % debug_count == (debug_count-1) && ENABLE_DEBUG_PRINT) gettimeofday(&debug_tv5, NULL);
         } while (m_running);
 #else
         } while (isRunning());

diff --git a/ec/hrpEC/hrpEC.h b/ec/hrpEC/hrpEC.h
@@ -13,6 +13,10 @@
 
 #include "hrpsys/idl/ExecutionProfileService.hh"
 
+#ifdef __QNX__
+using std::fprintf;
+#endif
+
 namespace RTC
 {
   class hrpExecutionContext
@@ -52,6 +56,14 @@ namespace RTC
           }
       }
     }
+    void printRTCProcessingTime (std::vector<double>& processes)
+    {
+      fprintf(stderr, "[hrpEC] ");
+      for (unsigned int i=0; i< processes.size(); i++){
+        fprintf(stderr, "%s(%4.2f), ", rtc_names[i].c_str(),processes[i]*1e3);
+      }
+      fprintf(stderr, "[ms]\n");
+    };
     OpenHRP::ExecutionProfileService::Profile m_profile;
     struct timeval m_tv;
     int m_priority;

diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp
@@ -19,8 +19,9 @@
 using namespace hrp;
 
 
-robot::robot(double dt) : m_fzLimitRatio(0), m_maxZmpError(DEFAULT_MAX_ZMP_ERROR), m_calibRequested(false), m_pdgainsFilename("PDgains.sav"), wait_sem(0), m_reportedEmergency(true), m_dt(dt), m_accLimit(0)
+robot::robot(double dt) : m_fzLimitRatio(0), m_maxZmpError(DEFAULT_MAX_ZMP_ERROR), m_calibRequested(false), m_pdgainsFilename("PDgains.sav"), m_reportedEmergency(true), m_dt(dt), m_accLimit(0)
 {
+    sem_init(&wait_sem, 0, 0);
     m_rLegForceSensorId = m_lLegForceSensorId = -1;
 }
 
@@ -145,7 +146,7 @@ void robot::startInertiaSensorCalibration()
 
     inertia_calib_counter=CALIB_COUNT;
 
-    wait_sem.wait();
+    sem_wait(&wait_sem);
 }
 
 void robot::startForceSensorCalibration()
@@ -162,15 +163,15 @@ void robot::startForceSensorCalibration()
 
     force_calib_counter=CALIB_COUNT;
 
-    wait_sem.wait();
+    sem_wait(&wait_sem);
 }
 
 void robot::initializeJointAngle(const char *name, const char *option)
 {
     m_calibJointName = name;
     m_calibOptions   = option;
     m_calibRequested = true;
-    wait_sem.wait();
+    sem_wait(&wait_sem);
 }
 
 void robot::calibrateInertiaSensorOneStep()
@@ -228,7 +229,7 @@ void robot::calibrateInertiaSensorOneStep()
             }
 #endif
 
-            wait_sem.post();
+            sem_post(&wait_sem);
         }
     }
 }
@@ -252,7 +253,7 @@ void robot::calibrateForceSensorOneStep()
                 write_force_offset(j,  force_sum[j].data());
             }
 
-            wait_sem.post();
+            sem_post(&wait_sem);
         }
     }
 }
@@ -284,7 +285,7 @@ void robot::oneStep()
         ::initializeJointAngle(m_calibJointName.c_str(), 
                                m_calibOptions.c_str());
         m_calibRequested = false;
-        wait_sem.post();
+        sem_post(&wait_sem);
     }
 }
 

diff --git a/rtc/RobotHardware/robot.h b/rtc/RobotHardware/robot.h
@@ -2,7 +2,7 @@
 #define __ROBOT_H__
 
 #include <boost/array.hpp>
-#include <boost/interprocess/sync/interprocess_semaphore.hpp>
+#include <semaphore.h>
 #include <hrpModel/Body.h>
 
 /**
@@ -335,7 +335,7 @@ class robot : public hrp::Body
     std::string m_calibJointName, m_calibOptions;
     std::string m_pdgainsFilename;
     bool m_reportedEmergency;
-    boost::interprocess::interprocess_semaphore wait_sem;
+    sem_t wait_sem;
     double m_dt;
     std::vector<double> m_commandOld, m_velocityOld;
     hrp::Vector3 G;

diff --git a/rtc/SequencePlayer/SequencePlayer.cpp b/rtc/SequencePlayer/SequencePlayer.cpp
@@ -55,14 +55,14 @@ SequencePlayer::SequencePlayer(RTC::Manager* manager)
       m_optionalDataOut("optionalData", m_optionalData),
       m_SequencePlayerServicePort("SequencePlayerService"),
       // </rtc-template>
-      m_waitSem(0),
       m_robot(hrp::BodyPtr()),
       m_debugLevel(0),
       m_error_pos(0.0001),
       m_error_rot(0.001),
       m_iteration(50),
       dummy(0)
 {
+    sem_init(&m_waitSem, 0, 0);
     m_service0.player(this);
     m_clearFlag = false;
     m_waitFlag = false;
@@ -232,14 +232,14 @@ RTC::ReturnCode_t SequencePlayer::onExecute(RTC::UniqueId ec_id)
         if (m_waitFlag){
             m_gname = "";
             m_waitFlag = false;
-            m_waitSem.post();
+            sem_post(&m_waitSem);
         }
     }
     if (m_seq->isEmpty()){
         m_clearFlag = false;
         if (m_waitFlag){
             m_waitFlag = false;
-            m_waitSem.post();
+            sem_post(&m_waitSem);
         }
     }else{
 	Guard guard(m_mutex);
@@ -335,7 +335,7 @@ void SequencePlayer::waitInterpolation()
         std::cerr << __PRETTY_FUNCTION__ << std::endl;
     }
     m_waitFlag = true;
-    m_waitSem.wait();
+    sem_wait(&m_waitSem);
 }
 
 bool SequencePlayer::waitInterpolationOfGroup(const char *gname)
@@ -345,7 +345,7 @@ bool SequencePlayer::waitInterpolationOfGroup(const char *gname)
     }
     m_gname = gname;
     m_waitFlag = true;
-    m_waitSem.wait();
+    sem_wait(&m_waitSem);
     return true;
 }
 

diff --git a/rtc/SequencePlayer/SequencePlayer.h b/rtc/SequencePlayer/SequencePlayer.h
@@ -10,7 +10,7 @@
 #ifndef SEQUENCEPLAYER_H
 #define SEQUENCEPLAYER_H
 
-#include <boost/interprocess/sync/interprocess_semaphore.hpp>
+#include <semaphore.h>
 #include <rtm/Manager.h>
 #include <rtm/DataFlowComponentBase.h>
 #include <rtm/CorbaPort.h>
@@ -181,7 +181,7 @@ class SequencePlayer
  private:
   seqplay *m_seq;
   bool m_clearFlag, m_waitFlag;
-  boost::interprocess::interprocess_semaphore m_waitSem;
+  sem_t m_waitSem;
   hrp::BodyPtr m_robot;
   std::string m_gname;
   unsigned int m_debugLevel;

diff --git a/rtc/StateHolder/StateHolder.cpp b/rtc/StateHolder/StateHolder.cpp
@@ -54,15 +54,15 @@ StateHolder::StateHolder(RTC::Manager* manager)
     m_TimeKeeperServicePort("TimeKeeperService"),
     // </rtc-template>
     m_timeCount(0),
-    m_waitSem(0),
-    m_timeSem(0),
     dummy(0)
 {
 
   m_service0.setComponent(this);
   m_service1.setComponent(this);
   m_requestGoActual = false;
 
+  sem_init(&m_waitSem, 0, 0);
+  sem_init(&m_timeSem, 0, 0);
 }
 
 StateHolder::~StateHolder()
@@ -253,7 +253,7 @@ RTC::ReturnCode_t StateHolder::onExecute(RTC::UniqueId ec_id)
 
     if (m_requestGoActual){
         m_requestGoActual = false;
-        m_waitSem.post();
+        sem_post(&m_waitSem);
     }
 
     if (m_basePosIn.isNew()){
@@ -321,7 +321,7 @@ RTC::ReturnCode_t StateHolder::onExecute(RTC::UniqueId ec_id)
 
     if (m_timeCount > 0){
         m_timeCount--;
-        if (m_timeCount == 0) m_timeSem.post();
+        if (m_timeCount == 0) sem_post(&m_timeSem);
     }
 
     return RTC::RTC_OK;
@@ -367,7 +367,7 @@ void StateHolder::goActual()
 {
     std::cout << "StateHolder::goActual()" << std::endl;
     m_requestGoActual = true;
-    m_waitSem.wait();
+    sem_wait(&m_waitSem);
 }
 
 void StateHolder::getCommand(StateHolderService::Command &com)
@@ -388,7 +388,7 @@ void StateHolder::getCommand(StateHolderService::Command &com)
 void StateHolder::wait(CORBA::Double tm)
 {
     m_timeCount = tm/m_dt;
-    m_timeSem.wait();
+    sem_wait(&m_timeSem);
 }
 
 extern "C"

diff --git a/rtc/StateHolder/StateHolder.h b/rtc/StateHolder/StateHolder.h
@@ -10,7 +10,7 @@
 #ifndef NULL_COMPONENT_H
 #define NULL_COMPONENT_H
 
-#include <boost/interprocess/sync/interprocess_semaphore.hpp>
+#include <semaphore.h>
 #include <rtm/Manager.h>
 #include <rtm/DataFlowComponentBase.h>
 #include <rtm/CorbaPort.h>
@@ -165,7 +165,7 @@ class StateHolder
 
  private:
   int m_timeCount;
-  boost::interprocess::interprocess_semaphore m_waitSem, m_timeSem;
+  sem_t m_waitSem, m_timeSem;
   bool m_requestGoActual;
   double m_dt;
   int dummy;