Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044 #ifndef ORO_KDL_TYPEKIT_HPP
00045 #define ORO_KDL_TYPEKIT_HPP
00046
00047 #include "kdlTypekitTypes.hpp"
00048 #include <rtt/types/TypekitPlugin.hpp>
00049 #include <kdl/frames.hpp>
00050 #include <rtt/types/Types.hpp>
00051 #include <rtt/types/TemplateTypeInfo.hpp>
00052 #include <rtt/types/SequenceTypeInfo.hpp>
00053 #include <rtt/types/StructTypeInfo.hpp>
00054 #include <rtt/types/carray.hpp>
00055 #include <rtt/types/CArrayTypeInfo.hpp>
00056 #include <rtt/types/Operators.hpp>
00057 #include <rtt/types/OperatorTypes.hpp>
00058 #include <rtt/internal/mystd.hpp>
00059 #include <rtt/os/StartStopManager.hpp>
00060 #include <rtt/internal/GlobalService.hpp>
00061
00062 #include <kdl/frames_io.hpp>
00063
00064 #include <kdl/jntarray.hpp>
00065 #include <kdl/jacobian.hpp>
00066
00067 #include "motionproperties.hpp"
00068 #include "kinfamproperties.hpp"
00069
00070 namespace KDL
00071 {
00072 using namespace RTT;
00073 using namespace RTT::detail;
00074 using namespace std;
00075
00085 template<class KDLType, int size>
00086 double& get_container_item(KDLType & cont, int index)
00087 {
00088 if (index >= size || index < 0)
00089 return internal::NA<double&>::na();
00090 return cont[index];
00091 };
00092
00100 template<class KDLType, int size>
00101 double get_container_item_copy(const KDLType & cont, int index)
00102 {
00103 if (index >= size || index < 0)
00104 return internal::NA<double>::na();
00105 return cont[index];
00106 };
00107
00111 class KDLTypekitPlugin
00112 : public RTT::types::TypekitPlugin
00113 {
00114 public:
00115 virtual std::string getName();
00116
00117 virtual bool loadTypes();
00118 virtual bool loadConstructors();
00119 virtual bool loadOperators();
00120 };
00121
00125 template<class KDLType, int size>
00126 struct KDLVectorTypeInfo
00127 : public StructTypeInfo<KDLType,true>
00128 {
00129 KDLVectorTypeInfo(std::string name) : StructTypeInfo<KDLType,true>(name) {}
00130
00131 virtual bool decomposeTypeImpl(const KDLType& source, PropertyBag& targetbag ) const {
00132 decomposeProperty( source, targetbag );
00133 return true;
00134 }
00135
00136 virtual bool composeTypeImpl(const PropertyBag& source, KDLType& result) const {
00137 return composeProperty( source, result );
00138 }
00139
00140 base::DataSourceBase::shared_ptr getMember(base::DataSourceBase::shared_ptr item,
00141 base::DataSourceBase::shared_ptr id) const {
00142
00143 typename internal::DataSource<int>::shared_ptr id_indx = internal::DataSource<int>::narrow( internal::DataSourceTypeInfo<int>::getTypeInfo()->convert(id).get() );
00144 if ( id_indx ) {
00145 try {
00146 if ( item->isAssignable() )
00147 return internal::newFunctorDataSource(&get_container_item<KDLType,size>, internal::GenerateDataSource()(item.get(), id_indx.get() ) );
00148 else
00149 return internal::newFunctorDataSource(&get_container_item_copy<KDLType,size>, internal::GenerateDataSource()(item.get(), id_indx.get() ) );
00150 } catch(...) {}
00151 }
00152 if (id_indx) {
00153 log(Error) << "KDLVectorTypeInfo: Invalid index : " << id_indx->get() <<":"<< id_indx->getTypeName() << endlog();
00154 }
00155 if ( ! id_indx)
00156 log(Error) << "KDLVectorTypeInfo: Not a member or index : " << id <<":"<< id->getTypeName() << endlog();
00157 return base::DataSourceBase::shared_ptr();
00158 }
00159 };
00160
00164 template<class KDLType>
00165 struct KDLTypeInfo
00166 : public StructTypeInfo<KDLType,true>
00167 {
00168 KDLTypeInfo(std::string name) : StructTypeInfo<KDLType,true>(name) {}
00169
00170 virtual bool decomposeTypeImpl(const KDLType& source, PropertyBag& targetbag ) const {
00171 decomposeProperty( source, targetbag );
00172 return true;
00173 }
00174
00175 virtual bool composeTypeImpl(const PropertyBag& source, KDLType& result) const {
00176 return composeProperty( source, result );
00177 }
00178
00179 };
00180
00184 extern KDLTypekitPlugin KDLTypekit;
00185 }
00186 #endif