diff --git a/src/liboncilla/Oncilla.cpp b/src/liboncilla/Oncilla.cpp index d5832f3..0e18514 100644 --- a/src/liboncilla/Oncilla.cpp +++ b/src/liboncilla/Oncilla.cpp @@ -1,106 +1,109 @@ /* * Oncilla.cpp * * Created on: 15 dec. 2011 * Author: Alexandre Tuleu, Arne Nordmann */ #include "Oncilla.h" #include namespace rci { namespace oncilla { ::rci::oncilla::Synchronizer::Ptr Oncilla::s_synchronizer; std::string Oncilla::nameOfLeg(Leg l){ static const char * legNames[NUM_LEGS + 1] = { "Left Fore", //LEFT_FORE "Right Fore", //RIGHT_FORE "Left Hind", //LEFT_HIND "Right Hind", //RIGHT_HIND "Undefined" }; //oncly consider index in range [0 NUM_LEGS], defaulting to NUM_LEGS if out of bound. int index((l < 0 ) ? NUM_LEGS : (l > NUM_LEGS ? NUM_LEGS : l)); return legNames[index]; } Oncilla::Oncilla(){ if(!s_synchronizer){ if(!d_backend){ BackendLoader bl; //we have to keep alive the pointer or dlclose() will be called once //We free him. Or we could add BackendLoader as a member of Oncilla // I prefer only to have the Object creator interface. d_backend = bl.SuitableBackend(); } s_synchronizer = d_backend->CreateSynchronizer(); init(); } } Oncilla::~Oncilla() { } void Oncilla::init() { d_L0s.reserve(4); d_L1s.reserve(4); d_L2s.reserve(4); d_L3s.reserve(4); LIBONCILLA_FOREACH_LEG(l){ d_L0s.push_back(d_backend->CreateL0(l,Oncilla::nameOfLeg(l) + " Oncilla L0")); d_L1s.push_back(d_backend->CreateL1(l,Oncilla::nameOfLeg(l) + " Oncilla L1")); d_L2s.push_back(d_backend->CreateL2(l,Oncilla::nameOfLeg(l) + " Oncilla L2")); d_L3s.push_back(d_backend->CreateL3(l,Oncilla::nameOfLeg(l) + " Oncilla L3")); } d_trunk = d_backend->CreateTrunk(); d_supervisor = Supervisor::Ptr(new Supervisor(*d_backend)); + + s_synchronizer->calibrateIfNeeded(); + } const L0::Ptr & Oncilla::getL0(Leg l) const { return this->d_L0s[l]; } const L1::Ptr & Oncilla::getL1(Leg l) const { return this->d_L1s[l]; } const L2::Ptr & Oncilla::getL2(Leg l) const { return this->d_L2s[l]; } const L3::Ptr & Oncilla::getL3(Leg l) const { return this->d_L3s[l]; } const Trunk::Ptr & Oncilla::getTrunk() const { return this->d_trunk; } const rci::oncilla::Synchronizer::Ptr & 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; } const rci::oncilla::Supervisor::Ptr & Oncilla::getSupervisor() const { return d_supervisor; } } } diff --git a/src/liboncilla/Synchronizer.h b/src/liboncilla/Synchronizer.h index 727a60f..438e0a3 100644 --- a/src/liboncilla/Synchronizer.h +++ b/src/liboncilla/Synchronizer.h @@ -1,57 +1,60 @@ #pragma once #include #include #include #include #include "common.h" namespace rci { namespace oncilla { /** * Synchronizer * Different OncillaSynchronizers may be for example: * * Update with a given frequency * ** Send data every time * ** Send only if new data * * Update only if new data (event-based) * * Triggered from robot side (e.g. FRI) */ class Synchronizer: public ::rci::Synchronizer { public: typedef boost::shared_ptr Ptr; Synchronizer(const std::string &name); virtual ~Synchronizer(); /** * */ friend std::ostream & operator<<(std::ostream& os, const Synchronizer& val); virtual double lastProcessTimeStep() const = 0; virtual void processAsync(); virtual void waitForProcessAsync(); virtual bool start(); + + virtual void calibrateIfNeeded() = 0; + protected : virtual void ProcessAsyncPrimpl() = 0; virtual void WaitForProcessAsyncPrimpl() = 0; bool d_inProcess; }; } } diff --git a/test/nodes/OncillaL1L2Test.cpp b/test/nodes/OncillaL1L2Test.cpp index 20e7fd8..729851e 100644 --- a/test/nodes/OncillaL1L2Test.cpp +++ b/test/nodes/OncillaL1L2Test.cpp @@ -1,69 +1,72 @@ #include #include #include "liboncilla/Synchronizer.h" using namespace nemo; using namespace rci; using namespace rci::oncilla; class MockSynchronizer: public ::rci::oncilla::Synchronizer { public: MockSynchronizer() : Synchronizer("Sync") { } virtual void ProcessAsyncPrimpl() { } virtual void WaitForProcessAsyncPrimpl() { } virtual double lastProcessTimeStep() const { return 0.005; } virtual void switchToSynchronizationModeHook(){ } + + virtual void calibrateIfNeeded(){ + } }; class OncillaL1L2Test: public ::testing::Test { protected: OncillaL1L2Test() : joint(), ja_values(), ji_values(), ja(), ji(), synchr() { joint = rci::oncilla::L1::Ptr(new L1(synchr, "FooBar")); ja_values = RealVector(dim(1), 1.234); ji_values = RealVector(dim(2), 1.234); ji_values[1] = 2.345; ja = JointAngles::fromRad(ja_values); ji = JointImpedancePtr(new JointImpedance(ji_values)); } virtual ~OncillaL1L2Test() { } rci::oncilla::L1L2::Ptr joint; RealVector ja_values, ji_values; JointAnglesPtr ja; JointImpedancePtr ji; MockSynchronizer synchr; }; TEST_F(OncillaL1L2Test, testInitialialState) { EXPECT_NO_THROW(joint->getLastPositionCommand()); EXPECT_EQ(0.0,joint->getLastPositionCommand()->rad()); } TEST_F(OncillaL1L2Test, testPositionCommand) { EXPECT_NO_THROW(joint->getLastPositionCommand()); EXPECT_EQ(0.0,joint->getLastPositionCommand()->rad()); joint->setJointPosition(ja); EXPECT_EQ(1.234, joint->getLastPositionCommand()->rad()); }