Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00019 #include "SampleRH2.h"
00020
00021 #include <iostream>
00022 #include <hrpUtil/Eigen3d.h>
00023
00024 #define ROOT_FILE "etc/body.dat"
00025
00026 namespace {
00027 const bool CONTROLLER_BRIDGE_DEBUG = false;
00028 }
00029
00030
00031
00032
00033 static const char* samplepd_spec[] =
00034 {
00035 "implementation_id", "SampleRH2",
00036 "type_name", "SampleRH2",
00037 "description", "Sample RH2 component",
00038 "version", "0.1",
00039 "vendor", "AIST",
00040 "category", "Generic",
00041 "activity_type", "DataFlowComponent",
00042 "max_instance", "10",
00043 "language", "C++",
00044 "lang_type", "compile",
00045
00046
00047 ""
00048 };
00049
00050
00051 SampleRH2::SampleRH2(RTC::Manager* manager)
00052 : RTC::DataFlowComponentBase(manager),
00053
00054 m_root_transOut("root_trans", m_root_trans)
00055
00056 {
00057 if( CONTROLLER_BRIDGE_DEBUG )
00058 {
00059 std::cout << "SampleRH2::SampleRH2" << std::endl;
00060 }
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071 }
00072
00073 SampleRH2::~SampleRH2()
00074 {
00075 closeFiles();
00076 }
00077
00078
00079 RTC::ReturnCode_t SampleRH2::onInitialize()
00080 {
00081
00082
00083 if( CONTROLLER_BRIDGE_DEBUG )
00084 {
00085 std::cout << "onInitialize" << std::endl;
00086 }
00087
00088
00089
00090
00091 addOutPort("root_trans", m_root_transOut);
00092
00093
00094 return RTC::RTC_OK;
00095 }
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120 RTC::ReturnCode_t SampleRH2::onActivated(RTC::UniqueId ec_id)
00121 {
00122
00123 std::cout << "on Activated" << std::endl;
00124 openFiles();
00125
00126
00127
00128
00129 return RTC::RTC_OK;
00130 }
00131
00132
00133 RTC::ReturnCode_t SampleRH2::onDeactivated(RTC::UniqueId ec_id)
00134 {
00135 std::cout << "on Deactivated" << std::endl;
00136 closeFiles();
00137 return RTC::RTC_OK;
00138 }
00139
00140
00141 RTC::ReturnCode_t SampleRH2::onExecute(RTC::UniqueId ec_id)
00142 {
00143 if( CONTROLLER_BRIDGE_DEBUG )
00144 {
00145 std::cout << "SampleRH2::onExecute" << std::endl;
00146 }
00147
00148 if(!root.eof()){
00149 double time;
00150 root >> time;
00151 root >> m_root_trans.data.position.x
00152 >> m_root_trans.data.position.y
00153 >> m_root_trans.data.position.z;
00154 double rotation[4];
00155 for(int i=0; i<4; i++)
00156 root >> rotation[i];
00157 hrp::Matrix33 T;
00158 hrp::calcRodrigues(T, hrp::Vector3(rotation[0], rotation[1], rotation[2]), rotation[3]);
00159 hrp::Vector3 rpy = hrp::rpyFromRot(T);
00160 m_root_trans.data.orientation.r = rpy[0];
00161 m_root_trans.data.orientation.p = rpy[1];
00162 m_root_trans.data.orientation.y = rpy[2];
00163 }
00164
00165 m_root_transOut.write();
00166
00167 return RTC::RTC_OK;
00168 }
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206 void SampleRH2::openFiles()
00207 {
00208 root.open(ROOT_FILE);
00209 if (!root.is_open())
00210 {
00211 std::cerr << ROOT_FILE << " not opened" << std::endl;
00212 }
00213 }
00214
00215 void SampleRH2::closeFiles()
00216 {
00217 if(root.is_open()){
00218 root.close();
00219 root.clear();
00220 }
00221 }
00222
00223
00224 extern "C"
00225 {
00226
00227 DLL_EXPORT void SampleRH2Init(RTC::Manager* manager)
00228 {
00229 coil::Properties profile(samplepd_spec);
00230 manager->registerFactory(profile,
00231 RTC::Create<SampleRH2>,
00232 RTC::Delete<SampleRH2>);
00233 }
00234
00235 };
00236