diff --git a/src/liboncilla/Synchronizer.cpp b/src/liboncilla/Synchronizer.cpp index 0ce4d6d..9077288 100644 --- a/src/liboncilla/Synchronizer.cpp +++ b/src/liboncilla/Synchronizer.cpp @@ -1,18 +1,54 @@ #include "Synchronizer.h" #include using namespace std; namespace rci { namespace oncilla { Synchronizer::Synchronizer(const std::string &sname) - : ::rci::Synchronizer(sname){ + : ::rci::Synchronizer(sname) + , d_inProcess(false){ } Synchronizer::~Synchronizer() { } + +bool Synchronizer::start(){ + d_inProcess = false; + return rci::Synchronizer::start(); +} + + +void Synchronizer::processAsync(){ + if(!tooBusy()){ + start(); + } + if(d_inProcess){ + throw std::logic_error("Previous processing is not terminated."); + } else { + d_inProcess = true; + } + + ProcessAsyncPrimpl(); +} + +void Synchronizer::waitForProcessAsync(){ + if(!tooBusy()){ + start(); + } + + if(!d_inProcess){ + //just do nothing, since nothing to do; + return; + } + + WaitForProcessAsyncPrimpl(); + + d_inProcess = false; +} + } } diff --git a/src/liboncilla/Synchronizer.h b/src/liboncilla/Synchronizer.h index 327c9ab..727a60f 100644 --- a/src/liboncilla/Synchronizer.h +++ b/src/liboncilla/Synchronizer.h @@ -1,46 +1,57 @@ #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(); +protected : + virtual void ProcessAsyncPrimpl() = 0; + + virtual void WaitForProcessAsyncPrimpl() = 0; + + bool d_inProcess; }; } } diff --git a/src/liboncilla/nodes/L1L2.cpp b/src/liboncilla/nodes/L1L2.cpp index c825fbe..55177cb 100644 --- a/src/liboncilla/nodes/L1L2.cpp +++ b/src/liboncilla/nodes/L1L2.cpp @@ -1,67 +1,79 @@ #include "L1L2.h" #include +#include + using namespace std; namespace rci{ namespace oncilla{ L1L2::L1L2(Synchronizer &s, const std::string &name) : ResourceNode(name) , Controlled() , Sensing() , PositionControlled() , PositionSensing() , TorqueControlled() , ImpedanceControlled() - , synchronizer(s) { + , d_synchronizer(s) { /// \todo add default value as setting this->_controlMode = modeJointPositionControl; this->_lastCommandedPosition = JointAngles::create(1,0.0); this->_latestJointPosition = JointAngles::create(2,0.0); } L1L2::~L1L2() { } std::string L1L2::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } +bool L1L2::setJointImpedance(JointImpedancePtr ji){ + if(d_synchronizer.tooBusy()) { + throw UnavailableInterfaceError("Joint impedance could not be set once Synchronizer is started.", + "liboncilla","setJointImpedance","L1L2", + "Synchronizer is started by either start(), process() or processAsync()," + "you can stop() it first"); + } + return ImpedanceControlled::setJointImpedance(ji); +} + bool L1L2::isConverged() const{ throw NotImplementedError("liboncilla","isConverged","L1L2"); } void L1L2::unsafeSetCommand(double value){ this->_lastCommandedPosition->setValue(0,value); } double L1L2::unsafeGetCommand() const{ return this->_lastCommandedPosition->asDouble(0); } double L1L2::unsafeGetMotorAxisPosition() const{ return this->_latestJointPosition->asDouble(MotorAxisIndex); } double L1L2::unsafeGetMagneticEncoderPosition() const{ return this->_latestJointPosition->asDouble(MagneticEncoderIndex); } void L1L2::unsafeUpdateMotorAxisPosition(double value){ this->_latestJointPosition->setValue(MotorAxisIndex,value); } void L1L2::unsafeUpdateMagneticEncoderPosition(double value){ this->_latestJointPosition->setValue(MagneticEncoderIndex,value); } } } diff --git a/src/liboncilla/nodes/L1L2.h b/src/liboncilla/nodes/L1L2.h index 0631796..a40b90e 100644 --- a/src/liboncilla/nodes/L1L2.h +++ b/src/liboncilla/nodes/L1L2.h @@ -1,83 +1,85 @@ #pragma once #include #include #include #include #include namespace rci{ namespace oncilla{ class Synchronizer; class L1L2; /** * Node class, representing the hip node of the quadruped robot. * * @todo In case of simulation, this node can also sense the power consumption. */ class L1L2: public rci::ResourceNode, public rci::Controlled, public rci::Sensing, public rci::PositionControlled, public rci::PositionSensing, // Two encoder values public rci::VelocityControlled, public rci::TorqueControlled, public rci::ImpedanceControlled { public: typedef boost::shared_ptr Ptr; const static unsigned int MotorAxisIndex = 0; const static unsigned int MagneticEncoderIndex = 1; /** * Special constructor to also link to webots */ - L1L2(Synchronizer & s, - const std::string & name = "Oncilla Hip"); + L1L2(rci::oncilla::Synchronizer & s, + const std::string & name = "Oncilla Hip"); virtual ~L1L2(); + bool setJointImpedance(JointImpedancePtr ); + bool isConverged() const; /** * Print */ std::string print() const; protected : void unsafeSetCommand(double value); double unsafeGetCommand() const; double unsafeGetMotorAxisPosition() const; double unsafeGetMagneticEncoderPosition() const; void unsafeUpdateMotorAxisPosition(double value); void unsafeUpdateMagneticEncoderPosition(double value); private: /** * Reference to OncillaSynchronizer * @todo This is a dangerous reference which will become invalid as soon as * OncillaSynchronizer gets deleted outside. */ - Synchronizer &synchronizer; + rci::oncilla::Synchronizer & d_synchronizer; }; typedef L1L2 L1; typedef L1L2 L2; } } diff --git a/test/nodes/OncillaL1L2Test.cpp b/test/nodes/OncillaL1L2Test.cpp index b534a7e..20e7fd8 100644 --- a/test/nodes/OncillaL1L2Test.cpp +++ b/test/nodes/OncillaL1L2Test.cpp @@ -1,69 +1,69 @@ #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 processAsync() { + virtual void ProcessAsyncPrimpl() { } - virtual void waitForProcessAsync() { + virtual void WaitForProcessAsyncPrimpl() { } virtual double lastProcessTimeStep() const { return 0.005; } virtual void switchToSynchronizationModeHook(){ } }; 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()); }