diff --git a/ec/hrpEC/hrpEC-common.cpp b/ec/hrpEC/hrpEC-common.cpp index 2df03505994..3eb03f9b5b8 100644 --- a/ec/hrpEC/hrpEC-common.cpp +++ b/ec/hrpEC/hrpEC-common.cpp @@ -4,9 +4,8 @@ #include #endif -#ifdef __QNX__ -using std::fprintf; -#endif +//#define ENABLE_DEBUG_PRINT (true) +#define ENABLE_DEBUG_PRINT (false) namespace RTC { @@ -36,6 +35,9 @@ 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(); @@ -43,6 +45,8 @@ namespace RTC return 0; } do{ + loop++; + if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT) gettimeofday(&debug_tv1, NULL); if (!waitForNextPeriod()){ unlock_iob(); close_iob(); @@ -51,7 +55,7 @@ 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; @@ -59,6 +63,7 @@ namespace RTC } 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 index bce2bb920c2..531b32fdae3 100644 --- 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& 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 index 8f074d71d25..26b9ac6a527 100644 --- 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,7 +163,7 @@ void robot::startForceSensorCalibration() force_calib_counter=CALIB_COUNT; - wait_sem.wait(); + sem_wait(&wait_sem); } void robot::initializeJointAngle(const char *name, const char *option) @@ -170,7 +171,7 @@ 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 index 33a7d449de5..62ffd17c654 100644 --- a/rtc/RobotHardware/robot.h +++ b/rtc/RobotHardware/robot.h @@ -2,7 +2,7 @@ #define __ROBOT_H__ #include -#include +#include #include /** @@ -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 m_commandOld, m_velocityOld; hrp::Vector3 G; diff --git a/rtc/SequencePlayer/SequencePlayer.cpp b/rtc/SequencePlayer/SequencePlayer.cpp index 5e13f42b997..8776c55d46d 100644 --- a/rtc/SequencePlayer/SequencePlayer.cpp +++ b/rtc/SequencePlayer/SequencePlayer.cpp @@ -55,7 +55,6 @@ SequencePlayer::SequencePlayer(RTC::Manager* manager) m_optionalDataOut("optionalData", m_optionalData), m_SequencePlayerServicePort("SequencePlayerService"), // - m_waitSem(0), m_robot(hrp::BodyPtr()), m_debugLevel(0), m_error_pos(0.0001), @@ -63,6 +62,7 @@ SequencePlayer::SequencePlayer(RTC::Manager* manager) 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 index 9f63978235e..44756b9ebeb 100644 --- a/rtc/SequencePlayer/SequencePlayer.h +++ b/rtc/SequencePlayer/SequencePlayer.h @@ -10,7 +10,7 @@ #ifndef SEQUENCEPLAYER_H #define SEQUENCEPLAYER_H -#include +#include #include #include #include @@ -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 index 2eb9e1553ea..1e78b7df0a7 100644 --- a/rtc/StateHolder/StateHolder.cpp +++ b/rtc/StateHolder/StateHolder.cpp @@ -54,8 +54,6 @@ StateHolder::StateHolder(RTC::Manager* manager) m_TimeKeeperServicePort("TimeKeeperService"), // m_timeCount(0), - m_waitSem(0), - m_timeSem(0), dummy(0) { @@ -63,6 +61,8 @@ StateHolder::StateHolder(RTC::Manager* manager) 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 index c9207acf047..23eb804121b 100644 --- a/rtc/StateHolder/StateHolder.h +++ b/rtc/StateHolder/StateHolder.h @@ -10,7 +10,7 @@ #ifndef NULL_COMPONENT_H #define NULL_COMPONENT_H -#include +#include #include #include #include @@ -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;