diff --git a/src/liboncilla/CMakeLists.txt b/src/liboncilla/CMakeLists.txt index 8532345..76cda1e 100644 --- a/src/liboncilla/CMakeLists.txt +++ b/src/liboncilla/CMakeLists.txt @@ -1,47 +1,55 @@ set(ONCILLAHEADERS nodes/Trunk.h nodes/L0.h nodes/L1L2.h nodes/L3.h + nodes/Nodes.h + nodes/SupervisorL4.h + nodes/SupervisorTrunk.h + nodes/SupervisorNodes.h Oncilla.h Synchronizer.h + Supervisor.h utils/BackendLoader.h utils/OncillaBackend.h common.h exceptions/LoadingError.h exceptions/NotImplementedError.h) set(ONCILLASOURCES nodes/Trunk.cpp nodes/L0.cpp nodes/L1L2.cpp nodes/L3.cpp + nodes/SupervisorTrunk.cpp + nodes/SupervisorL4.cpp Oncilla.cpp Synchronizer.cpp + Supervisor.cpp utils/OncillaBackend.cpp exceptions/LoadingError.cpp exceptions/NotImplementedError.cpp utils/BackendLoader.cpp) add_library(oncilla SHARED ${ONCILLASOURCES} ${ONCILLAHEADERS}) target_link_libraries(oncilla ${RCI_LIBRARIES} ${BIOROB_CPP_LIBRARIES}) set_target_properties(oncilla PROPERTIES VERSION ${VERSION_STRING} SOVERSION ${VERSION_ABI}) install(TARGETS oncilla DESTINATION lib) set(DEST_INCLUDE_DIR include/liboncilla-${VERSION_API}) install(DIRECTORY ${PROJECT_SOURCE_DIR}/src/liboncilla ${PROJECT_BINARY_DIR}/src/liboncilla DESTINATION ${DEST_INCLUDE_DIR} FILES_MATCHING PATTERN "*.h" PATTERN "CMakeFiles" EXCLUDE ) diff --git a/src/liboncilla/Oncilla.cpp b/src/liboncilla/Oncilla.cpp index 1bd2af1..72885b0 100644 --- a/src/liboncilla/Oncilla.cpp +++ b/src/liboncilla/Oncilla.cpp @@ -1,109 +1,111 @@ /* * 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; 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){ 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() { } #define CREATE_NODES(LegName,Leg)do{\ - d_L0s.push_back(L0::Ptr(d_backend->CreateL0(Leg,LegName " Oncilla L0" )));\ - d_L1s.push_back(L1::Ptr(d_backend->CreateL1(Leg,\ - LegName " Oncilla L1")));\ - d_L2s.push_back(L2::Ptr(d_backend->CreateL2(Leg,\ - LegName " Oncilla L2")));\ - d_L3s.push_back(L3::Ptr(d_backend->CreateL3(Leg,LegName " Oncilla L3")));\ + d_L0s.push_back(d_backend->CreateL0(Leg,LegName " Oncilla L0" ));\ + d_L1s.push_back(d_backend->CreateL1(Leg,\ + LegName " Oncilla L1"));\ + d_L2s.push_back(d_backend->CreateL2(Leg,\ + LegName " Oncilla L2"));\ + d_L3s.push_back(d_backend->CreateL3(Leg,LegName " Oncilla L3"));\ }while(0) void Oncilla::init() { d_L0s.reserve(4); d_L1s.reserve(4); d_L2s.reserve(4); d_L3s.reserve(4); - CREATE_NODES("Left Fore",LEFT_FORE); - CREATE_NODES("Right Fore",RIGHT_FORE); - CREATE_NODES("Left Hind",LEFT_HIND); - CREATE_NODES("Right Hind",RIGHT_HIND); + for(rci::oncilla::Leg l = LEFT_FORE; l < NUM_LEGS; ++l){ + d_L0s.push_back(d_backend->CreateL0(l,rci::oncilla::LegNames[l] + std::string(" Oncilla L0"))); + d_L1s.push_back(d_backend->CreateL1(l,rci::oncilla::LegNames[l] + std::string(" Oncilla L1"))); + d_L2s.push_back(d_backend->CreateL2(l,rci::oncilla::LegNames[l] + std::string(" Oncilla L2"))); + d_L3s.push_back(d_backend->CreateL3(l,rci::oncilla::LegNames[l] + std::string(" Oncilla L3"))); + } - d_trunk = Trunk::Ptr(new Trunk("Oncilla Trunk")); + d_trunk = d_backend->CreateTrunk(); } 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; } } } diff --git a/src/liboncilla/Oncilla.h b/src/liboncilla/Oncilla.h index 1ec0fc7..1a496e8 100644 --- a/src/liboncilla/Oncilla.h +++ b/src/liboncilla/Oncilla.h @@ -1,60 +1,64 @@ /* * Oncilla.h * * Created on: 15 dec. 2011 * Author: Alexandre Tuleu, Arne Nordmann */ #pragma once #include #include #include #include "common.h" +#include "Supervisor.h" #include #include #include namespace rci { namespace oncilla { class BackendLoader; class Oncilla { public: typedef boost::shared_ptr Ptr; static const std::string & nameOfLeg(Leg l); Oncilla(); virtual ~Oncilla(); const L0::Ptr & getL0(Leg l) const; const L1::Ptr & getL1(Leg l) const; const L2::Ptr & getL2(Leg l) const; const L3::Ptr & getL3(Leg l) const; const Trunk::Ptr & getTrunk() const; const rci::oncilla::Synchronizer::Ptr & getSynchronizer() const; + const rci::oncilla::Supervisor::Ptr & getSupervisor() const; private: typedef std::vector LegNames; static LegNames s_legNames; void init(); static rci::oncilla::Synchronizer::Ptr s_synchronizer; std::vector d_L0s; std::vector d_L1s; std::vector d_L2s; std::vector d_L3s; Trunk::Ptr d_trunk; + OncillaBackend::Ptr d_backend; + }; } } diff --git a/src/liboncilla/Supervisor.cpp b/src/liboncilla/Supervisor.cpp new file mode 100644 index 0000000..5115031 --- /dev/null +++ b/src/liboncilla/Supervisor.cpp @@ -0,0 +1,27 @@ +/* + * Supervisor.cpp + * + * Created on: Jan 21, 2013 + * Author: tuleu + */ + +#include "Supervisor.h" +#include +namespace rci { +namespace oncilla { + +Supervisor::Supervisor(OncillaBackend & backend) + : d_l4s(4,SupervisorL4::Ptr()) + , d_trunk(backend.CreateSupervisorTrunk()){ + for(rci::oncilla::Leg l = LEFT_FORE; + l < rci::oncilla::NUM_LEGS ; ++l){ + d_l4s[l] = backend.CreateSupervisorL4(l,LegNames[l] + std::string(" Oncilla Supervised L4")); + } +} + +Supervisor::~Supervisor(){ + // TODO Auto-generated destructor stub +} + +} /* namespace oncilla */ +} /* namespace rci */ diff --git a/src/liboncilla/Supervisor.h b/src/liboncilla/Supervisor.h new file mode 100644 index 0000000..21b7abb --- /dev/null +++ b/src/liboncilla/Supervisor.h @@ -0,0 +1,38 @@ +/* + * Supervisor.h + * + * Created on: Jan 21, 2013 + * Author: tuleu + */ + +#ifndef SUPERVISOR_H_ +#define SUPERVISOR_H_ + +#include +#include "common.h" + +#include + +class OncillaBackend; + +namespace rci { +namespace oncilla { + +class Supervisor { +public: + typedef boost::shared_ptr Ptr; + Supervisor(OncillaBackend & backend); + virtual ~Supervisor(); + + const SupervisorTrunk::Ptr & getTrunk(); + + const SupervisorL4::Ptr & getL4(Leg l); +private: + std::vector d_l4s; + SupervisorTrunk::Ptr d_trunk; + +}; + +} /* namespace oncilla */ +} /* namespace rci */ +#endif /* SUPERVISOR_H_ */ diff --git a/src/liboncilla/common.h b/src/liboncilla/common.h index 040dbdd..b7ca4aa 100644 --- a/src/liboncilla/common.h +++ b/src/liboncilla/common.h @@ -1,25 +1,33 @@ /** * \file common.h * * \date Jun 28, 2012 * \author Alexandre Tuleu */ #ifndef LIBONCILLA_COMMON_H_ #define LIBONCILLA_COMMON_H_ + namespace rci{ namespace oncilla{ enum Leg { LEFT_FORE = 0, RIGHT_FORE = 1, LEFT_HIND = 2, RIGHT_HIND = 3, NUM_LEGS = 4 }; + + static const char* LegNames[NUM_LEGS] = { + "Left Fore", + "Right Fore", + "Left Hind", + "Right Hind" + }; } } #endif // LIBONCILLA_COMMON_H_ diff --git a/src/liboncilla/nodes/SupervisorL4.cpp b/src/liboncilla/nodes/SupervisorL4.cpp new file mode 100644 index 0000000..9edc0c6 --- /dev/null +++ b/src/liboncilla/nodes/SupervisorL4.cpp @@ -0,0 +1,21 @@ +/* + * SupervisorL4.cpp + * + * Created on: Jan 21, 2013 + * Author: tuleu + */ + +#include "SupervisorL4.h" + +namespace rci { +namespace oncilla { + +SupervisorL4::SupervisorL4(const std::string & name) + : rci::ResourceNode(name){ +} + +SupervisorL4::~SupervisorL4(){ +} + +} /* namespace oncilla */ +} /* namespace rci */ diff --git a/src/liboncilla/nodes/SupervisorL4.h b/src/liboncilla/nodes/SupervisorL4.h new file mode 100644 index 0000000..ba9ea56 --- /dev/null +++ b/src/liboncilla/nodes/SupervisorL4.h @@ -0,0 +1,30 @@ +/* + * SupervisorL4.h + * + * Created on: Jan 21, 2013 + * Author: tuleu + */ + +#ifndef SUPERVISORL4_H_ +#define SUPERVISORL4_H_ + +#include +#include +#include + +namespace rci { +namespace oncilla { + +class SupervisorL4 : public rci::ResourceNode, + public rci::Sensing, + public rci::PoseSensing, + public rci::ForceSensing { +public: + typedef boost::shared_ptr Ptr; + SupervisorL4(const std::string & name); + virtual ~SupervisorL4(); +}; + +} /* namespace oncilla */ +} /* namespace rci */ +#endif /* SUPERVISORL4_H_ */ diff --git a/src/liboncilla/nodes/SupervisorNodes.h b/src/liboncilla/nodes/SupervisorNodes.h new file mode 100644 index 0000000..2c67f2f --- /dev/null +++ b/src/liboncilla/nodes/SupervisorNodes.h @@ -0,0 +1,14 @@ +/* + * SupervisorNodes.h + * + * Created on: Jan 21, 2013 + * Author: tuleu + */ + +#ifndef LIBONCILLA_SUPERVISORNODES_H_ +#define SUPERVISORNODES_H_ + +#include "SupervisorTrunk.h" +#include "SupervisorL4.h" + +#endif /* LIBONCILLA_SUPERVISORNODES_H_ */ diff --git a/src/liboncilla/nodes/SupervisorTrunk.cpp b/src/liboncilla/nodes/SupervisorTrunk.cpp new file mode 100644 index 0000000..4ff9dbe --- /dev/null +++ b/src/liboncilla/nodes/SupervisorTrunk.cpp @@ -0,0 +1,21 @@ +/* + * SupervisorTrunk.cpp + * + * Created on: Jan 21, 2013 + * Author: tuleu + */ + +#include "SupervisorTrunk.h" + +namespace rci { +namespace oncilla { + +SupervisorTrunk::SupervisorTrunk() + : rci::ResourceNode("Supervised Oncilla Trunk"){ +} + +SupervisorTrunk::~SupervisorTrunk(){ +} + +} /* namespace oncilla */ +} /* namespace rci */ diff --git a/src/liboncilla/nodes/SupervisorTrunk.h b/src/liboncilla/nodes/SupervisorTrunk.h new file mode 100644 index 0000000..25163ef --- /dev/null +++ b/src/liboncilla/nodes/SupervisorTrunk.h @@ -0,0 +1,31 @@ +/* + * SupervisorTrunk.h + * + * Created on: Jan 21, 2013 + * Author: tuleu + */ + +#ifndef SUPERVISORTRUNK_H_ +#define SUPERVISORTRUNK_H_ + +#include +#include +#include + + +namespace rci { +namespace oncilla { + +class SupervisorTrunk : public rci::ResourceNode, + public rci::Sensing, + public rci::PoseSensing { +public: + typedef boost::shared_ptr Ptr; + + SupervisorTrunk(); + virtual ~SupervisorTrunk(); +}; + +} /* namespace oncilla */ +} /* namespace rci */ +#endif /* SUPERVISORTRUNK_H_ */ diff --git a/src/liboncilla/nodes/Trunk.cpp b/src/liboncilla/nodes/Trunk.cpp index 4fe6cdd..efae147 100644 --- a/src/liboncilla/nodes/Trunk.cpp +++ b/src/liboncilla/nodes/Trunk.cpp @@ -1,37 +1,37 @@ #include "Trunk.h" #include "liboncilla/exceptions/NotImplementedError.h" using namespace std; namespace rci { namespace oncilla { -Trunk::Trunk(const std::string & name) - : ResourceNode(name) +Trunk::Trunk() + : ResourceNode("Oncilla Trunk") , Sensing() { } Trunk::~Trunk() { } TranslationalAccelerationPtr Trunk::getTranslationalAcceleration() const{ throw NotImplementedError("OncillaTrunk","getTranslationalAcceleration"); } OrientationPtr Trunk::getOrientation() const{ throw NotImplementedError("OncillaTrunk","Orientation"); } std::string Trunk::print() const { ostringstream outstream(ostringstream::out); outstream.precision(3); // Precision when printing double values outstream << "" << endl; return outstream.str(); } } } diff --git a/src/liboncilla/nodes/Trunk.h b/src/liboncilla/nodes/Trunk.h index 945aa9a..b9a1910 100644 --- a/src/liboncilla/nodes/Trunk.h +++ b/src/liboncilla/nodes/Trunk.h @@ -1,44 +1,44 @@ #pragma once #include #include #include #include #include #include namespace rci { namespace oncilla { class Trunk; /** * Node class, representing the quadruped trunk, including pose sensing. * * @todo Expose translational accelerations and rotational velocities */ class Trunk: public rci::ResourceNode, public Sensing, public TranslationalAccelerationSensing, public OrientationSensing { public: typedef boost::shared_ptr Ptr; - Trunk(const std::string & name = "Oncilla Trunk"); + Trunk(); virtual ~Trunk(); virtual TranslationalAccelerationPtr getTranslationalAcceleration() const; virtual OrientationPtr getOrientation() const; /** * Print */ std::string print() const; }; } } diff --git a/src/liboncilla/utils/OncillaBackend.h b/src/liboncilla/utils/OncillaBackend.h index efbf731..97748be 100644 --- a/src/liboncilla/utils/OncillaBackend.h +++ b/src/liboncilla/utils/OncillaBackend.h @@ -1,50 +1,59 @@ /* * Backend.h * * Created on: Jan 18, 2013 * Author: tuleu */ #ifndef LIBONCILLA_ONCILLABACKEND_H_ #define LIBONCILLA_ONCILLABACKEND_H_ #include #include #include #include namespace rci{ namespace oncilla{ class Synchronizer; class L0; class L1L2; class L3; typedef L1L2 L1; typedef L1L2 L2; + class Trunk; + class SupervisorTrunk; + class SupervisorL4; } } class OncillaBackend { BIOROB_CPP_DECLARE_PLUGIN(OncillaBackend); DISABLE_ASSIGNEMENT(OncillaBackend); DISABLE_COPY(OncillaBackend); public : typedef std::tr1::shared_ptr Ptr; virtual ~OncillaBackend(); virtual boost::shared_ptr CreateSynchronizer() = 0; virtual boost::shared_ptr CreateL0(rci::oncilla::Leg l, const std::string & name ) = 0; virtual boost::shared_ptr CreateL1(rci::oncilla::Leg l, const std::string & name ) = 0; virtual boost::shared_ptr CreateL2(rci::oncilla::Leg l, const std::string & name ) = 0; virtual boost::shared_ptr CreateL3(rci::oncilla::Leg l, const std::string & name ) = 0; + virtual boost::shared_ptr CreateTrunk() = 0; + + virtual boost::shared_ptr CreateSupervisorTrunk() = 0; + virtual boost::shared_ptr CreateSupervisorL4(rci::oncilla::Leg l, + const std::string & name ) = 0; + }; #endif /* LIBONCILLA_ONCILLABACKEND_H_ */