Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Update semaphore and EcexutionContext. #970

Merged
merged 2 commits into from
Apr 8, 2016
Merged

Update semaphore and EcexutionContext. #970

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3d017f6
[rtc/RobotHardware/robot.[cpp,h] ,rtc/SequencePlayer/SequencePlayer.[…
snozawa Apr 7, 2016
ed0499e
[ec/hrpEC/hrpEC-common.cpp, hrpEC.h] Add ordinaly debug message for p…
snozawa Apr 7, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 25 additions & 9 deletions ec/hrpEC/hrpEC-common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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();
Expand All @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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()){
Expand All @@ -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());
Expand Down
12 changes: 12 additions & 0 deletions ec/hrpEC/hrpEC.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,10 @@

#include "hrpsys/idl/ExecutionProfileService.hh"

#ifdef __QNX__
using std::fprintf;
#endif

namespace RTC
{
class hrpExecutionContext
Expand Down Expand Up @@ -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;
Expand Down
15 changes: 8 additions & 7 deletions rtc/RobotHardware/robot.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -145,7 +146,7 @@ void robot::startInertiaSensorCalibration()

inertia_calib_counter=CALIB_COUNT;

wait_sem.wait();
sem_wait(&wait_sem);
}

void robot::startForceSensorCalibration()
Expand All @@ -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()
Expand Down Expand Up @@ -228,7 +229,7 @@ void robot::calibrateInertiaSensorOneStep()
}
#endif

wait_sem.post();
sem_post(&wait_sem);
}
}
}
Expand All @@ -252,7 +253,7 @@ void robot::calibrateForceSensorOneStep()
write_force_offset(j, force_sum[j].data());
}

wait_sem.post();
sem_post(&wait_sem);
}
}
}
Expand Down Expand Up @@ -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);
}
}

Expand Down
4 changes: 2 additions & 2 deletions rtc/RobotHardware/robot.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>

/**
Expand Down Expand Up @@ -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;
Expand Down
10 changes: 5 additions & 5 deletions rtc/SequencePlayer/SequencePlayer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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)
Expand All @@ -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;
}

Expand Down
4 changes: 2 additions & 2 deletions rtc/SequencePlayer/SequencePlayer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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;
Expand Down
12 changes: 6 additions & 6 deletions rtc/StateHolder/StateHolder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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()){
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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)
Expand All @@ -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"
Expand Down
4 changes: 2 additions & 2 deletions rtc/StateHolder/StateHolder.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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;
Expand Down