Homec4science
Diffusion liboncilla (tuleu-dev) Browse

liboncilla/src/Oncilla.cpp
29079f3278ectuleu-dev

Last Change
Raw File

Oncilla.cpp
View Options

/*
* Oncilla.cpp
*
* Created on: 15 dec. 2011
* Author: Alexandre Tuleu, Arne Nordmann
*/
#include "Oncilla.h"
#include "OncillaTrunk.h"
#include "OncillaL0.h"
#include "OncillaL1L2.h"
#include "OncillaL3.h"
#include "OncillaL4.h"
#include "BackendFactory.h"
namespace rci {
namespace oncilla {
OncillaSynchronizerPtr Oncilla::s_synchronizer;
Oncilla::LegNames Oncilla::s_legNames;
const std::string & Oncilla::nameOfLeg(Leg l){
if(s_legNames.empty()){
s_legNames.push_back("Left Fore");
s_legNames.push_back("Right Fore");
s_legNames.push_back("Left Hind");
s_legNames.push_back("Right Hind");
s_legNames.push_back("Undefined");
}
if(l >= NUM_LEGS){
return s_legNames.back();
}
return s_legNames[l];
}
Oncilla::Oncilla(){
if(!s_synchronizer){
BackendFactory::EntryPtr e = BackendFactory::Instance().HighestPriorityBackend();
if( e == 0){
throw std::runtime_error("No backend for liboncilla were found. Are you"
" sure you are linking your program with a backend like "
"'liboncilla-webots' or 'liboncilla-hw' ?.");
}
OncillaSynchronizerPtr p((*e)());
s_synchronizer = p;
init();
}
}
Oncilla::~Oncilla() {
}
#define CREATE_NODES(LegName)do{\
d_L0s.push_back(OncillaL0Ptr(new OncillaL0(LegName " Oncilla L0" )));\
d_L1s.push_back(OncillaL1Ptr(new OncillaL1L2(*s_synchronizer,\
LegName " Oncilla L1")));\
d_L2s.push_back(OncillaL2Ptr(new OncillaL1L2(*s_synchronizer,\
LegName " Oncilla L2")));\
d_L3s.push_back(OncillaL3Ptr(new OncillaL3(LegName " Oncilla L3")));\
d_L4s.push_back(OncillaL4Ptr(new OncillaL4(LegName " Oncilla L4")));\
}while(0)
#define REGISTER_NODES(Type)do{\
for(std::vector < Oncilla ## Type ## Ptr >::const_iterator n = d_ ## Type ## s.begin();\
n != d_ ## Type ## s.end();\
++n){\
Oncilla::Leg l = static_cast<Oncilla::Leg>(n - d_ ## Type ## s.begin());\
s_synchronizer->register ## Type ## Node(l,*n);\
}\
}while(0)
void Oncilla::init() {
d_L0s.reserve(4);
d_L1s.reserve(4);
d_L2s.reserve(4);
d_L3s.reserve(4);
d_L4s.reserve(4);
CREATE_NODES("Left Fore");
CREATE_NODES("Right Fore");
CREATE_NODES("Left Hind");
CREATE_NODES("Right Hind");
d_trunk = OncillaTrunkPtr(new OncillaTrunk("Oncilla Trunk"));
REGISTER_NODES(L0);
REGISTER_NODES(L1);
REGISTER_NODES(L2);
REGISTER_NODES(L3);
REGISTER_NODES(L4);
s_synchronizer->registerTrunkNode(d_trunk);
}
OncillaL0Ptr Oncilla::getL0(Leg l) const {
return this->d_L0s[l];
}
OncillaL1Ptr Oncilla::getL1(Leg l) const {
return this->d_L1s[l];
}
OncillaL2Ptr Oncilla::getL2(Leg l) const {
return this->d_L2s[l];
}
OncillaL3Ptr Oncilla::getL3(Leg l) const {
return this->d_L3s[l];
}
OncillaL4Ptr Oncilla::getL4(Leg l) const {
return this->d_L4s[l];
}
OncillaTrunkPtr Oncilla::getTrunk() const {
return this->d_trunk;
}
OncillaSynchronizerPtr Oncilla::getSynchronizer() const {
if(!s_synchronizer){
throw std::logic_error("Internal error : synchronizer seems unitialized"
", but it should never happen. Please report this bug.");
}
return this->s_synchronizer;
}
}
}
c4science · Help